Add support for odom factors; still needs to be more thoroughly tested

include/odometry_cost_functor.h

@@ -0,0 +1,121 @@
+#ifndef UT_VSLAM_ODOMETRY_COST_FUNCTOR_H
+#define UT_VSLAM_ODOMETRY_COST_FUNCTOR_H
+
+#include <ceres/autodiff_cost_function.h>
+#include <vslam_types.h>
+#include <vslam_util.h>
+
+#include <eigen3/Eigen/Dense>
+
+namespace vslam_solver {
+
+/**
+ * Cost functor for odometry constraints.
+ */
+class OdometryCostFunctor {
+ public:
+  /**
+   * Constructor.
+   *
+   * @param factor Odometry factor that the cost should be based on.
+   */
+  OdometryCostFunctor(const vslam_types::OdometryFactor& factor)
+      : R_odom(factor.rotation),
+        T_odom(factor.translation),
+        sqrt_information(factor.sqrt_information) {}
+
+  /**
+   * Compute the residual for the odometry constraint.
+   *
+   * @tparam T                  Type that the cost functor is evaluating.
+   * @param pose_i[in]      Robot's pose in the world frame corresponding to
+   *                        the first node This is a 6 entry array with the
+   *                        first 3 entries corresponding to the translation
+   *                        and the second 3 entries containing the axis-angle
+   *                        representation (with angle given by the magnitude
+   *                        of the vector).
+   * @param pose_j[in]      Robot's pose in the world frame corresponding to
+   *                        the location of the second feature. This is a 6
+   *                        entry array with the first 3 entries corresponding
+   *                        to the translation and the second 3 entries
+   *                        containing the axis-angle representation (with
+   *                        angle given by the magnitude of the vector).
+   * @param residual[out]   Residual giving the error. Contains 6 entries
+   *                        (1 per pose dimension). First 3 are translation,
+   *                        second 3 are rotation.
+   *
+   * @return True if the residual was computed successfully, false otherwise.
+   */
+  template <typename T>
+  bool operator()(const T* pose_i, const T* pose_j, T* residual) const {
+    // Predicted pose_j = pose_i * odometry.
+    // Hence, error = pose_j.inverse() * pose_i * odometry;
+    typedef Eigen::Matrix<T, 3, 1> Vector3T;
+    typedef Eigen::Matrix<T, 3, 3> Matrix3T;
+
+    const Matrix3T Ri =
+        vslam_util::Exp(Vector3T(pose_i[3], pose_i[4], pose_i[5]));
+    const Matrix3T Rj =
+        vslam_util::Exp(Vector3T(pose_j[3], pose_j[4], pose_j[5]));
+
+    const Vector3T Ti(pose_i[0], pose_i[1], pose_i[2]);
+    const Vector3T Tj(pose_j[0], pose_j[1], pose_j[2]);
+
+    // Extract the error in translation.
+    const Vector3T error_translation =
+        Rj.transpose() * (Ri * T_odom.cast<T>() - (Tj - Ti));
+
+    residual[0] = error_translation[0];
+    residual[1] = error_translation[1];
+    residual[2] = error_translation[2];
+
+    // Extract the error in rotation in angle-axis form.
+    const Matrix3T error_rotation_matrix =
+        Rj.transpose() * Ri * R_odom.cast<T>();
+    const Vector3T error_rotation = vslam_util::Log(error_rotation_matrix);
+
+    residual[3] = error_rotation[0];
+    residual[4] = error_rotation[1];
+    residual[5] = error_rotation[2];
+
+    Eigen::Map<Eigen::Matrix<T, 6, 1>> residuals(residual);
+
+    residuals.applyOnTheLeft(sqrt_information.template cast<T>());
+
+    return true;
+  }
+
+  /**
+   * Create a cost function for the given odometry factor.
+   *
+   * @param factor Odometry factor encoding relative pose and uncertainty
+   * between two nodes.
+   *
+   * @return Cost function that generates a cost for two robot poses determined
+   * by how well they align with the odometry data.
+   */
+  static ceres::AutoDiffCostFunction<OdometryCostFunctor, 6, 6, 6>* create(
+      const vslam_types::OdometryFactor& factor) {
+    OdometryCostFunctor* residual = new OdometryCostFunctor(factor);
+    return new ceres::AutoDiffCostFunction<OdometryCostFunctor, 6, 6, 6>(
+        residual);
+  }
+
+  /**
+   * Rotation measurement.
+   */
+  const Eigen::Matrix3f R_odom;
+
+  /**
+   * Translation measurement.
+   */
+  const Eigen::Vector3f T_odom;
+
+  /**
+   * Sqrt information matrix (sqrt of inv. of covariance).
+   */
+  const Eigen::Matrix<double, 6, 6> sqrt_information;
+};
+}  // namespace vslam_solver
+
+#endif  // UT_VSLAM_ODOMETRY_COST_FUNCTOR_H

