camera_cost_functor.h

#ifndef CALICO_SENSORS_CAMERA_COST_FUNCTOR_H_
#define CALICO_SENSORS_CAMERA_COST_FUNCTOR_H_
 
#include <iostream>
#include <memory>
#include <string>
#include <vector>
 
#include "calico/sensors/camera_models.h"
#include "calico/trajectory.h"
#include "ceres/cost_function.h"
 
namespace calico::sensors {
 
// Enum listing the positions of parameters for a camera cost function.
enum class CameraParameterIndices : int {
  // Camera intrinsics.
  kIntrinsicsIndex = 0,
  // Extrinsic parameters of the camera relative to its sensor rig.
  kExtrinsicsRotationIndex = 1,
  kExtrinsicsTranslationIndex = 2,
  // Sensor latency.
  kLatencyIndex = 3,
  // Parameters related to some detected "model" object in the world:
  //   1. The point resolved in the model frame.
  //   2. The rotation portion of the model pose resolved in the world
  //      frame.
  //   3. The translation portion of the model pose resolved in the world
  //      frame.
  kModelPointIndex = 4,
  kModelRotationIndex = 5,
  kModelTranslationIndex = 6,
  // Rotation and position control points of the associated spline segment as
  // two Nx3 matrices (rotation then position) where N is the spline order.
  kSensorRigPoseSplineControlPointsIndex = 7,
};
 
// Generic auto-differentiation camera cost functor. Residuals will be based on
// how the camera model is initialized.
class CameraCostFunctor {
 public:
  static constexpr int kCameraResidualSize = 2;
  explicit CameraCostFunctor(CameraIntrinsicsModel camera_model,
                             const Eigen::Vector2d& pixel, double sigma,
                             double stamp,
                             const Trajectory& sp_T_world_sensorrig);
 
  // Convenience function for creating a camera cost function.
  static ceres::CostFunction* CreateCostFunction(
      const Eigen::Vector2d& pixel, double sigma,
      CameraIntrinsicsModel camera_model, Eigen::VectorXd& intrinsics,
      Pose3d& extrinsics, double& latency, Eigen::Vector3d& t_model_point,
      Pose3d& T_world_model, Trajectory& trajectory_world_sensorrig,
      double stamp, std::vector<double*>& parameters);
 
