Merge branch 'develop' into wrap/pybind-stl

.github/workflows/build-windows.yml

@@ -21,24 +21,15 @@ jobs:
         # Github Actions requires a single row to be added to the build matrix.
         # See https://help.github.com/en/articles/workflow-syntax-for-github-actions.
         name: [
-            #TODO This build fails, need to understand why.
-            # windows-2016-cl,
             windows-2019-cl,
           ]
 
         build_type: [
             Debug,
-            #TODO(Varun) The release build takes over 2.5 hours, need to figure out why.
+            Release
-            # Release
           ]
         build_unstable: [ON]
         include:
-          #TODO This build fails, need to understand why.
-          # - name: windows-2016-cl
-          #   os: windows-2016
-          #   compiler: cl
-          #   platform: 32
-
           - name: windows-2019-cl
             os: windows-2019
             compiler: cl
@@ -125,4 +116,3 @@ jobs:
 
           # Run GTSAM_UNSTABLE tests
           #cmake --build build -j 4 --config ${{ matrix.build_type }} --target check.base_unstable
-          

gtsam/geometry/Pose2.h

@@ -258,10 +258,19 @@ public:
   inline const Rot2&   r() const { return r_; }
 
   /// translation
-  inline const Point2& translation() const { return t_; }
+  inline const Point2& translation(OptionalJacobian<2, 3> Hself={}) const {
+    if (Hself) {
+      *Hself = Matrix::Zero(2, 3);
+      (*Hself).block<2, 2>(0, 0) = rotation().matrix();
+    }
+    return t_;
+  }
 
   /// rotation
-  inline const Rot2&   rotation() const { return r_; }
+  inline const Rot2&   rotation(OptionalJacobian<1, 3> Hself={}) const {
+    if (Hself) *Hself << 0, 0, 1;
+    return r_;
+  }
 
   //// return transformation matrix
   GTSAM_EXPORT Matrix3 matrix() const;

gtsam/geometry/geometry.i

@@ -434,7 +434,9 @@ class Pose2 {
   gtsam::Rot2 bearing(const gtsam::Point2& point) const;
   double range(const gtsam::Point2& point) const;
   gtsam::Point2 translation() const;
+  gtsam::Point2 translation(Eigen::Ref<Eigen::MatrixXd> Hself) const;
   gtsam::Rot2 rotation() const;
+  gtsam::Rot2 rotation(Eigen::Ref<Eigen::MatrixXd> Hself) const;
   Matrix matrix() const;
 
   // enabling serialization functionality

gtsam/geometry/tests/testPose2.cpp

@@ -474,6 +474,33 @@ TEST( Pose2, compose_matrix )
   EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT
 }
 
