ceres pnp optimization

launch/odometry_eval.launch

@@ -18,7 +18,7 @@
             camera_model: '$(arg camera_model)'
             camera_parameters: $(arg camera_params)
             detector_type: 'GFTT'
-            detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
+            detector_parameters: {maxCorners: 1000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
             tracker_window_size: 128
             tracker_num_scales: 4
             tracker_delta_pixel_error_threshold: 3.0

src/module/odometry_module.cc

@@ -7,13 +7,14 @@ namespace omni_slam
 namespace module
 {
 
-OdometryModule::OdometryModule(std::unique_ptr<odometry::PNP> &pnp)
-    : pnp_(std::move(pnp))
+OdometryModule::OdometryModule(std::unique_ptr<odometry::PNP> &pnp, std::unique_ptr<optimization::BundleAdjuster> &bundle_adjuster)
+    : pnp_(std::move(pnp)),
+    bundleAdjuster_(std::move(bundle_adjuster))
 {
 }
 
-OdometryModule::OdometryModule(std::unique_ptr<odometry::PNP> &&pnp)
-    : OdometryModule(pnp)
+OdometryModule::OdometryModule(std::unique_ptr<odometry::PNP> &&pnp, std::unique_ptr<optimization::BundleAdjuster> &&bundle_adjuster)
+    : OdometryModule(pnp, bundle_adjuster)
 {
 }
 
@@ -22,6 +23,11 @@ void OdometryModule::Update(std::vector<data::Landmark> &landmarks, data::Frame
     pnp_->Compute(landmarks, frame);
 }
 
+void OdometryModule::BundleAdjust(std::vector<data::Landmark> &landmarks)
+{
+    bundleAdjuster_->Optimize(landmarks);
+}
+
 OdometryModule::Stats& OdometryModule::GetStats()
 {
     return stats_;

src/module/odometry_module.h

@@ -6,6 +6,7 @@
 #include <memory>
 
 #include "odometry/pnp.h"
+#include "optimization/bundle_adjuster.h"
 #include "data/landmark.h"
 
 #include <pcl/common/projection_matrix.h>
@@ -25,15 +26,17 @@ class OdometryModule
     {
     };
 
-    OdometryModule(std::unique_ptr<odometry::PNP> &pnp);
-    OdometryModule(std::unique_ptr<odometry::PNP> &&pnp);
+    OdometryModule(std::unique_ptr<odometry::PNP> &pnp, std::unique_ptr<optimization::BundleAdjuster> &bundle_adjuster);
+    OdometryModule(std::unique_ptr<odometry::PNP> &&pnp, std::unique_ptr<optimization::BundleAdjuster> &&bundle_adjuster);
 
     void Update(std::vector<data::Landmark> &landmarks, data::Frame &frame);
+    void BundleAdjust(std::vector<data::Landmark> &landmarks);
 
     Stats& GetStats();
 
 private:
     std::shared_ptr<odometry::PNP> pnp_;
+    std::shared_ptr<optimization::BundleAdjuster> bundleAdjuster_;
 
     Stats stats_;
 };

src/odometry/pnp.cc

@@ -1,6 +1,11 @@
 #include "pnp.h"
 #include "lambda_twist.h"
 
+#include <ceres/ceres.h>
+#include "optimization/reprojection_error.h"
+#include "camera/double_sphere.h"
+#include "camera/perspective.h"
+
 #include "util/tf_util.h"
 
 #include <random>
@@ -22,6 +27,7 @@ int PNP::Compute(const std::vector<data::Landmark> &landmarks, data::Frame &fram
     std::vector<Vector3d> xs;
     std::vector<Vector2d> yns;
     std::vector<Vector3d> ys;
+    std::vector<const data::Feature*> features;
     for (const data::Landmark &landmark : landmarks)
     {
         if (landmark.IsObservedInFrame(frame.GetID()))
@@ -43,10 +49,16 @@ int PNP::Compute(const std::vector<data::Landmark> &landmarks, data::Frame &fram
             pix << feat->GetKeypoint().pt.x, feat->GetKeypoint().pt.y;
             yns.push_back(pix);
             ys.push_back(feat->GetBearing());
+            features.push_back(feat);
         }
     }
     Matrix<double, 3, 4> pose;
     int inliers = RANSAC(xs, ys, yns, frame.GetCameraModel(), pose);
+    std::vector<int> indices = GetInlierIndices(xs, yns, pose, frame.GetCameraModel());
+    if (inliers > 3)
+    {
+        Refine(xs, features, indices, pose);
+    }
     frame.SetEstimatedInversePose(pose);
     return inliers;
 }
@@ -74,7 +86,7 @@ int PNP::RANSAC(const std::vector<Vector3d> &xs, const std::vector<Vector3d> &ys
             continue;
         }
 
-        int inliers = CountInliers(xs, yns, iterPose, camera_model);
+        int inliers = GetInlierIndices(xs, yns, iterPose, camera_model).size();
         if (inliers > maxInliers)
         {
             maxInliers = inliers;
@@ -84,6 +96,55 @@ int PNP::RANSAC(const std::vector<Vector3d> &xs, const std::vector<Vector3d> &ys
     return maxInliers;
 }
 
+bool PNP::Refine(const std::vector<Vector3d> &xs, const std::vector<const data::Feature*> &features, const std::vector<int> indices, Matrix<double, 3, 4> &pose) const
+{
+    ceres::Problem problem;
+    ceres::LossFunction *loss_function = nullptr;
+    std::vector<double> landmarks;
+    landmarks.reserve(3 * indices.size());
+    Quaterniond quat(pose.block<3, 3>(0, 0));
+    quat.normalize();
+    Vector3d t(pose.block<3, 1>(0, 3));
+    std::vector<double> quatData(quat.coeffs().data(), quat.coeffs().data() + 4);
+    std::vector<double> tData(t.data(), t.data() + 3);
+    for (int i : indices)
+    {
+        const Vector3d &x = xs[i];
+        landmarks.push_back(x(0));
+        landmarks.push_back(x(1));
+        landmarks.push_back(x(2));
+        ceres::CostFunction *cost_function = nullptr;
+        if (features[i]->GetFrame().GetCameraModel().GetType() == camera::CameraModel<>::kPerspective)
+        {
+            cost_function = optimization::ReprojectionError<camera::Perspective>::Create(*features[i]);
+        }
+        else if (features[i]->GetFrame().GetCameraModel().GetType() == camera::CameraModel<>::kDoubleSphere)
+        {
+            cost_function = optimization::ReprojectionError<camera::DoubleSphere>::Create(*features[i]);
+        }
+        if (cost_function != nullptr)
+        {
+            problem.AddResidualBlock(cost_function, loss_function, &quatData[0], &tData[0], &landmarks[landmarks.size() - 3]);
+            problem.SetParameterBlockConstant(&landmarks[landmarks.size() - 3]);
+        }
+    }
+    ceres::Solver::Options options;
+    options.max_num_iterations = 10;
+    options.linear_solver_type = ceres::DENSE_SCHUR;
+    options.function_tolerance = 1e-6;
+    options.gradient_tolerance = 1e-6;
+    ceres::Solver::Summary summary;
+    ceres::Solve(options, &problem, &summary);
+    if (!summary.IsSolutionUsable())
+    {
+        return false;
+    }
+    const Quaterniond quatRes = Map<const Quaterniond>(&quatData[0]);
+    const Vector3d tRes = Map<const Vector3d>(&tData[0]);
+    pose = util::TFUtil::QuaternionTranslationToPoseMatrix(quatRes, tRes);
+    return true;
+}
+
 double PNP::P4P(const std::vector<Vector3d> &xs, const std::vector<Vector3d> &ys, const std::vector<Vector2d> &yns, std::vector<int> indices, const camera::CameraModel<> &camera_model, Matrix<double, 3, 4> &pose) const
 {
     std::vector<Matrix3d> Rs(4);
@@ -135,9 +196,9 @@ double PNP::P4P(const std::vector<Vector3d> &xs, const std::vector<Vector3d> &ys
     return error;
 }
 
-int PNP::CountInliers(const std::vector<Vector3d> &xs, const std::vector<Vector2d> &yns, const Matrix<double, 3, 4> &pose, const camera::CameraModel<> &camera_model) const
+std::vector<int> PNP::GetInlierIndices(const std::vector<Vector3d> &xs, const std::vector<Vector2d> &yns, const Matrix<double, 3, 4> &pose, const camera::CameraModel<> &camera_model) const
 {
-    int inliers = 0;
+    std::vector<int> indices;
     double thresh = reprojThreshold_ * reprojThreshold_;
     for (int i = 0; i < xs.size(); i++)
     {
@@ -147,9 +208,12 @@ int PNP::CountInliers(const std::vector<Vector3d> &xs, const std::vector<Vector2
             continue;
         }
         double err = (xr - yns[i]).squaredNorm();
-        inliers += (err < thresh) ? 1 : 0;
+        if (err < thresh)
+        {
+            indices.push_back(i);
+        }
     }
-    return inliers;
+    return indices;
 }
 
 }

src/odometry/pnp.h

@@ -22,8 +22,9 @@ class PNP
 
 private:
     int 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;
+    bool Refine(const std::vector<Vector3d> &xs, const std::vector<const data::Feature*> &features, const std::vector<int> indices, Matrix<double, 3, 4> &pose) const;
     double P4P(const std::vector<Vector3d> &xs, const std::vector<Vector3d> &ys, const std::vector<Vector2d> &yns, std::vector<int> indices, const camera::CameraModel<> &camera_model, Matrix<double, 3, 4> &pose) const;
-    int CountInliers(const std::vector<Vector3d> &xs, const std::vector<Vector2d> &yns, const Matrix<double, 3, 4> &pose, const camera::CameraModel<> &camera_model) const;
+    std::vector<int> GetInlierIndices(const std::vector<Vector3d> &xs, const std::vector<Vector2d> &yns, const Matrix<double, 3, 4> &pose, const camera::CameraModel<> &camera_model) const;
 
     int ransacIterations_;
     double reprojThreshold_;

src/optimization/bundle_adjuster.cc

@@ -24,7 +24,7 @@ bool BundleAdjuster::Optimize(std::vector<data::Landmark> &landmarks)
 {
     std::vector<double> landmarkEstimates;
     landmarkEstimates.reserve(3 * landmarks.size());
-    std::map<int, std::vector<double>> framePoses;
+    std::map<int, std::pair<std::vector<double>, std::vector<double>>> framePoses;
     std::map<int, data::Frame*> estFrames;
     ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);
     for (const data::Landmark &landmark : landmarks)
@@ -69,17 +69,29 @@ bool BundleAdjuster::Optimize(std::vector<data::Landmark> &landmarks)
                 }
                 if (framePoses.find(feature.GetFrame().GetID()) == framePoses.end())
                 {
-                    framePoses[feature.GetFrame().GetID()] = std::vector<double>(feature.GetFrame().GetInversePose().data(), feature.GetFrame().GetInversePose().data() + 12);
-                    problem_->AddParameterBlock(&framePoses[feature.GetFrame().GetID()][0], 12);
-                    problem_->SetParameterBlockConstant(&framePoses[feature.GetFrame().GetID()][0]);
+                    const Matrix<double, 3, 4> &pose = feature.GetFrame().GetInversePose();
+                    Quaterniond quat(pose.block<3, 3>(0, 0));
+                    quat.normalize();
+                    const Vector3d &t = pose.block<3, 1>(0, 3);
+                    framePoses[feature.GetFrame().GetID()] = {std::vector<double>(quat.coeffs().data(), quat.coeffs().data() + 4), std::vector<double>(t.data(), t.data() + 3)};
+                    problem_->AddParameterBlock(&framePoses[feature.GetFrame().GetID()].first[0], 4);
+                    problem_->AddParameterBlock(&framePoses[feature.GetFrame().GetID()].second[0], 3);
+                    problem_->SetParameterBlockConstant(&framePoses[feature.GetFrame().GetID()].first[0]);
+                    problem_->SetParameterBlockConstant(&framePoses[feature.GetFrame().GetID()].second[0]);
                 }
             }
             else if (feature.GetFrame().HasEstimatedPose())
             {
                 if (framePoses.find(feature.GetFrame().GetID()) == framePoses.end())
                 {
-                    framePoses[feature.GetFrame().GetID()] = std::vector<double>(feature.GetFrame().GetInversePose().data(), feature.GetFrame().GetInversePose().data() + 12);
-                    problem_->AddParameterBlock(&framePoses[feature.GetFrame().GetID()][0], 12);
+                    const Matrix<double, 3, 4> &pose = feature.GetFrame().GetInversePose();
+                    Quaterniond quat(pose.block<3, 3>(0, 0));
+                    quat.normalize();
+                    const Vector3d &t = pose.block<3, 1>(0, 3);
+                    framePoses[feature.GetFrame().GetID()] = {std::vector<double>(quat.coeffs().data(), quat.coeffs().data() + 4), std::vector<double>(t.data(), t.data() + 3)};
+                    problem_->AddParameterBlock(&framePoses[feature.GetFrame().GetID()].first[0], 4);
+                    problem_->AddParameterBlock(&framePoses[feature.GetFrame().GetID()].second[0], 3);
+                    problem_->SetParameterization(&framePoses[feature.GetFrame().GetID()].first[0], new ceres::EigenQuaternionParameterization());
                     estFrames[feature.GetFrame().GetID()] = const_cast<data::Frame*>(&feature.GetFrame());
                 }
             }
@@ -98,11 +110,15 @@ bool BundleAdjuster::Optimize(std::vector<data::Landmark> &landmarks)
             }
             if (cost_function != nullptr)
             {
-                problem_->AddResidualBlock(cost_function, loss_function, &framePoses[feature.GetFrame().GetID()][0], &landmarkEstimates[landmarkEstimates.size() - 3]);
+                problem_->AddResidualBlock(cost_function, loss_function, &framePoses[feature.GetFrame().GetID()].first[0], &framePoses[feature.GetFrame().GetID()].second[0], &landmarkEstimates[landmarkEstimates.size() - 3]);
             }
         }
     }
 
