odom bundle adjustment

raphaelchang · Sep 16, 2019 · 0b9db2b · 0b9db2b
1 parent 64b3406
commit 0b9db2b
Show file tree

Hide file tree

Showing 16 changed files with 211 additions and 56 deletions.
diff --git a/launch/odometry_eval.launch b/launch/odometry_eval.launch
@@ -11,8 +11,10 @@
         <param name="depth_image_topic" value="/unity_ros/Sphere/FisheyeDepthCamera/image_raw" />
         <param name="pose_topic" value="/unity_ros/Sphere/TrueState/pose" />
         <param name="tracked_image_topic" value="/omni_slam/tracked" />
-        <param name="odometry_topic" value="/omni_slam/odometry" />
+        <param name="odometry_estimate_topic" value="/omni_slam/odometry" />
         <param name="odometry_ground_truth_topic" value="/omni_slam/odometry_truth" />
+        <param name="path_estimate_topic" value="/omni_slam/odometry_path" />
+        <param name="path_ground_truth_topic" value="/omni_slam/odometry_path_truth" />
         <param name="rate" value="$(arg rate)" />
         <rosparam subst_value="true">
             camera_model: '$(arg camera_model)'
@@ -24,10 +26,12 @@
             tracker_delta_pixel_error_threshold: 3.0
             tracker_error_threshold: 20.0
             min_features_per_region: 10
-            pnp_inlier_threshold: 5.0
-            pnp_iterations: 1000
+            pnp_inlier_threshold: 3.0
+            pnp_iterations: 2000
             bundle_adjustment_max_iterations: 1000
+            bundle_adjustment_loss_coefficient: 0.05
             bundle_adjustment_logging: true
+            bundle_adjustment_num_threads: 20
         </rosparam>
     </node>
 </launch>

diff --git a/launch/reconstruction_eval.launch b/launch/reconstruction_eval.launch
@@ -23,10 +23,12 @@
             tracker_delta_pixel_error_threshold: 3.0
             tracker_error_threshold: 20.0
             min_features_per_region: 10
-            max_reprojection_error: 0.0
+            max_reprojection_error: 5.0
             min_triangulation_angle: 3.0
             bundle_adjustment_max_iterations: 1000
+            bundle_adjustment_loss_coefficient: 0.1
             bundle_adjustment_logging: true
+            bundle_adjustment_num_threads: 20
         </rosparam>
     </node>
 </launch>