include/slam_backend_solver.h

@@ -122,6 +122,26 @@ class SLAMSolver {
    * on any particular instantiation of a SLAM problem.
    */
   SLAMSolverOptimizerParams solver_optimization_params_;
+
+  /**
+   * Add odometry factors to the ceres problem
+   *
+   * @tparam FeatureTrackType               Feature track type.
+   * @param slam_problem                    SLAM problem that contains info
+   *                                        about the odom factors.
+   * @param problem[in/out]                 Ceres problem that will have
+   *                                        residual blocks added to it.
+   * @param updated_solved_nodes[in/out]    Nodes in the SLAM problem that will
+   *                                        be updated during optimization. The
+   *                                        values will not change here, but
+   *                                        they will be tied to the ceres
+   *                                        problem.
+   */
+  template <typename FeatureTrackType>
+  void addOdometryFactors(
+      const vslam_types::UTSLAMProblem<FeatureTrackType> &slam_problem,
+      ceres::Problem &problem,
+      std::vector<vslam_types::SLAMNode> *updated_solved_nodes);
 };
 
 /**

include/vslam_types.h

@@ -71,6 +71,63 @@ struct VisionFeature {
   }
 };
 
+/**
+ * Represents an IMU or odometry, or fused IMU-odometry observation.
+ */
+struct OdometryFactor {
+  /**
+   * ID of first pose.
+   */
+  uint64_t pose_i;
+
+  /**
+   * ID of second pose.
+   */
+  uint64_t pose_j;
+
+  /**
+   * Translation to go from pose i to pose j.
+   */
+  Eigen::Vector3f translation;
+
+  /**
+   * Rotation to go from pose i to pose j.
+   */
+  Eigen::Quaternionf rotation;
+
+  /**
+   * Sqrt information matrix (square root of inverse covariance). First 3
+   * entries are translation, second 3 are rotation.
+   */
+  Eigen::Matrix<double, 6, 6> sqrt_information;
+
+  /**
+   * Default constructor: do nothing.
+   */
+  OdometryFactor() {}
+
+  /**
+   * Convenience constructor: initialize everything.
+   *
+   * @param pose_i              Id of the first pose.
+   * @param pose_j              Id of the second pose.
+   * @param translation         Measured translation from pose i to pose j.
+   * @param rotation            Measured rotation from pose i to pose j.
+   * @param sqrt_information    Sqrt information matrix (square root of inverse
+   *                            covariance). First 3 entries are translation,
+   *                            second 3 are rotation.
+   */
+  OdometryFactor(const uint64_t& pose_i,
+                 const uint64_t& pose_j,
+                 const Eigen::Vector3f& translation,
+                 const Eigen::Quaternionf& rotation,
+                 const Eigen::Matrix<double, 6, 6>& sqrt_information)
+      : pose_i(pose_i),
+        pose_j(pose_j),
+        translation(translation),
+        rotation(rotation) {}
+};
+
 /**
  * Structureless vision feature track.
  */
