LM for camera calibration

include/basalt/optimization/accumulator.h

@@ -220,15 +220,16 @@ class SparseHashAccumulator {
     b.template segment<ROWS>(i) += data;
   }
 
-  inline VectorX solve() const {
+  inline VectorX solve(Scalar alpha = 1e-6) const {
     SparseMatrix sm(b.rows(), b.rows());
 
     auto t1 = std::chrono::high_resolution_clock::now();
     std::vector<T> triplets;
     triplets.reserve(hash_map.size() * 36 + b.rows());
 
+    if (alpha > 0)
       for (int i = 0; i < b.rows(); i++) {
-      triplets.emplace_back(i, i, 0.000001);
+        triplets.emplace_back(i, i, alpha);
       }
 
     for (const auto& kv : hash_map) {

include/basalt/optimization/linearize.h

@@ -134,11 +134,10 @@ struct LinearizeBase {
   template <class CamT>
   inline void linearize_point(const Eigen::Vector2d& corner_pos, int corner_id,
                               const Eigen::Matrix4d& T_c_w, const CamT& cam,
-                              PoseCalibH<CamT::N>& cph, double& error,
+                              PoseCalibH<CamT::N>* cph, double& error,
                               int& num_points, double& reproj_err) const {
     Eigen::Matrix<double, 2, 4> d_r_d_p;
     Eigen::Matrix<double, 2, CamT::N> d_r_d_param;
-    Eigen::Matrix<double, 4, 6> d_point_d_xi;
 
     BASALT_ASSERT_STREAM(
         corner_id < int(common_data.aprilgrid_corner_pos_3d->size()),
@@ -147,28 +146,39 @@ struct LinearizeBase {
     Eigen::Vector4d point3d =
         T_c_w * common_data.aprilgrid_corner_pos_3d->at(corner_id);
 
-    d_point_d_xi.topLeftCorner<3, 3>().setIdentity();
-    d_point_d_xi.topRightCorner<3, 3>() = -Sophus::SO3d::hat(point3d.head<3>());
-    d_point_d_xi.row(3).setZero();
-
     Eigen::Vector2d proj;
-    bool valid = cam.project(point3d, proj, &d_r_d_p, &d_r_d_param);
+    bool valid;
-
+    if (cph) {
+      valid = cam.project(point3d, proj, &d_r_d_p, &d_r_d_param);
+    } else {
+      valid = cam.project(point3d, proj);
+    }
     if (!valid) return;
 
-    Eigen::Matrix<double, 2, 6> d_r_d_xi = d_r_d_p * d_point_d_xi;
     Eigen::Vector2d residual = proj - corner_pos;
 
     double e = residual.norm();
     double huber_weight =
         e < common_data.huber_thresh ? 1.0 : common_data.huber_thresh / e;
 
-    cph.H_pose_accum += huber_weight * d_r_d_xi.transpose() * d_r_d_xi;
+    if (cph) {
-    cph.b_pose_accum += huber_weight * d_r_d_xi.transpose() * residual;
+      Eigen::Matrix<double, 4, 6> d_point_d_xi;
 
-    cph.H_intr_pose_accum += huber_weight * d_r_d_param.transpose() * d_r_d_xi;
+      d_point_d_xi.topLeftCorner<3, 3>().setIdentity();
-    cph.H_intr_accum += huber_weight * d_r_d_param.transpose() * d_r_d_param;
+      d_point_d_xi.topRightCorner<3, 3>() =
-    cph.b_intr_accum += huber_weight * d_r_d_param.transpose() * residual;
+          -Sophus::SO3d::hat(point3d.head<3>());
+      d_point_d_xi.row(3).setZero();
+
+      Eigen::Matrix<double, 2, 6> d_r_d_xi = d_r_d_p * d_point_d_xi;
+
+      cph->H_pose_accum += huber_weight * d_r_d_xi.transpose() * d_r_d_xi;
+      cph->b_pose_accum += huber_weight * d_r_d_xi.transpose() * residual;
+
+      cph->H_intr_pose_accum +=
+          huber_weight * d_r_d_param.transpose() * d_r_d_xi;
+      cph->H_intr_accum += huber_weight * d_r_d_param.transpose() * d_r_d_param;
+      cph->b_intr_accum += huber_weight * d_r_d_param.transpose() * residual;
+    }
 
     error += huber_weight * e * e * (2 - huber_weight);
     reproj_err += e;

include/basalt/optimization/poses_linearize.h

@@ -117,7 +117,7 @@ struct LinearizePosesOpt : public LinearizeBase<Scalar> {
 
             for (size_t i = 0; i < acd.corner_pos.size(); i++) {
               this->linearize_point(acd.corner_pos[i], acd.corner_id[i],
-                                    T_c_w_m, cam, cph, err, num_inliers,
+                                    T_c_w_m, cam, &cph, err, num_inliers,
                                     reproj_err);
             }
 
@@ -175,6 +175,89 @@ struct LinearizePosesOpt : public LinearizeBase<Scalar> {
   }
 };
 
+template <typename Scalar>
+struct ComputeErrorPosesOpt : public LinearizeBase<Scalar> {
+  static const int POSE_SIZE = LinearizeBase<Scalar>::POSE_SIZE;
+
+  typedef Sophus::SE3<Scalar> SE3;
+  typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
+  typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
+  typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
+  typedef Eigen::Matrix<Scalar, 6, 1> Vector6;
+
+  typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
+  typedef Eigen::Matrix<Scalar, 6, 6> Matrix6;
+
+  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
+  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
+
+  typedef typename Eigen::vector<AprilgridCornersData>::const_iterator
+      AprilgridCornersDataIter;
+
+  typedef typename LinearizeBase<Scalar>::CalibCommonData CalibCommonData;
+
+  Scalar error;
+  Scalar reprojection_error;
+  int num_points;
+
+  size_t opt_size;
+
+  const Eigen::unordered_map<int64_t, SE3>& timestam_to_pose;
+
+  ComputeErrorPosesOpt(
+      size_t opt_size,
+      const Eigen::unordered_map<int64_t, SE3>& timestam_to_pose,
+      const CalibCommonData& common_data)
+      : opt_size(opt_size), timestam_to_pose(timestam_to_pose) {
+    this->common_data = common_data;
+    error = 0;
+    reprojection_error = 0;
+    num_points = 0;
+  }
+
+  ComputeErrorPosesOpt(const ComputeErrorPosesOpt& other, tbb::split)
+      : opt_size(other.opt_size), timestam_to_pose(other.timestam_to_pose) {
+    this->common_data = other.common_data;
+    error = 0;
+    reprojection_error = 0;
+    num_points = 0;
+  }
+
+  void operator()(const tbb::blocked_range<AprilgridCornersDataIter>& r) {
+    for (const AprilgridCornersData& acd : r) {
+      std::visit(
+          [&](const auto& cam) {
+            SE3 T_w_i = timestam_to_pose.at(acd.timestamp_ns);
+            SE3 T_w_c =
+                T_w_i * this->common_data.calibration->T_i_c[acd.cam_id];
+            SE3 T_c_w = T_w_c.inverse();
+            Eigen::Matrix4d T_c_w_m = T_c_w.matrix();
+
+            double err = 0;
+            double reproj_err = 0;
+            int num_inliers = 0;
+
+            for (size_t i = 0; i < acd.corner_pos.size(); i++) {
+              this->linearize_point(acd.corner_pos[i], acd.corner_id[i],
+                                    T_c_w_m, cam, nullptr, err, num_inliers,
+                                    reproj_err);
+            }
+
+            error += err;
+            reprojection_error += reproj_err;
+            num_points += num_inliers;
+          },
+          this->common_data.calibration->intrinsics[acd.cam_id].variant);
+    }
+  }
+
+  void join(ComputeErrorPosesOpt& rhs) {
+    error += rhs.error;
+    reprojection_error += rhs.reprojection_error;
+    num_points += rhs.num_points;
+  }
+};  // namespace basalt
+
 }  // namespace basalt
 
 #endif

include/basalt/optimization/poses_optimize.h

@@ -60,7 +60,7 @@ class PosesOptimization {
       typename Eigen::vector<AprilgridCornersData>::const_iterator;
 
  public:
-  PosesOptimization() {}
+  PosesOptimization() : lambda(1e-6), min_lambda(1e-6), max_lambda(10) {}
 
   bool initializeIntrinsics(
       size_t cam_id, const Eigen::vector<Eigen::Vector2d> &corners,
@@ -146,20 +146,58 @@ class PosesOptimization {
     num_points = lopt.num_points;
     reprojection_error = lopt.reprojection_error;
 
-    Eigen::VectorXd inc = -lopt.accum.solve();
+    std::cout << "[LINEARIZE] Error: " << lopt.error << " num points "
+              << lopt.num_points << std::endl;
+
+    bool step = false;
+    int max_iter = 10;
+
+    while (!step && max_iter > 0) {
+      Eigen::unordered_map<int64_t, Sophus::SE3d> timestam_to_pose_backup =
+          timestam_to_pose;
+      Eigen::vector<SE3> T_i_c_backup = calib->T_i_c;
+      Eigen::vector<GenericCamera<Scalar>> intrinsics_backup =
+          calib->intrinsics;
+
+      Eigen::VectorXd inc = -lopt.accum.solve(lambda);
 
       for (auto &kv : timestam_to_pose) {
-      kv.second *= Sophus::expd(inc.segment<POSE_SIZE>(offset_poses[kv.first]));
+        kv.second *=
+            Sophus::expd(inc.segment<POSE_SIZE>(offset_poses[kv.first]));
       }
 
       for (size_t i = 0; i < calib->T_i_c.size(); i++) {
-      calib->T_i_c[i] *= Sophus::expd(inc.segment<POSE_SIZE>(offset_T_i_c[i]));
+        calib->T_i_c[i] *=
+            Sophus::expd(inc.segment<POSE_SIZE>(offset_T_i_c[i]));
       }
 
       for (size_t i = 0; i < calib->intrinsics.size(); i++) {
         auto &c = calib->intrinsics[i];
         c.applyInc(inc.segment(offset_cam_intrinsics[i], c.getN()));
       }
+
+      ComputeErrorPosesOpt<double> eopt(problem_size, timestam_to_pose, ccd);
+      tbb::parallel_reduce(april_range, eopt);
+
+      if (eopt.error <= lopt.error) {
+        std::cout << "\t[ACCEPTED] lambda:" << lambda
+                  << " Error: " << eopt.error << " num points "
+                  << eopt.num_points << std::endl;
+
+        lambda = std::max(min_lambda, lambda / 2);
+        step = true;
+      } else {
+        std::cout << "\t[REJECTED] lambda:" << lambda
+                  << " Error: " << eopt.error << " num points "
+                  << eopt.num_points << std::endl;
+        lambda = std::min(max_lambda, 2 * lambda);
+
+        timestam_to_pose = timestam_to_pose_backup;
+        calib->T_i_c = T_i_c_backup;
+        calib->intrinsics = intrinsics_backup;
+      }
+      max_iter--;
+    }
   }
 
   void recompute_size() {
@@ -241,6 +279,8 @@ class PosesOptimization {
   typename LinearizePosesOpt<
       Scalar, SparseHashAccumulator<Scalar>>::CalibCommonData ccd;
 
+  Scalar lambda, min_lambda, max_lambda;
+
   size_t problem_size;
 
   std::unordered_map<int64_t, size_t> offset_poses;

include/basalt/optimization/spline_linearize.h

@@ -395,7 +395,7 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
 
             for (size_t i = 0; i < acd.corner_pos.size(); i++) {
               this->linearize_point(acd.corner_pos[i], acd.corner_id[i],
-                                    T_c_w_m, cam, cph, err, num_inliers,
+                                    T_c_w_m, cam, &cph, err, num_inliers,
                                     reproj_err);
             }