diff --git a/src/data/frame.cc b/src/data/frame.cc
@@ -6,8 +6,10 @@ namespace omni_slam
 namespace data
 {
 
-Frame::Frame(const int id, cv::Mat &image, cv::Mat &depth_image, Matrix<double, 3, 4>  &pose, double time, camera::CameraModel<> &camera_model)
-    : id_(id),
+int Frame::lastFrameId_ = 0;
+
+Frame::Frame(cv::Mat &image, cv::Mat &depth_image, Matrix<double, 3, 4>  &pose, double time, camera::CameraModel<> &camera_model)
+    : id_(lastFrameId_++),
     image_(image.clone()),
     depthImage_(depth_image.clone()),
     pose_(pose),
@@ -19,8 +21,8 @@ Frame::Frame(const int id, cv::Mat &image, cv::Mat &depth_image, Matrix<double,
     hasDepth_ = true;
 }
 
-Frame::Frame(const int id, cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model)
-    : id_(id),
+Frame::Frame(cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model)
+    : id_(lastFrameId_++),
     image_(image.clone()),
     depthImage_(depth_image.clone()),
     timeSec_(time),
@@ -30,8 +32,8 @@ Frame::Frame(const int id, cv::Mat &image, cv::Mat &depth_image, double time, ca
     hasDepth_ = true;
 }
 
-Frame::Frame(const int id, cv::Mat &image, Matrix<double, 3, 4>  &pose, double time, camera::CameraModel<> &camera_model)
-    : id_(id),
+Frame::Frame(cv::Mat &image, Matrix<double, 3, 4>  &pose, double time, camera::CameraModel<> &camera_model)
+    : id_(lastFrameId_++),
     image_(image.clone()),
     pose_(pose),
     invPose_(util::TFUtil::InversePoseMatrix(pose)),
@@ -42,8 +44,8 @@ Frame::Frame(const int id, cv::Mat &image, Matrix<double, 3, 4>  &pose, double t
     hasDepth_ = false;
 }
 
-Frame::Frame(const int id, cv::Mat &image, double time, camera::CameraModel<> &camera_model)
-    : id_(id),
+Frame::Frame(cv::Mat &image, double time, camera::CameraModel<> &camera_model)
+    : id_(lastFrameId_++),
     timeSec_(time),
     cameraModel_(camera_model)
 {
@@ -136,13 +138,29 @@ void Frame::SetDepthImage(cv::Mat &depth_image)
     hasDepth_ = true;
 }
 
+void Frame::SetEstimatedPose(const Matrix<double, 3, 4> &pose, const std::vector<int> &landmark_ids)
+{
+    poseEstimate_ = pose;
+    invPoseEstimate_ = util::TFUtil::InversePoseMatrix(pose);
+    estLandmarkIds_ = std::unordered_set<int>(landmark_ids.begin(), landmark_ids.end());
+    hasPoseEstimate_ = true;
+}
+
 void Frame::SetEstimatedPose(const Matrix<double, 3, 4> &pose)
 {
     poseEstimate_ = pose;
     invPoseEstimate_ = util::TFUtil::InversePoseMatrix(pose);
     hasPoseEstimate_ = true;
 }
 
+void Frame::SetEstimatedInversePose(const Matrix<double, 3, 4> &pose, const std::vector<int> &landmark_ids)
+{
+    invPoseEstimate_ = pose;
+    poseEstimate_ = util::TFUtil::InversePoseMatrix(pose);
+    estLandmarkIds_ = std::unordered_set<int>(landmark_ids.begin(), landmark_ids.end());
+    hasPoseEstimate_ = true;
+}
+
 void Frame::SetEstimatedInversePose(const Matrix<double, 3, 4> &pose)
 {
     invPoseEstimate_ = pose;
@@ -165,6 +183,11 @@ bool Frame::HasEstimatedPose() const
     return hasPoseEstimate_;
 }
 
+bool Frame::IsEstimatedByLandmark(const int landmark_id) const
+{
+    return estLandmarkIds_.find(landmark_id) != estLandmarkIds_.end();
+}
+
 void Frame::CompressImages()
 {
     if (isCompressed_)
@@ -203,5 +226,10 @@ bool Frame::IsCompressed() const
     return isCompressed_;
 }
 
+const double Frame::GetTime() const
+{
+    return timeSec_;
+}
+
 }
 }
diff --git a/src/data/frame.h b/src/data/frame.h
@@ -3,6 +3,7 @@
 
 #include <opencv2/opencv.hpp>
 #include <Eigen/Dense>
+#include <unordered_set>
 #include "camera/camera_model.h"
 
 using namespace Eigen;
@@ -15,10 +16,10 @@ namespace data
 class Frame
 {
 public:
-    Frame(const int id, cv::Mat &image, cv::Mat &depth_image, Matrix<double, 3, 4>  &pose, double time, camera::CameraModel<> &camera_model);
-    Frame(const int id, cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model);
-    Frame(const int id, cv::Mat &image, Matrix<double, 3, 4>  &pose, double time, camera::CameraModel<> &camera_model);
-    Frame(const int id, cv::Mat &image, double time, camera::CameraModel<> &camera_model);
+    Frame(cv::Mat &image, cv::Mat &depth_image, Matrix<double, 3, 4>  &pose, double time, camera::CameraModel<> &camera_model);
+    Frame(cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model);
+    Frame(cv::Mat &image, Matrix<double, 3, 4>  &pose, double time, camera::CameraModel<> &camera_model);
+    Frame(cv::Mat &image, double time, camera::CameraModel<> &camera_model);
     Frame(const Frame &other);
 
     const Matrix<double, 3, 4>& GetPose() const;
@@ -29,14 +30,18 @@ class Frame
     const Matrix<double, 3, 4>& GetEstimatedPose() const;
     const Matrix<double, 3, 4>& GetEstimatedInversePose() const;
     const int GetID() const;
+    const double GetTime() const;
 
     bool HasPose() const;
     bool HasDepthImage() const;
     bool HasEstimatedPose() const;
+    bool IsEstimatedByLandmark(const int landmark_id) const;
 
     void SetPose(Matrix<double, 3, 4> &pose);
     void SetDepthImage(cv::Mat &depth_image);
+    void SetEstimatedPose(const Matrix<double, 3, 4> &pose, const std::vector<int> &landmark_ids);
     void SetEstimatedPose(const Matrix<double, 3, 4> &pose);
+    void SetEstimatedInversePose(const Matrix<double, 3, 4> &pose, const std::vector<int> &landmark_ids);
     void SetEstimatedInversePose(const Matrix<double, 3, 4> &pose);
 
     void CompressImages();
@@ -56,11 +61,15 @@ class Frame
     double timeSec_;
     camera::CameraModel<> &cameraModel_;
 
+    std::unordered_set<int> estLandmarkIds_;
+
     bool hasPose_;
     bool hasDepth_;
     bool hasPoseEstimate_{false};
 
     bool isCompressed_{false};
+
+    static int lastFrameId_;
 };
 
 }

