new triangulation method

include/basalt/vi_estimator/ba_base.h

@@ -148,6 +148,7 @@ class BundleAdjustmentBase {
 
     Eigen::Matrix<double, 2, 4> Jp;
     bool valid = cam.project(p_t_3d, res, &Jp);
+    valid &= res.array().isFinite().all();
 
     if (!valid) {
       //      std::cerr << " Invalid projection! kpt_pos.dir "
@@ -197,6 +198,7 @@ class BundleAdjustmentBase {
 
     Eigen::Matrix<double, 2, 4> Jp;
     bool valid = cam.project(p_h_3d, res, &Jp);
+    valid &= res.array().isFinite().all();
 
     if (!valid) {
       //      std::cerr << " Invalid projection! kpt_pos.dir "
@@ -246,9 +248,46 @@ class BundleAdjustmentBase {
                                 Eigen::MatrixXd& marg_H,
                                 Eigen::VectorXd& marg_b);
 
-  static Eigen::Vector4d triangulate(const Eigen::Vector3d& p0_3d,
-                                     const Eigen::Vector3d& p1_3d,
-                                     const Sophus::SE3d& T_0_1);
+  /// Triangulates the point and returns homogenous representation. First 3
+  /// components - unit-length direction vector. Last component inverse
+  /// distance.
+  template <class Derived>
+  static Eigen::Matrix<typename Derived::Scalar, 4, 1> triangulate(
+      const Eigen::MatrixBase<Derived>& p0_3d,
+      const Eigen::MatrixBase<Derived>& p1_3d,
+      const Sophus::SE3<typename Derived::Scalar>& T_0_1) {
+    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
+
+    using Scalar = typename Derived::Scalar;
+    using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+    using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
+    using Mat33 = Eigen::Matrix<Scalar, 3, 3>;
+
+    Vec3 t12 = T_0_1.translation();
+    Mat33 R12 = T_0_1.rotationMatrix();
+    Eigen::Matrix<Scalar, 3, 4> P1 = Eigen::Matrix<Scalar, 3, 4>::Zero();
+    P1.template block<3, 3>(0, 0) = Eigen::Matrix<Scalar, 3, 3>::Identity();
+    Eigen::Matrix<Scalar, 3, 4> P2 = Eigen::Matrix<Scalar, 3, 4>::Zero();
+    P2.template block<3, 3>(0, 0) = R12.transpose();
+    P2.template block<3, 1>(0, 3) = -R12.transpose() * t12;
+    Vec3 f1 = p0_3d;
+    Vec3 f2 = p1_3d;
+
+    Eigen::Matrix<Scalar, 4, 4> A(4, 4);
+    A.row(0) = f1[0] * P1.row(2) - f1[2] * P1.row(0);
+    A.row(1) = f1[1] * P1.row(2) - f1[2] * P1.row(1);
+    A.row(2) = f2[0] * P2.row(2) - f2[2] * P2.row(0);
+    A.row(3) = f2[1] * P2.row(2) - f2[2] * P2.row(1);
+
+    Eigen::JacobiSVD<Eigen::Matrix<Scalar, 4, 4>> mySVD(A, Eigen::ComputeFullV);
+    Vec4 worldPoint = mySVD.matrixV().col(3);
+    worldPoint /= worldPoint.template head<3>().norm();
+
+    // Enforse same direction of bearing vector and initial point
+    if (p0_3d.dot(worldPoint.template head<3>()) < 0) worldPoint *= -1;
+
+    return worldPoint;
+  }
 
   template <class AccumT>
   static void linearizeAbs(const Eigen::MatrixXd& rel_H,

src/vi_estimator/ba_base.cpp

@@ -431,29 +431,6 @@ void BundleAdjustmentBase::get_current_points(
   }
 }
 
-Eigen::Vector4d BundleAdjustmentBase::triangulate(const Eigen::Vector3d& p0_3d,
-                                                  const Eigen::Vector3d& p1_3d,
-                                                  const Sophus::SE3d& T_0_1) {
-  Eigen::Vector3d p1_3d_unrotated = T_0_1.so3() * p1_3d;
-  Eigen::Vector2d b;
-  b[0] = T_0_1.translation().dot(p0_3d);
-  b[1] = T_0_1.translation().dot(p1_3d_unrotated);
-  Eigen::Matrix2d A;
-  A(0, 0) = p0_3d.dot(p0_3d);
-  A(1, 0) = p0_3d.dot(p1_3d_unrotated);
-  A(0, 1) = -A(1, 0);
-  A(1, 1) = -p1_3d_unrotated.dot(p1_3d_unrotated);
-  Eigen::Vector2d lambda = A.inverse() * b;
-  Eigen::Vector3d xm = lambda[0] * p0_3d;
-  Eigen::Vector3d xn = T_0_1.translation() + lambda[1] * p1_3d_unrotated;
-
-  Eigen::Vector4d p0_triangulated;
-  p0_triangulated.head<3>() = (xm + xn) / 2;
-  p0_triangulated[3] = 0;
-
-  return p0_triangulated;
-}
-
 void BundleAdjustmentBase::marginalizeHelper(Eigen::MatrixXd& abs_H,
                                              Eigen::VectorXd& abs_b,
                                              const std::set<int>& idx_to_keep,

src/vi_estimator/keypoint_vio.cpp

@@ -335,20 +335,18 @@ bool KeypointVioEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas,
         Eigen::Vector4d p0_triangulated =
             triangulate(p0_3d.head<3>(), p1_3d.head<3>(), T_0_1);
 
-        if (p0_triangulated[2] > 0) {
+        if (p0_triangulated.array().isFinite().all() &&
+            p0_triangulated[3] > 0 && p0_triangulated[3] < 3.0) {
           KeypointPosition kpt_pos;
           kpt_pos.kf_id = tcidl;
           kpt_pos.dir = StereographicParam<double>::project(p0_triangulated);
-          kpt_pos.id = 1.0 / p0_triangulated.norm();
-
-          if (kpt_pos.id > 0 && kpt_pos.id < 10) {
+          kpt_pos.id = p0_triangulated[3];
           kpts[lm_id] = kpt_pos;
 
           num_points_added++;
           valid_kp = true;
         }
       }
-      }
 
       if (valid_kp) {
         for (const auto& kv_obs : kp_obs) {

src/vi_estimator/nfr_mapper.cpp

@@ -632,12 +632,13 @@ void NfrMapper::setup_opt() {
       Eigen::Vector4d pos_3d = triangulate(
           pos_3d_h.head<3>(), pos_3d_o.head<3>(), T_w_h.inverse() * T_w_o);
 
-      if (pos_3d[2] < 0.5 || pos_3d.norm() < 0.5) continue;
+      if (!pos_3d.array().isFinite().all() || pos_3d[3] <= 0 || pos_3d[3] > 2.0)
+        continue;
 
       KeypointPosition pos;
       pos.kf_id = tcid_h;
       pos.dir = StereographicParam<double>::project(pos_3d);
-      pos.id = 1.0 / pos_3d.norm();
+      pos.id = pos_3d[3];
 
       kpts[kv.first] = pos;