DirectLandmarkCostFunction3DWithTwoObservations

landmark_direct_cost_function.h

// Cost function measuring the weighted error between the observed pose given by
// the landmark measurement and the linearly interpolated pose.
class DirectLandmarkCostFunction3DWithTwoObservations {
 public:
  using LandmarkObservation =
      PoseGraphInterface::LandmarkNode::LandmarkObservation;

  static ceres::CostFunction* CreateAutoDiffCostFunction(
      const LandmarkObservation& observation1, const NodeSpec3D& prev_node1,
      const NodeSpec3D& next_node1, const LandmarkObservation& observation2,
      const NodeSpec3D& prev_node2, const NodeSpec3D& next_node2) {
    return new ceres::AutoDiffCostFunction<
        DirectLandmarkCostFunction3DWithTwoObservations, 6 /* residuals */,
        4 /* previous node1 rotation variables */,
        3 /* previous node1 translation variables */,
        4 /* next node1 rotation variables */,
        3 /* next node1 translation variables */,
        4 /* previous2 node rotation variables */,
        3 /* previous2 node translation variables */,
        4 /* next node2 rotation variables */,
        3 /* next node2 translation variables */>(
        new DirectLandmarkCostFunction3DWithTwoObservations(
            observation1, prev_node1, next_node1, 
            observation2, prev_node2, next_node2));
  }

  template <typename T>
  bool operator()(const T* const prev_node_rotation1,
                  const T* const prev_node_translation1,
                  const T* const next_node_rotation1,
                  const T* const next_node_translation1,
                  const T* const prev_node_rotation2,
                  const T* const prev_node_translation2,
                  const T* const next_node_rotation2,
                  const T* const next_node_translation2, T* const e) const {
    const std::tuple<std::array<T, 4>, std::array<T, 3>>
        interpolated_rotation_and_translation1 = InterpolateNodes3D(
            prev_node_rotation1, prev_node_translation1, next_node_rotation1,
            next_node_translation1, interpolation_parameter1_);
    const std::tuple<std::array<T, 4>, std::array<T, 3>>
        interpolated_rotation_and_translation2 = InterpolateNodes3D(
            prev_node_rotation2, prev_node_translation2, next_node_rotation2,
            next_node_translation2, interpolation_parameter2_);
    std::array<T, 6> error;

    const auto& node1_rotation =
        std::get<0>(interpolated_rotation_and_translation1).data();
    const auto& node1_translation =
        std::get<1>(interpolated_rotation_and_translation1).data();
    const Eigen::Quaternion<T> R_node1(node1_rotation[0], node1_rotation[1],
                                       node1_rotation[2], node1_rotation[3]);
    const Eigen::Matrix<T, 3, 1> t_node1(
        node1_translation[0], node1_translation[1], node1_translation[2]);
    const Eigen::Quaternion<T> R_obs1 =
        landmark_to_tracking_transform1_.rotation().cast<T>();
    const Eigen::Matrix<T, 3, 1> t_obs1 =
        landmark_to_tracking_transform1_.translation().cast<T>();

    const Eigen::Quaternion<T> R_obs1_global = R_node1 * R_obs1;
    const Eigen::Matrix<T, 3, 1> t_obs1_global = t_node1 + R_node1 * t_obs1;

    const auto& node2_rotation =
        std::get<0>(interpolated_rotation_and_translation2).data();
    const auto& node2_translation =
        std::get<1>(interpolated_rotation_and_translation2).data();
    const Eigen::Quaternion<T> R_node2(node2_rotation[0], node2_rotation[1],
                                       node2_rotation[2], node2_rotation[3]);
    const Eigen::Matrix<T, 3, 1> t_node2(
        node2_translation[0], node2_translation[1], node2_translation[2]);
    const Eigen::Quaternion<T> R_obs2 =
        landmark_to_tracking_transform2_.rotation().cast<T>();
    const Eigen::Matrix<T, 3, 1> t_obs2 =
        landmark_to_tracking_transform2_.translation().cast<T>();

    const Eigen::Quaternion<T> R_obs2_global = R_node2 * R_obs2;
    const Eigen::Matrix<T, 3, 1> t_obs2_global = t_node2 + R_node2 * t_obs2;

    const Eigen::Quaternion<T> diff_quaternion =
        R_obs2_global.conjugate() * R_obs1_global;
    const Eigen::Matrix<T, 3, 1> angle_axis_difference =
        transform::RotationQuaternionToAngleAxisVector(diff_quaternion);

    error = {{(t_obs1_global[0] - t_obs2_global[0]) * translation_weight_,
              (t_obs1_global[1] - t_obs2_global[1]) * translation_weight_,
              (t_obs1_global[2] - t_obs2_global[2]) * translation_weight_,
              angle_axis_difference[0] * rotation_weight_,
              angle_axis_difference[1] * rotation_weight_,
              angle_axis_difference[2] * rotation_weight_}};
              
    std::copy(std::begin(error), std::end(error), e);
    return true;
  }