+
+
+/* ************************************************************************* */
+TEST( Pose2, translation )  {
+  Pose2 pose(3.5, -8.2, 4.2);
+
+  Matrix actualH;
+  EXPECT(assert_equal((Vector2() << 3.5, -8.2).finished(), pose.translation(actualH), 1e-8));
+
+  std::function<Point2(const Pose2&)> f = [](const Pose2& T) { return T.translation(); };
+  Matrix numericalH = numericalDerivative11<Point2, Pose2>(f, pose);
+  EXPECT(assert_equal(numericalH, actualH, 1e-6));
+}
+
+/* ************************************************************************* */
+TEST( Pose2, rotation ) {
+  Pose2 pose(3.5, -8.2, 4.2);
+
+  Matrix actualH(4, 3);
+  EXPECT(assert_equal(Rot2(4.2), pose.rotation(actualH), 1e-8));
+
+  std::function<Rot2(const Pose2&)> f = [](const Pose2& T) { return T.rotation(); };
+  Matrix numericalH = numericalDerivative11<Rot2, Pose2>(f, pose);
+  EXPECT(assert_equal(numericalH, actualH, 1e-6));
+}
+
+
 /* ************************************************************************* */
 TEST( Pose2, between )
 {

gtsam/geometry/triangulation.cpp

@@ -85,7 +85,8 @@ Vector4 triangulateHomogeneousDLT(
 
 Point3 triangulateLOST(const std::vector<Pose3>& poses,
                        const Point3Vector& calibratedMeasurements,
-                       const SharedIsotropic& measurementNoise) {
+                       const SharedIsotropic& measurementNoise,
+                       double rank_tol) {
   size_t m = calibratedMeasurements.size();
   assert(m == poses.size());
 
@@ -96,17 +97,38 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
   for (size_t i = 0; i < m; i++) {
     const Pose3& wTi = poses[i];
     // TODO(akshay-krishnan): are there better ways to select j?
-    const int j = (i + 1) % m;
+    int j = (i + 1) % m;
     const Pose3& wTj = poses[j];
 
-    const Point3 d_ij = wTj.translation() - wTi.translation();
+    Point3 d_ij = wTj.translation() - wTi.translation();
+    Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]);
+    Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
+    double num_i = wZi.cross(wZj).norm();
+    double den_i = d_ij.cross(wZj).norm();
 
-    const Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]);
+    // Handle q_i = 0 (or NaN), which arises if the measurement vectors, wZi and 
-    const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
+    // wZj, coincide (or the baseline vector coincides with the jth measurement 
+    // vector).
+    if (num_i == 0 || den_i == 0) {
+      bool success = false;
+      for (size_t k = 2; k < m; k++) {
+        j = (i + k) % m;
+        const Pose3& wTj = poses[j];
+
+        d_ij = wTj.translation() - wTi.translation();
+        wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
+        num_i = wZi.cross(wZj).norm();
+        den_i = d_ij.cross(wZj).norm();
+        if (num_i > 0 && den_i > 0) {
+          success = true;
+          break;
+        }
+      }
+      if (!success) throw(TriangulationUnderconstrainedException());
+    }
 
     // Note: Setting q_i = 1.0 gives same results as DLT.
-    const double q_i = wZi.cross(wZj).norm() /
+    const double q_i = num_i / (measurementNoise->sigma() * den_i);
-                       (measurementNoise->sigma() * d_ij.cross(wZj).norm());
 
     const Matrix23 coefficientMat =
         q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
@@ -115,7 +137,13 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
     A.block<2, 3>(2 * i, 0) << coefficientMat;
     b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation();
   }
-  return A.colPivHouseholderQr().solve(b);
+
+  Eigen::ColPivHouseholderQR<Matrix> A_Qr = A.colPivHouseholderQr();
+  A_Qr.setThreshold(rank_tol);
+
+  if (A_Qr.rank() < 3) throw(TriangulationUnderconstrainedException());
+
+  return A_Qr.solve(b);
 }
 
 Point3 triangulateDLT(

gtsam/geometry/triangulation.h

@@ -110,7 +110,8 @@ GTSAM_EXPORT Point3 triangulateDLT(
  */
 GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses,
                                     const Point3Vector& calibratedMeasurements,
-                                    const SharedIsotropic& measurementNoise);
+                                    const SharedIsotropic& measurementNoise,
+                                    double rank_tol = 1e-9);
 
 /**
  * Create a factor graph with projection factors from poses and one calibration
@@ -439,7 +440,8 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
     auto calibratedMeasurements =
         calibrateMeasurementsShared<CALIBRATION>(*sharedCal, measurements);
 
-    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
+    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, 
+                            rank_tol);
   } else {
     // construct projection matrices from poses & calibration
     auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
@@ -512,7 +514,8 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
     auto calibratedMeasurements =
         calibrateMeasurements<CAMERA>(cameras, measurements);
 
-    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
+    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, 
+                            rank_tol);
   } else {
     // construct projection matrices from poses & calibration
     auto projection_matrices = projectionMatricesFromCameras(cameras);
@@ -750,4 +753,3 @@ using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
 using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
 using CameraSetSpherical = CameraSet<SphericalCamera>;
 } // \namespace gtsam
-