diff --git a/src/data/landmark.cc b/src/data/landmark.cc
@@ -5,7 +5,10 @@ namespace omni_slam
 namespace data
 {
 
+int Landmark::lastLandmarkId_ = 0;
+
 Landmark::Landmark()
+    : id_(lastLandmarkId_++)
 {
 }
 
@@ -21,7 +24,7 @@ void Landmark::AddObservation(Feature obs, bool compute_gnd)
         if (obs.HasEstimatedWorldPoint())
         {
             posEstimate_ = obs.GetEstimatedWorldPoint();
-            obsForEst_.push_back(obs);
+            estFrameIds_.insert(obs.GetFrame().GetID());
             hasPosEstimate_ = true;
         }
     }
@@ -38,11 +41,6 @@ const std::vector<Feature>& Landmark::GetObservations() const
     return obs_;
 }
 
-const std::vector<Feature>& Landmark::GetObservationsForEstimate() const
-{
-    return obsForEst_;
-}
-
 std::vector<Feature>& Landmark::GetObservations()
 {
     return obs_;
@@ -83,18 +81,24 @@ const Feature* Landmark::GetObservationByFrameID(const int frame_id) const
     return &obs_[idToIndex_.at(frame_id)];
 }
 
+void Landmark::SetEstimatedPosition(const Vector3d &pos, const std::vector<int> &frame_ids)
+{
+    posEstimate_ = pos;
+    estFrameIds_ = std::unordered_set<int>(frame_ids.begin(), frame_ids.end());
+    hasPosEstimate_ = true;
+}
+
 void Landmark::SetEstimatedPosition(const Vector3d &pos)
 {
     posEstimate_ = pos;
-    obsForEst_.clear();
-    obsForEst_.reserve(obs_.size());
-    for (data::Feature &feat : obs_)
-    {
-        obsForEst_.push_back(feat);
-    }
     hasPosEstimate_ = true;
 }
 
+const int Landmark::GetID() const
+{
+    return id_;
+}
+
 Vector3d Landmark::GetGroundTruth() const
 {
     return groundTruth_;
@@ -115,5 +119,10 @@ bool Landmark::HasEstimatedPosition() const
     return hasPosEstimate_;
 }
 
+bool Landmark::IsEstimatedByFrame(const int frame_id) const
+{
+    return estFrameIds_.find(frame_id) != estFrameIds_.end();
+}
+
 }
 }
diff --git a/src/data/landmark.h b/src/data/landmark.h
@@ -3,6 +3,7 @@
 
 #include "feature.h"
 #include <vector>
+#include <unordered_set>
 #include <Eigen/Dense>
 
 using namespace Eigen;
@@ -19,26 +20,31 @@ class Landmark
     void AddObservation(Feature obs, bool compute_gnd = true);
     const std::vector<Feature>& GetObservations() const;
     std::vector<Feature>& GetObservations();
-    const std::vector<Feature>& GetObservationsForEstimate() const;
     bool IsObservedInFrame(const int frame_id) const;
     const int GetFirstFrameID() const;
     const int GetNumObservations() const;
     const Feature* GetObservationByFrameID(const int frame_id) const;
