Add interface and change projection model

.gitignore

@@ -12,7 +12,7 @@ ba_log.ubjson
 __pycache__
 /*.cereal
 /rootba_config.toml
-
+.vscode
 # generic:
 .DS_Store
 *~

src/rootba/bal/bal_bundle_adjustment_helper.cpp

@@ -112,7 +112,7 @@ template <typename Scalar>
 bool BalBundleAdjustmentHelper<Scalar>::linearize_point(
     const Vec2& obs, const Vec3& lm_p_w, const SE3& T_c_w,
     const basalt::BalCamera<Scalar>& intr, const bool ignore_validity_check,
-    VecR& res, MatRP* d_res_d_xi, MatRI* d_res_d_i, MatRL* d_res_d_l) {
+    VecR& res, MatRP* d_res_d_xi, MatRI* d_res_d_i, MatRL* d_res_d_l, bool fix_cam) {
   Mat4 T_c_w_mat = T_c_w.matrix();
 
   Vec4 p_c_3d = T_c_w_mat * lm_p_w.homogeneous();
@@ -133,12 +133,16 @@ bool BalBundleAdjustmentHelper<Scalar>::linearize_point(
   }
 
   if (d_res_d_xi) {
-    Mat<Scalar, 4, POSE_SIZE> d_point_d_xi;
-    d_point_d_xi.template topLeftCorner<3, 3>() = Mat3::Identity();
-    d_point_d_xi.template topRightCorner<3, 3>() =
-        -SO3::hat(p_c_3d.template head<3>());
-    d_point_d_xi.row(3).setZero();
-    *d_res_d_xi = d_res_d_p * d_point_d_xi;
+    if(!fix_cam){
+      Mat<Scalar, 4, POSE_SIZE> d_point_d_xi;
+      d_point_d_xi.template topLeftCorner<3, 3>() = Mat3::Identity();
+      d_point_d_xi.template topRightCorner<3, 3>() =
+          -SO3::hat(p_c_3d.template head<3>());
+      d_point_d_xi.row(3).setZero();
+      *d_res_d_xi = d_res_d_p * d_point_d_xi;
+    }else{
+      d_res_d_xi->setZero();
+    }
   }
 
   if (d_res_d_l) {

src/rootba/bal/bal_bundle_adjustment_helper.hpp

@@ -82,7 +82,8 @@ class BalBundleAdjustmentHelper {
                               bool ignore_validity_check, VecR& res,
                               MatRP* d_res_d_xi = nullptr,
                               MatRI* d_res_d_i = nullptr,
-                              MatRL* d_res_d_l = nullptr);
+                              MatRL* d_res_d_l = nullptr,
+                              bool fix_cam = false);
 };
 
 }  // namespace rootba

src/rootba/bal/bal_problem.cpp

@@ -184,6 +184,49 @@ BalProblem<Scalar>::BalProblem(const std::string& path) {
   load_bal(path);
 }
 
+template <typename Scalar>
+void BalProblem<Scalar>::init_problem(int num_cams, int num_lms){
+    // clear memory and re-allocate
+    if (cameras_.capacity() > unsigned_cast(num_cams)) {
+      decltype(cameras_)().swap(cameras_);
+    }
+    if (landmarks_.capacity() > unsigned_cast(num_lms)) {
+      decltype(landmarks_)().swap(landmarks_);
+    }
+    cameras_.resize(num_cams);
+    landmarks_.resize(num_lms);
+    forceStopFlag_ = nullptr;
+}
+
+template <typename Scalar>
+void BalProblem<Scalar>::add_cam(int cam_idx, Eigen::Quaterniond &R_cw, Eigen::Vector3d &t_cw, Eigen::Vector4d &intr){
+  auto& cam = cameras_.at(cam_idx);
+
+  cam.T_c_w.so3()  = SO3(R_cw.cast<Scalar>());
+  cam.T_c_w.translation() = t_cw.cast<Scalar>();
+  cam.intrinsics = CameraModel();
+  cam.intrinsics.setPinholeIntrinsic(intr.cast<Scalar>());
+}
+
+template <typename Scalar>
+void BalProblem<Scalar>::add_landmark(int lm_idx, const Eigen::Vector3d &p_w){
+  landmarks_.at(lm_idx).p_w = p_w.cast<Scalar>();
+}
+
+template <typename Scalar>
+void BalProblem<Scalar>::set_observation(int cam_idx, int lm_idx, const Eigen::Vector2d &pixel_obs){
+  auto [obs, inserted] = landmarks_.at(lm_idx).obs.try_emplace(cam_idx);
+  CHECK(inserted) << "Repeated observation!";
+
+  obs->second.pos = pixel_obs.cast<Scalar>();
+}
+
+template <typename Scalar>
+void BalProblem<Scalar>::fix_cam(int cam_idx){
+  auto& cam = cameras_.at(cam_idx);
+  cam.setFixed(true);
+}
+
 template <typename Scalar>
 void BalProblem<Scalar>::load_bal(const std::string& path) {
   FILE* fptr = std::fopen(path.c_str(), "r");
@@ -213,6 +256,7 @@ void BalProblem<Scalar>::load_bal(const std::string& path) {
     cameras_.resize(num_cams);
     landmarks_.resize(num_lms);
 
+    LOG(INFO) << "cam size:" << num_cams << "  num_lms size:" << num_lms << std::endl;
     // parse observations
     for (int i = 0; i < num_obs; ++i) {
       int cam_idx;
@@ -238,11 +282,11 @@ void BalProblem<Scalar>::load_bal(const std::string& path) {
       // we don't have the "minus" like in the original Snavely model.
 
       // invert y axis
-      obs->second.pos.y() = -obs->second.pos.y();
+      // obs->second.pos.y() = -obs->second.pos.y();
     }
 
     // invert y and z axis (same as rotation around x by 180; self-inverse)
-    const SO3 axis_inversion = SO3(Vec3(1, -1, -1).asDiagonal());
+    // const SO3 axis_inversion = SO3(Vec3(1, -1, -1).asDiagonal());
 
     // parse camera parameters
     for (int i = 0; i < num_cams; ++i) {
@@ -252,9 +296,12 @@ void BalProblem<Scalar>::load_bal(const std::string& path) {
       params = paramsd.cast<Scalar>();
 
       auto& cam = cameras_.at(i);
-      cam.T_c_w.so3() = axis_inversion * SO3::exp(params.template head<3>());
-      cam.T_c_w.translation() = axis_inversion * params.template segment<3>(3);
+      cam.T_c_w.so3() = SO3::exp(params.template head<3>());
+      cam.T_c_w.translation() = params.template segment<3>(3);
       cam.intrinsics = CameraModel(params.template tail<3>());
+      Eigen::Vector4d intrinsics(718.856, 718.856, 607.1928, 185.2157);
+
+      cam.intrinsics.setPinholeIntrinsic(intrinsics.cast<Scalar>());
     }
 
     // parse landmark parameters

src/rootba/bal/bal_problem.hpp

@@ -94,6 +94,13 @@ class BalProblem {
       intrinsics = CameraModel(p.template tail<3>());
     }
 
+    inline void setFixed(bool fixed = true){
+      fixed_=fixed;
+    }
+    bool isFixed() const{
+      return fixed_;
+    }
+
     void apply_inc_pose(const Vec6& inc) { inc_pose(inc, T_c_w); }
 
     inline static void inc_pose(const Vec6& inc, SE3& T_c_w) {
@@ -128,6 +135,7 @@ class BalProblem {
     }
 
    private:
+    bool fixed_ = false;
     SE3 T_c_w_backup_;
     CameraModel intrinsics_backup_;
   };
@@ -161,6 +169,16 @@ class BalProblem {
   BalProblem() = default;
   explicit BalProblem(const std::string& path);
 
+  void init_problem(int num_cams, int num_lms);
+
+  void add_cam(int cam_idx, Eigen::Quaterniond &R_cw, Eigen::Vector3d &t_cw, Eigen::Vector4d &intr);
+  void add_landmark(int lm_idx, const Eigen::Vector3d &p_w);
+  void set_observation(int cam_idx, int lm_idx, const Eigen::Vector2d &pixel_obs);
+  void fix_cam(int cam_idx);
+
+  void set_force_stop_flag(bool *flag) { forceStopFlag_ = flag; }
+  bool terminate() {return forceStopFlag_ ? (*forceStopFlag_) : false; }
+
   void load_bal(const std::string& path);
   void load_bundler(const std::string& path);
 
@@ -227,6 +245,7 @@ class BalProblem {
   Cameras cameras_;
   Landmarks landmarks_;
 
+  bool *forceStopFlag_;
   /// quiet means no INFO level log output
   bool quiet_ = false;
 };

src/rootba/bal/bal_residual_options.hpp

@@ -50,14 +50,14 @@ struct BalResidualOptions : public VisitableOptions<BalResidualOptions> {
   BEGIN_VISITABLES(BalResidualOptions);
 
   VISITABLE_META(RobustNorm, robust_norm,
-                 init(RobustNorm::NONE)
+                 init(RobustNorm::HUBER)
                      .help("which robust norm to use. NONE: squared norm, "
                            "HUBER: Huber norm."));
 
   // ceres uses value of 1.0 in bundle_adjuster example
   VISITABLE_META(
       double, huber_parameter,
-      init(1).range(0, 10).help("huber parameter for robust norm in pixels"));
+      init(sqrt(5.991)).range(0, 10).help("huber parameter for robust norm in pixels"));
 
   END_VISITABLES;
 };

src/rootba/bal/solver_options.hpp

@@ -84,15 +84,15 @@ struct SolverOptions : public VisitableOptions<SolverOptions> {
 
   // solver type
   VISITABLE_META(SolverType, solver_type,
-                 init(SolverType::SQUARE_ROOT)
+                 init(SolverType::SCHUR_COMPLEMENT)
                      .help("Solver type; 'SQUARE_ROOT' for square root BA, "
                            "'SCHUR_COMPLEMENT' for classical Schur complement "
                            "BA, 'CERES' for a Ceres-based implementation."));
 
   // verbosity
   VISITABLE_META(
       int, verbosity_level,
-      init(2).range(0, 2).help("Output verbosity level. 0: silent, 1: brief "
+      init(0).range(0, 2).help("Output verbosity level. 0: silent, 1: brief "
                                "report (one line), 2: full report"));
 
   // debug output
@@ -120,7 +120,7 @@ struct SolverOptions : public VisitableOptions<SolverOptions> {
   // optimized cost in lm (for ceres we can only do Error and ErrorValid)
   VISITABLE_META(
       OptimizedCost, optimized_cost,
-      init(OptimizedCost::ERROR)
+      init(OptimizedCost::ERROR_VALID)
           .help("Which cost to consider for the 'cost decreased?' check in "
                 "Levenberg-Marqardt. ERROR considers all residuals, "
                 "ERROR_VALID ignores residuals with negative z, and "

src/rootba/ceres/CMakeLists.txt

@@ -19,6 +19,8 @@ target_sources(rootba_ceres
 
 # include order: ceres first; ensure ceres is found first in the specified location, not in /usr/local (on macOS with homebrew)
 target_link_libraries(rootba_ceres PUBLIC Ceres::ceres)
+# add_subdirectory(g2o)
+# target_link_libraries(rootba_ceres PUBLIC g2o)
 
 target_link_libraries(rootba_ceres
   PUBLIC

src/rootba/ceres/bal_bundle_adjustment.cpp

@@ -44,6 +44,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rootba/ceres/bal_residuals.hpp"
 #include "rootba/ceres/option_utils.hpp"
 
+// #include "g2o/g2o/core/block_solver.h"
+// #include "g2o/g2o/core/optimization_algorithm_levenberg.h"
+// #include "g2o/g2o/solvers/linear_solver_eigen.h"
+// #include "g2o/g2o/types/types_six_dof_expmap.h"
+// #include "g2o/g2o/core/robust_kernel_impl.h"
+// #include "g2o/g2o/solvers/linear_solver_dense.h"
+// #include "g2o/g2o/types/types_seven_dof_expmap.h"
+
 namespace rootba {
 
 void bundle_adjust_ceres(BalProblem<double>& bal_problem,
@@ -123,4 +131,86 @@ void bundle_adjust_ceres(BalProblem<double>& bal_problem,
   }
 }
 
+
+// void bundle_adjust_g2o(BalProblem<double>& bal_problem,
+//                        const SolverOptions& solver_options, BaLog* log) {
+//   // options
+//   const bool projection_validitity_check =
+//       ceres_use_projection_validity_check(solver_options);
+
+//   // Setup optimizer
+//   g2o::SparseOptimizer optimizer;
+//   g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
+
+//   linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
+
+//   g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
+
+//   g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
+//   optimizer.setAlgorithm(solver);
+//   optimizer.setVerbose(true);
+
+//   const rootba::BalProblem<double>::Cameras &cams = bal_problem.cameras();
+//   const rootba::BalProblem<double>::Landmarks &landmarks = bal_problem.landmarks();
+
+//   for (int i = 0; i < cams.size(); ++i) {
+//     const rootba::BalProblem<double>::Camera &cam = cams.at(i);
+//     g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
+//     vSE3->setEstimate(g2o::SE3Quat(cam.T_c_w.so3().matrix(),cam.T_c_w.translation()));
+//     vSE3->setId(i);
+//     vSE3->setFixed(i==0);
+//     optimizer.addVertex(vSE3);
+//   }
+
+//   for (int i = 0; i < landmarks.size(); ++i) {
+//     const rootba::BalProblem<double>::Landmark &lm = landmarks.at(i);
+
+//     g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
+//     vPoint->setEstimate(lm.p_w);
+//     int id = cams.size()+i;
+//     vPoint->setId(id);
+//     vPoint->setMarginalized(true);
+//     optimizer.addVertex(vPoint);
+
+//     for (const auto& [cam_idx, obs] : lm.obs) {
+//       g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
+
+//       e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
+//       e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(cam_idx)));
+//       e->setMeasurement(obs.pos);
+//       e->setInformation(Eigen::Matrix2d::Identity());
+
+//       g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
+//       e->setRobustKernel(rk);
+//       rk->setDelta(sqrt(5.991));
+
+//       e->fx = 718.856;
+//       e->fy = 718.856;
+//       e->cx = 607.1928;
+//       e->cy = 185.2157;
+//       optimizer.addEdge(e);
+//     }
+//   }
+
+//   optimizer.initializeOptimization();
+//   optimizer.optimize(100);
+//   using SO3 = Sophus::SO3<double>;
+
+//   //Keyframes
+//   // for (int i = 0; i < cams.size(); ++i) {
+//   //     g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(i));
+//   //     g2o::SE3Quat SE3quat = vSE3->estimate();
+
+//   //     rootba::BalProblem<double>::Camera &cam = bal_problem.cameras().at(i);
+//   //     cam.T_c_w.so3() = SO3(SE3quat.rotation());
+
+//   //     cam.T_c_w.translation() = SE3quat.translation();
+//   // }
+
+//   // //Points
+//   // for (int i = 0; i < landmarks.size(); ++i) {
+//   //     g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(i+cams.size()));
+//   //     bal_problem.landmarks().at(i).p_w = vPoint->estimate();
+//   // }
+// }
 }  // namespace rootba

src/rootba/ceres/bal_bundle_adjustment.hpp

@@ -44,4 +44,8 @@ void bundle_adjust_ceres(BalProblem<double>& bal_problem,
                          const SolverOptions& solver_options,
                          BaLog* log = nullptr);
 
+void bundle_adjust_g2o(BalProblem<double>& bal_problem,
+                         const SolverOptions& solver_options,
+                         BaLog* log = nullptr);
+
 }

src/rootba/qr/impl/landmark_block_base.ipp

@@ -113,7 +113,7 @@ void LandmarkBlockBase<T, Scalar, POSE_SIZE>::linearize_landmark(
     Vec2 res;
     const bool valid = BalBundleAdjustmentHelper<Scalar>::linearize_point(
         obs.pos, lm_ptr_->p_w, cam.T_c_w, cam.intrinsics, true, res, &Jp, &Ji,
-        &Jl);
+        &Jl, cam.isFixed());
 
     if (!options_.use_valid_projections_only || valid) {
       numerically_valid = numerically_valid && Jl.array().isFinite().all() &&