 private:
  DirectLandmarkCostFunction3DWithTwoObservations(
      const LandmarkObservation& observation1, const NodeSpec3D& prev_node1,
      const NodeSpec3D& next_node1, const LandmarkObservation& observation2,
      const NodeSpec3D& prev_node2, const NodeSpec3D& next_node2)
      : landmark_to_tracking_transform1_(
            observation1.landmark_to_tracking_transform),
        interpolation_parameter1_(
            common::ToSeconds(observation1.time - prev_node1.time) /
            common::ToSeconds(next_node1.time - prev_node1.time)),
        landmark_to_tracking_transform2_(
            observation2.landmark_to_tracking_transform),
        interpolation_parameter2_(
            common::ToSeconds(observation2.time - prev_node2.time) /
            common::ToSeconds(next_node2.time - prev_node2.time)),
        translation_weight_(observation1.translation_weight),
        rotation_weight_(observation1.rotation_weight) {
    CHECK_EQ(observation1.translation_weight, observation2.translation_weight);
    CHECK_EQ(observation1.rotation_weight, observation2.rotation_weight);
  }

  const transform::Rigid3d landmark_to_tracking_transform1_;
  const double interpolation_parameter1_;
  const transform::Rigid3d landmark_to_tracking_transform2_;
  const double interpolation_parameter2_;
  const double translation_weight_;
  const double rotation_weight_;
};

optimization_problem_3d.cc

void AddLandmarkCostFunctions(...) {
    ....
if (landmark_node.second.landmark_observations.size() == 2) {
      const auto& observation1 =
          landmark_node.second.landmark_observations.at(0);
      const auto& observation2 =
          landmark_node.second.landmark_observations.at(1);
      const auto& begin_of_trajectory1 =
          node_data.BeginOfTrajectory(observation1.trajectory_id);
      const auto& begin_of_trajectory2 =
          node_data.BeginOfTrajectory(observation2.trajectory_id);
      // The landmark observation was made before the trajectory was created.
      if (observation1.time < begin_of_trajectory1->data.time) {
        continue;
      }
      if (observation2.time < begin_of_trajectory2->data.time) {
        continue;
      }
      // Find the trajectory nodes before and after the landmark observation.
      auto next1 =
          node_data.lower_bound(observation1.trajectory_id, observation1.time);
      auto next2 =
          node_data.lower_bound(observation2.trajectory_id, observation2.time);
      // The landmark observation was made, but the next trajectory node has
      // not been added yet.
      if (next1 == node_data.EndOfTrajectory(observation1.trajectory_id)) {
        continue;
      }
      if (next2 == node_data.EndOfTrajectory(observation2.trajectory_id)) {
        continue;
      }
      if (next1 == begin_of_trajectory1) {
        next1 = std::next(next1);
      }
      if (next2 == begin_of_trajectory2) {
        next2 = std::next(next2);
      }
      auto prev1 = std::prev(next1);
      auto prev2 = std::prev(next2);
      // Add parameter blocks for the landmark ID if they were not added before.
      CeresPose* prev_node_pose1 = &C_nodes->at(prev1->id);
      CeresPose* next_node_pose1 = &C_nodes->at(next1->id);
      CeresPose* prev_node_pose2 = &C_nodes->at(prev2->id);
      CeresPose* next_node_pose2 = &C_nodes->at(next2->id);
      problem->AddResidualBlock(
          DirectLandmarkCostFunction3DWithTwoObservations::
              CreateAutoDiffCostFunction(observation1, prev1->data, next1->data,
                                         observation2, prev2->data, next2->data),
          nullptr /* loss function */, prev_node_pose1->rotation(),
          prev_node_pose1->translation(), next_node_pose1->rotation(),
          next_node_pose1->translation(), prev_node_pose2->rotation(),
          prev_node_pose2->translation(), next_node_pose2->rotation(),
          next_node_pose2->translation());
    } else { ... }