diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 3d4bf3faf..0434577c1 100644 --- a/.github/workflows/build-windows.yml +++ b/.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 - diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index c3f588dcc..f1b38c5a6 100644 --- a/gtsam/geometry/Pose2.h +++ b/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; diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 4d265fca2..60d6a2bed 100644 --- a/gtsam/geometry/geometry.i +++ b/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 Hself) const; gtsam::Rot2 rotation() const; + gtsam::Rot2 rotation(Eigen::Ref Hself) const; Matrix matrix() const; // enabling serialization functionality diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 8c1bdccc0..b52a6e590 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/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 f = [](const Pose2& T) { return T.translation(); }; + Matrix numericalH = numericalDerivative11(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 f = [](const Pose2& T) { return T.rotation(); }; + Matrix numericalH = numericalDerivative11(f, pose); + EXPECT(assert_equal(numericalH, actualH, 1e-6)); +} + + /* ************************************************************************* */ TEST( Pose2, between ) { diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 06fb8fafe..039dfd74d 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -85,7 +85,8 @@ Vector4 triangulateHomogeneousDLT( Point3 triangulateLOST(const std::vector& 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& 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]); - const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]); + // Handle q_i = 0 (or NaN), which arises if the measurement vectors, wZi and + // 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() / - (measurementNoise->sigma() * d_ij.cross(wZj).norm()); + const double q_i = num_i / (measurementNoise->sigma() * den_i); const Matrix23 coefficientMat = q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * @@ -115,7 +137,13 @@ Point3 triangulateLOST(const std::vector& 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 A_Qr = A.colPivHouseholderQr(); + A_Qr.setThreshold(rank_tol); + + if (A_Qr.rank() < 3) throw(TriangulationUnderconstrainedException()); + + return A_Qr.solve(b); } Point3 triangulateDLT( diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 7e58cee2d..3a8398804 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -110,7 +110,8 @@ GTSAM_EXPORT Point3 triangulateDLT( */ GTSAM_EXPORT Point3 triangulateLOST(const std::vector& 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& poses, auto calibratedMeasurements = calibrateMeasurementsShared(*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& cameras, auto calibratedMeasurements = calibrateMeasurements(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>; using CameraSetCal3Unified = CameraSet>; using CameraSetSpherical = CameraSet; } // \namespace gtsam -