+    void SetEstimatedPosition(const Vector3d &pos, const std::vector<int> &frame_ids);
     void SetEstimatedPosition(const Vector3d &pos);
 
+    const int GetID() const;
     Vector3d GetGroundTruth() const;
     Vector3d GetEstimatedPosition() const;
     bool HasGroundTruth() const;
     bool HasEstimatedPosition() const;
+    bool IsEstimatedByFrame(const int frame_id) const;
 
 private:
+    const int id_;
     std::vector<Feature> obs_;
-    std::vector<Feature> obsForEst_;
+    std::unordered_set<int> estFrameIds_;
     std::map<int, int> idToIndex_;
     Vector3d groundTruth_;
     Vector3d posEstimate_;
     bool hasGroundTruth_{false};
     bool hasPosEstimate_{false};
+
+    static int lastLandmarkId_;
 };
 
 }

diff --git a/src/odometry/pnp.cc b/src/odometry/pnp.cc
@@ -16,9 +16,10 @@ namespace omni_slam
 namespace odometry
 {
 
-PNP::PNP(int ransac_iterations, double reprojection_threshold)
+PNP::PNP(int ransac_iterations, double reprojection_threshold, int num_refine_threads)
     : ransacIterations_(ransac_iterations),
-    reprojThreshold_(reprojection_threshold)
+    reprojThreshold_(reprojection_threshold),
+    numRefineThreads_(num_refine_threads)
 {
 }
 
@@ -28,6 +29,7 @@ int PNP::Compute(const std::vector<data::Landmark> &landmarks, data::Frame &fram
     std::vector<Vector2d> yns;
     std::vector<Vector3d> ys;
     std::vector<const data::Feature*> features;
+    std::map<int, int> indexToId;
     for (const data::Landmark &landmark : landmarks)
     {
         if (landmark.IsObservedInFrame(frame.GetID()))
@@ -50,6 +52,7 @@ int PNP::Compute(const std::vector<data::Landmark> &landmarks, data::Frame &fram
             yns.push_back(pix);
             ys.push_back(feat->GetBearing());
             features.push_back(feat);
+            indexToId[xs.size() - 1] = landmark.GetID();
         }
     }
     Matrix<double, 3, 4> pose;
@@ -59,13 +62,20 @@ int PNP::Compute(const std::vector<data::Landmark> &landmarks, data::Frame &fram
     {
         Refine(xs, features, indices, pose);
     }
-    frame.SetEstimatedInversePose(pose);
+    std::vector<int> inlierIds;
+    inlierIds.reserve(indices.size());
+    for (int inx : indices)
+    {
+        inlierIds.push_back(indexToId[inx]);
+    }
+    frame.SetEstimatedInversePose(pose, inlierIds);
     return inliers;
 }
 
 int PNP::RANSAC(const std::vector<Vector3d> &xs, const std::vector<Vector3d> &ys, const std::vector<Vector2d> &yns, const camera::CameraModel<> &camera_model, Matrix<double, 3, 4> &pose) const
 {
     int maxInliers = 0;
+    #pragma omp parallel for
     for (int i = 0; i < ransacIterations_; i++)
     {
         std::random_device rd;
@@ -87,10 +97,13 @@ int PNP::RANSAC(const std::vector<Vector3d> &xs, const std::vector<Vector3d> &ys
         }
 
         int inliers = GetInlierIndices(xs, yns, iterPose, camera_model).size();
-        if (inliers > maxInliers)
+        #pragma omp critical
         {
-            maxInliers = inliers;
-            pose = iterPose;
+            if (inliers > maxInliers)
+            {
+                maxInliers = inliers;
+                pose = iterPose;
+            }
         }
     }
     return maxInliers;
@@ -133,6 +146,7 @@ bool PNP::Refine(const std::vector<Vector3d> &xs, const std::vector<const data::
     options.linear_solver_type = ceres::DENSE_SCHUR;
     options.function_tolerance = 1e-6;
     options.gradient_tolerance = 1e-6;
+    options.num_threads = numRefineThreads_;
     ceres::Solver::Summary summary;
     ceres::Solve(options, &problem, &summary);
     if (!summary.IsSolutionUsable())