  // Parameters to the cost function:
  //   intrinsics:
  //     All parameters in the intrinsics model as an Eigen column vector.
  //     Order of the parameters will need to be in agreement with the model
  //     being used.
  //   q_sensorrig_camera:
  //     Rotation from sensorrig frame to camera frame as a quaternion.
  //   t_sensorrig_camera:
  //     Position of camera relative to sensorrig origin resolved in the
  //     sensorrig frame.
  //   latency:
  //     Sensor latency in seconds.
  //   q_world_model:
  //     Rotation from world frame to model frame as a quaternion.
  //   t_model_point:
  //     Position of the point in the model resolved int he model frame.
  //   t_world_model:
  //     Position of model relative to world origin resolved in the world frame.
  //   control_points:
  //     Control points for the entire pose trajectory.
  template <typename T>
  bool operator()(T const* const* parameters, T* residual) {
    // Parse intrinsics.
    const T* intrinsics_ptr = static_cast<const T*>(
        &(parameters[static_cast<int>(CameraParameterIndices::kIntrinsicsIndex)]
                    [0]));
    const int parameter_size = camera_model_->NumberOfParameters();
    const Eigen::VectorX<T> intrinsics =
        Eigen::Map<const Eigen::VectorX<T>>(intrinsics_ptr, parameter_size);
    // Parse extrinsics.
    const Eigen::Map<const Eigen::Quaternion<T>> q_sensorrig_camera(
        &(parameters[static_cast<int>(
            CameraParameterIndices::kExtrinsicsRotationIndex)][0]));
    const Eigen::Map<const Eigen::Vector3<T>> t_sensorrig_camera(
        &(parameters[static_cast<int>(
            CameraParameterIndices::kExtrinsicsTranslationIndex)][0]));
    // Parse latency.
    const T latency =
        parameters[static_cast<int>(CameraParameterIndices::kLatencyIndex)][0];
    // Parse model point and model pose resolved in the world frame.
    const Eigen::Map<const Eigen::Vector3<T>> t_model_point(
        &(parameters[static_cast<int>(CameraParameterIndices::kModelPointIndex)]
                    [0]));
    const Eigen::Map<const Eigen::Quaternion<T>> q_world_model(
        &(parameters[static_cast<int>(
            CameraParameterIndices::kModelRotationIndex)][0]));
    const Eigen::Map<const Eigen::Vector3<T>> t_world_model(
        &(parameters[static_cast<int>(
            CameraParameterIndices::kModelTranslationIndex)][0]));
    // Parse sensor rig spline resolved in the world frame.
    const int num_rotation_control_points =
        rotation_trajectory_evaluation_params_.num_control_points;
    Eigen::MatrixX<T> rotation_control_points(num_rotation_control_points, 3);
    for (int i = 0; i < num_rotation_control_points; ++i) {
      rotation_control_points.row(i) = Eigen::Map<const Eigen::Vector3<T>>(
          &(parameters[static_cast<int>(
                           CameraParameterIndices::
                               kSensorRigPoseSplineControlPointsIndex) +
                       i][0]));
    }
    const int num_position_control_points =
        position_trajectory_evaluation_params_.num_control_points;
    Eigen::MatrixX<T> position_control_points(num_position_control_points, 3);
    for (int i = 0; i < num_position_control_points; ++i) {
      position_control_points.row(i) = Eigen::Map<const Eigen::Vector3<T>>(
          &(parameters[static_cast<int>(
                           CameraParameterIndices::
                               kSensorRigPoseSplineControlPointsIndex) +
                       i + num_rotation_control_points][0]));
    }
 
    const Eigen::MatrixX<T> rotation_basis_matrix =
        rotation_trajectory_evaluation_params_.basis_matrix.template cast<T>();
    const T rotation_knot0 =
        static_cast<T>(rotation_trajectory_evaluation_params_.knot0);
    const T rotation_knot1 =
        static_cast<T>(rotation_trajectory_evaluation_params_.knot1);
    const T rotation_stamp =
        static_cast<T>(rotation_trajectory_evaluation_params_.stamp) - latency;
 
    const Eigen::MatrixX<T> position_basis_matrix =
        position_trajectory_evaluation_params_.basis_matrix.template cast<T>();
    const T position_knot0 =
        static_cast<T>(position_trajectory_evaluation_params_.knot0);
    const T position_knot1 =
        static_cast<T>(position_trajectory_evaluation_params_.knot1);
    const T position_stamp =
        static_cast<T>(position_trajectory_evaluation_params_.stamp) - latency;
    // Evaluate the pose.
    const Eigen::Vector3<T> phi_sensorrig_world = -BSpline<3, T>::Evaluate(
        rotation_control_points, rotation_knot0, rotation_knot1,
        rotation_basis_matrix, rotation_stamp, 0);
    T q_sensorrig_world_array[4];
    ceres::AngleAxisToQuaternion(phi_sensorrig_world.data(),
                                 q_sensorrig_world_array);
    const Eigen::Quaternion<T> q_sensorrig_world(
        q_sensorrig_world_array[0], q_sensorrig_world_array[1],
        q_sensorrig_world_array[2], q_sensorrig_world_array[3]);
    const Eigen::Vector3<T> t_world_sensorrig = BSpline<3, T>::Evaluate(
        position_control_points, position_knot0, position_knot1,
        position_basis_matrix, position_stamp, 0);
 
    // Resolve the model point in the camera frame.
    const Eigen::Quaternion<T> q_camera_model =
        q_sensorrig_camera.inverse() * q_sensorrig_world * q_world_model;
    const Eigen::Vector3<T> t_world_camera =
        t_world_sensorrig + q_sensorrig_world.inverse() * t_sensorrig_camera;
    const Eigen::Vector3<T> t_model_camera =
        q_world_model.inverse() * (t_world_camera - t_world_model);
    const Eigen::Vector3<T> t_camera_point =
        q_camera_model * (t_model_point - t_model_camera);
    const absl::StatusOr<Eigen::Vector2<T>> projection =
        camera_model_->ProjectPoint(intrinsics, t_camera_point);
    // Assign the residual. If projection fails, it means the point is behind
    // the camera or at infinity. In this case, set the residual to zero so that
    // the optimizer can effectively ignore it. We do this because this point
    // might come back into the field of view.
    Eigen::Map<Eigen::Vector2<T>> error(residual);
    if (projection.ok()) {
      const Eigen::Vector2<T> pixel = pixel_.template cast<T>();
      error = (pixel - *projection) * static_cast<T>(information_);
      return true;
    }
    std::cerr << "projection failed inside cost functor: "
              << projection.status().message()
              << " stamp=" << rotation_trajectory_evaluation_params_.stamp
              << " t_camera_point=" << t_camera_point.transpose() << std::endl;
    return false;
  }
 
 private:
  Eigen::Vector2d pixel_;
  double information_;
  std::unique_ptr<CameraModel> camera_model_;
  TrajectoryEvaluationParams position_trajectory_evaluation_params_;
  TrajectoryEvaluationParams rotation_trajectory_evaluation_params_;
};
}  // namespace calico::sensors
 
#endif  // CALICO_SENSORS_CAMERA_COST_FUNCTOR_H_