diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index d9e605d55..bc843ad75 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -108,10 +108,10 @@ TEST(triangulation, twoCamerasUsingLOST) { // 1. Test simple triangulation, perfect in no noise situation boost::optional actual1 = // - triangulatePoint3>( - cameras, measurements, rank_tol, - /*optimize=*/false, measurementNoise, - /*use_lost_triangulation=*/true); + triangulatePoint3>(cameras, measurements, rank_tol, + /*optimize=*/false, + measurementNoise, + /*useLOST=*/true); EXPECT(assert_equal(kLandmark, *actual1, 1e-12)); // 3. Add some noise and try again: result should be ~ (4.995, @@ -145,10 +145,10 @@ TEST(triangulation, twoCamerasLOSTvsDLT) { SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2); boost::optional estimateLOST = // - triangulatePoint3>( - cameras, measurements, rank_tol, - /*optimize=*/false, measurementNoise, - /*use_lost_triangulation=*/true); + triangulatePoint3>(cameras, measurements, rank_tol, + /*optimize=*/false, + measurementNoise, + /*useLOST=*/true); // These values are from a MATLAB implementation. EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimateLOST, 1e-3)); diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 4a8bb279d..acb0ddf0e 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -107,20 +107,19 @@ Point3 triangulateLOST(const std::vector& poses, const Point3 d_ij = wTj.translation() - wTi.translation(); - const Point3 w_measurement_i = - wTi.rotation().rotate(calibratedMeasurements[i]); - const Point3 w_measurement_j = - wTj.rotation().rotate(calibratedMeasurements[j]); + const Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]); + const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]); - const double q_i = - w_measurement_i.cross(w_measurement_j).norm() / - (measurementNoise->sigma() * d_ij.cross(w_measurement_j).norm()); - const Matrix23 coefficient_mat = + // 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 Matrix23 coefficientMat = q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * wTi.rotation().matrix().transpose(); - A.block<2, 3>(2 * i, 0) << coefficient_mat; - b.block<2, 1>(2 * i, 0) << coefficient_mat * wTi.translation(); + A.block<2, 3>(2 * i, 0) << coefficientMat; + b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation(); } return A.colPivHouseholderQr().solve(b); } diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 1b0970fe2..aa9433484 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -415,8 +415,7 @@ inline Point3Vector calibrateMeasurements( * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation - * @param use_lost_triangulation whether to use the LOST algorithm instead of - * DLT + * @param useLOST whether to use the LOST algorithm instead of DLT * @return Returns a Point3 */ template @@ -425,13 +424,13 @@ Point3 triangulatePoint3(const std::vector& poses, const Point2Vector& measurements, double rank_tol = 1e-9, bool optimize = false, const SharedNoiseModel& model = nullptr, - const bool use_lost_triangulation = false) { + const bool useLOST = false) { assert(poses.size() == measurements.size()); if (poses.size() < 2) throw(TriangulationUnderconstrainedException()); // Triangulate linearly Point3 point; - if (use_lost_triangulation) { + if (useLOST) { // Reduce input noise model to an isotropic noise model using the mean of // the diagonal. const double measurementSigma = model ? model->sigmas().mean() : 1e-4; @@ -481,7 +480,7 @@ Point3 triangulatePoint3(const std::vector& poses, * @param measurements A vector of camera measurements * @param rank_tol rank tolerance, default 1e-9 * @param optimize Flag to turn on nonlinear refinement of triangulation - * @param use_lost_triangulation whether to use the LOST algorithm instead of + * @param useLOST whether to use the LOST algorithm instead of * DLT * @return Returns a Point3 */ @@ -490,7 +489,7 @@ Point3 triangulatePoint3(const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9, bool optimize = false, const SharedNoiseModel& model = nullptr, - const bool use_lost_triangulation = false) { + const bool useLOST = false) { size_t m = cameras.size(); assert(measurements.size() == m); @@ -498,7 +497,7 @@ Point3 triangulatePoint3(const CameraSet& cameras, // Triangulate linearly Point3 point; - if (use_lost_triangulation) { + if (useLOST) { // Reduce input noise model to an isotropic noise model using the mean of // the diagonal. const double measurementSigma = model ? model->sigmas().mean() : 1e-4; @@ -545,15 +544,14 @@ Point3 triangulatePoint3(const CameraSet& cameras, } /// Pinhole-specific version -template -Point3 triangulatePoint3( - const CameraSet >& cameras, - const Point2Vector& measurements, double rank_tol = 1e-9, - bool optimize = false, - const SharedNoiseModel& model = nullptr, - const bool use_lost_triangulation = false) { - return triangulatePoint3 > // - (cameras, measurements, rank_tol, optimize, model, use_lost_triangulation); +template +Point3 triangulatePoint3(const CameraSet>& cameras, + const Point2Vector& measurements, + double rank_tol = 1e-9, bool optimize = false, + const SharedNoiseModel& model = nullptr, + const bool useLOST = false) { + return triangulatePoint3> // + (cameras, measurements, rank_tol, optimize, model, useLOST); } struct GTSAM_EXPORT TriangulationParameters {