+    if (solverOptions_.minimizer_progress_to_stdout)
+    {
+        std::cout << "Running bundle adjustment..." << std::endl;
+    }
     ceres::Solver::Summary summary;
     ceres::Solve(solverOptions_, problem_.get(), &summary);
     if (solverOptions_.minimizer_progress_to_stdout)
@@ -139,7 +155,9 @@ bool BundleAdjuster::Optimize(std::vector<data::Landmark> &landmarks)
     }
     for (auto &frame : estFrames)
     {
-        const Matrix<double, 3, 4> pose = Map<const Matrix<double, 3, 4>>(&framePoses[frame.first][0]);
+        const Quaterniond quat = Map<const Quaterniond>(&framePoses[frame.first].first[0]);
+        const Vector3d t = Map<const Vector3d>(&framePoses[frame.first].second[0]);
+        const Matrix<double, 3, 4> pose = util::TFUtil::QuaternionTranslationToPoseMatrix(quat, t);
         frame.second->SetEstimatedInversePose(pose);
     }
     return true;

src/optimization/reprojection_error.h

@@ -20,11 +20,13 @@ class ReprojectionError
     }
 
     template<typename T>
-    bool operator()(const T* const camera_pose, const T* const point, T *reproj_error) const
+    bool operator()(const T* const camera_orientation, const T* const camera_translation, const T* const point, T *reproj_error) const
     {
         Matrix<T, 2, 1> reprojPoint;
         C<T> camera(feature_.GetFrame().GetCameraModel());
-        const Matrix<T, 3, 4> pose = Map<const Matrix<T, 3, 4>>(camera_pose);
+        const Quaternion<T> orientation = Map<const Quaternion<T>>(camera_orientation);
+        const Matrix<T, 3, 1> translation = Map<const Matrix<T, 3, 1>>(camera_translation);
+        const Matrix<T, 3, 4> pose = util::TFUtil::QuaternionTranslationToPoseMatrix(orientation, translation);
         const Matrix<T, 3, 1> worldPt = Map<const Matrix<T, 3, 1>>(point);
         const Matrix<T, 3, 1> camPt = util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(pose, worldPt));
         camera.ProjectToImage(camPt, reprojPoint);