@@ -244,6 +301,12 @@ struct UTSLAMProblem {
    * Robot/frame poses of the entire trajectory
    */
   std::vector<RobotPose> robot_poses;
+
+  /**
+   * Odometry factors.
+   */
+  std::vector<OdometryFactor> odom_factors;
+
   /**
    * Default constructor: do nothing.
    */

include/vslam_util.h

@@ -250,6 +250,20 @@ void CorruptRobotPoses(const Eigen::Matrix<T, 2, 1>& odom_alphas,
 Eigen::Vector3d CorruptFeature(const Eigen::Vector3d& sigma_linear,
                                Eigen::Vector3d& feature_init_pose);
 
+/**
+ * Get odom factors from the initial pose and a constant noise model.
+ *
+ * @param initial_trajectory    Initial trajectory.
+ * @param odom_alphas           Standard deviations per unit for the
+ *                              translation and rotation (translation is first).
+ *
+ * @return Odometry factors linking each pair of subsequent poses in the
+ * trajectory.
+ */
+std::vector<vslam_types::OdometryFactor> getOdomFactorsFromInitPosesAndNoise(
+    const std::vector<vslam_types::RobotPose>& initial_trajectory,
+    const Eigen::Vector2d& odom_alphas);
+
 /**
  * Adjust trajectory so that the first pose is at the origin and all
  * other poses are adjusted to maintain the same transform to the first pose.

src/main.cpp

@@ -76,6 +76,10 @@ int main(int argc, char **argv) {
   std::vector<vslam_types::RobotPose> adjusted_to_zero_answer;
   vslam_util::AdjustTrajectoryToStartAtZero(answer, adjusted_to_zero_answer);
 
+  // Uncomment to add odom factors from synthetic data
+  //  prob.odom_factors =
+  //      vslam_util::getOdomFactorsFromInitPosesAndNoise(answer, odom_alphas);
+
   if (FLAGS_save_poses) {
     vslam_util::SaveKITTIPoses(FLAGS_output_path + "start.txt",
                                adjusted_to_zero_answer);

src/slam_backend_solver.cpp

@@ -1,4 +1,5 @@
 #include <ceres/ceres.h>
+#include <odometry_cost_functor.h>
 #include <pairwise_2d_feature_cost_functor.h>
 #include <reprojection_cost_functor.h>
 #include <slam_backend_solver.h>
@@ -75,6 +76,7 @@ bool SLAMSolver::SolveSLAM(
                           slam_problem,
                           problem,
                           &slam_nodes);
+  addOdometryFactors(slam_problem, problem, &slam_nodes);
 
   // Set the first pose constant
   problem.SetParameterBlockConstant(slam_nodes[0].pose);
@@ -96,6 +98,25 @@ bool SLAMSolver::SolveSLAM(
           summary.termination_type == ceres::USER_SUCCESS);
 }
 
+template <typename FeatureTrackType>
+void SLAMSolver::addOdometryFactors(
+    const vslam_types::UTSLAMProblem<FeatureTrackType> &slam_problem,
+    ceres::Problem &problem,
+    std::vector<vslam_types::SLAMNode> *updated_solved_nodes) {
+  std::vector<vslam_types::SLAMNode> &solution = *updated_solved_nodes;
+  for (const vslam_types::OdometryFactor &factor : slam_problem.odom_factors) {
+    CHECK_GT(solution.size(), factor.pose_i);
+    CHECK_GT(solution.size(), factor.pose_j);
+    double *pose_i_block = solution[factor.pose_i].pose;
+    double *pose_j_block = solution[factor.pose_j].pose;
+
+    problem.AddResidualBlock(OdometryCostFunctor::create(factor),
+                             nullptr,
+                             pose_i_block,
+                             pose_j_block);
+  }
+}
+
 void AddStructurelessVisionFactors(
     const vslam_types::CameraIntrinsics &intrinsics,
     const vslam_types::CameraExtrinsics &extrinsics,
@@ -199,4 +220,17 @@ template bool SLAMSolver::SolveSLAM<vslam_types::StructuredVisionFeatureTrack,
     vslam_types::UTSLAMProblem<vslam_types::StructuredVisionFeatureTrack>
         &slam_problem,
     std::vector<vslam_types::RobotPose> &updated_robot_poses);
+
+template void SLAMSolver::addOdometryFactors<vslam_types::VisionFeatureTrack>(
+    const vslam_types::UTSLAMProblem<vslam_types::VisionFeatureTrack>
+        &slam_problem,
+    ceres::Problem &problem,
+    std::vector<vslam_types::SLAMNode> *updated_solved_nodes);
+
+template void
+SLAMSolver::addOdometryFactors<vslam_types::StructuredVisionFeatureTrack>(
+    const vslam_types::UTSLAMProblem<vslam_types::StructuredVisionFeatureTrack>
+        &slam_problem,
+    ceres::Problem &problem,
+    std::vector<vslam_types::SLAMNode> *updated_solved_nodes);
 }  // namespace vslam_solver

src/structured_main.cpp

@@ -75,6 +75,10 @@ int main(int argc, char **argv) {
                                adjusted_to_zero_answer);
   }
 
+  // Uncomment to add odom factors from synthetic data
+  //  prob.odom_factors =
+  //      vslam_util::getOdomFactorsFromInitPosesAndNoise(answer, odom_alphas);
+
   Eigen::Vector3d feature_sigma_linear(0.0, 0.0, 0.0);
   for (int i = 0; i < prob.tracks.size(); i++) {
     prob.tracks[i].point = vslam_util::getPositionRelativeToPose(

src/vslam_util.cpp

@@ -1,6 +1,8 @@
+#include <glog/logging.h>
 #include <vslam_util.h>
 
 #include <fstream>
+#include <unsupported/Eigen/MatrixFunctions>
 
 namespace vslam_util {
 
@@ -71,4 +73,44 @@ Eigen::Vector3d getPositionRelativeToPose(const vslam_types::RobotPose &pose_1,
   return transformed;
 }
 
+std::vector<vslam_types::OdometryFactor> getOdomFactorsFromInitPosesAndNoise(
+    const std::vector<vslam_types::RobotPose> &initial_trajectory,
+    const Eigen::Vector2d &odom_alphas) {
+  std::vector<vslam_types::OdometryFactor> odom_factors;
+
+  for (uint64_t i = 1; i < initial_trajectory.size(); i++) {
+    vslam_types::RobotPose relative_pose = getPose2RelativeToPose1(
+        initial_trajectory[i - 1], initial_trajectory[i]);
+
+    Eigen::Matrix<double, 6, 6> odom_cov_mat =
+        Eigen::Matrix<double, 6, 6>::Zero();
+
+    odom_cov_mat(0, 0) = pow(relative_pose.loc.x() * odom_alphas(1), 2);
+    odom_cov_mat(1, 1) = pow(relative_pose.loc.y() * odom_alphas(1), 2);
+    odom_cov_mat(2, 2) = pow(relative_pose.loc.z() * odom_alphas(1), 2);
+    odom_cov_mat(3, 3) =
+        pow(relative_pose.angle.angle() * relative_pose.angle.axis().x() *
+                odom_alphas(0),
+            2);
+    odom_cov_mat(4, 4) =
+        pow(relative_pose.angle.angle() * relative_pose.angle.axis().y() *
+                odom_alphas(0),
+            2);
+    odom_cov_mat(5, 5) =
+        pow(relative_pose.angle.angle() * relative_pose.angle.axis().z() *
+                odom_alphas(0),
+            2);
+
+    Eigen::Matrix<double, 6, 6> sqrt_information =
+        odom_cov_mat.inverse().sqrt();
+
+    vslam_types::OdometryFactor factor(i - 1,
+                                       i,
+                                       relative_pose.loc,
+                                       Eigen::Quaternionf(relative_pose.angle),
+                                       sqrt_information);
+  }
+  return odom_factors;
+}
+
 }  // namespace vslam_util