@@ -35,7 +37,7 @@ class ReprojectionError
 
     static ceres::CostFunction* Create(const data::Feature &feature)
     {
-        return new ceres::AutoDiffCostFunction<ReprojectionError, 2, 12, 3>(new ReprojectionError<C>(feature));
+        return new ceres::AutoDiffCostFunction<ReprojectionError, 2, 4, 3, 3>(new ReprojectionError<C>(feature));
     }
 
 private:

src/ros/odometry_eval.cc

@@ -1,6 +1,7 @@
 #include "odometry_eval.h"
 
 #include "odometry/pnp.h"
+#include "optimization/bundle_adjuster.h"
 #include "module/tracking_module.h"
 
 #include "geometry_msgs/PoseStamped.h"
@@ -26,8 +27,9 @@ OdometryEval::OdometryEval(const ::ros::NodeHandle &nh, const ::ros::NodeHandle
     nhp_.param("bundle_adjustment_logging", logCeres, false);
 
     unique_ptr<odometry::PNP> pnp(new odometry::PNP(iterations, reprojThresh));
+    unique_ptr<optimization::BundleAdjuster> bundleAdjuster(new optimization::BundleAdjuster(baMaxIter, logCeres));
 
-    odometryModule_.reset(new module::OdometryModule(pnp));
+    odometryModule_.reset(new module::OdometryModule(pnp, bundleAdjuster));
 }
 
 void OdometryEval::InitPublishers()
@@ -50,6 +52,7 @@ void OdometryEval::ProcessFrame(unique_ptr<data::Frame> &&frame)
 
 void OdometryEval::Finish()
 {
+    odometryModule_->BundleAdjust(trackingModule_->GetLandmarks());
 }
 
 void OdometryEval::GetResultsData(std::map<std::string, std::vector<std::vector<double>>> &data)