From 12b10a358a3842ba709c95e9f1208308431af1d9 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 27 Aug 2021 17:32:42 -0400 Subject: [PATCH] good progress - still need to work on TriangulatePoint3 --- gtsam/geometry/PinholeCamera.h | 5 ++++ gtsam/geometry/PinholePose.h | 4 +++ gtsam/geometry/SphericalCamera.h | 5 ++++ gtsam/geometry/StereoCamera.h | 5 ++++ gtsam/geometry/tests/testTriangulation.cpp | 29 ++++------------------ gtsam/geometry/triangulation.cpp | 7 ++---- gtsam/slam/TriangulationFactor.h | 2 +- 7 files changed, 27 insertions(+), 30 deletions(-) diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 205a14624..ecfbca057 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -318,6 +318,11 @@ public: return K_.K() * P; } + /// for Nonlinear Triangulation + Vector defaultErrorWhenTriangulatingBehindCamera() const { + return Eigen::Matrix::dimension,1>::Constant(2.0 * K_.fx());; + } + private: /** Serialization function */ diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 14196b9b2..5442ded84 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -422,6 +422,10 @@ public: return K_->K() * P; } + /// for Nonlinear Triangulation + Vector defaultErrorWhenTriangulatingBehindCamera() const { + return Eigen::Matrix::dimension,1>::Constant(2.0 * K_->fx());; + } /// @} private: diff --git a/gtsam/geometry/SphericalCamera.h b/gtsam/geometry/SphericalCamera.h index cb354f84b..72d44b94a 100644 --- a/gtsam/geometry/SphericalCamera.h +++ b/gtsam/geometry/SphericalCamera.h @@ -215,6 +215,11 @@ class GTSAM_EXPORT SphericalCamera { return Matrix34(pose_.inverse().matrix().block(0, 0, 3, 4)); } + /// for Nonlinear Triangulation + Vector defaultErrorWhenTriangulatingBehindCamera() const { + return Eigen::Matrix::dimension,1>::Constant(0.0); + } + private: /** Serialization function */ diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 3b5bdaefc..c53fc11c9 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -170,6 +170,11 @@ public: OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 = boost::none) const; + /// for Nonlinear Triangulation + Vector defaultErrorWhenTriangulatingBehindCamera() const { + return Eigen::Matrix::dimension,1>::Constant(2.0 * K_->fx());; + } + /// @} private: diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index dd5a74903..2173d5f12 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -456,36 +456,17 @@ TEST( triangulation, twoPoses_sphericalCamera) { double rank_tol = 1e-9; - // construct projection matrices from poses & calibration + // 1. Test linear triangulation via DLT std::vector> projection_matrices = getCameraProjectionMatrices(cameras); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); EXPECT(assert_equal(landmark, point, 1e-7)); - static const boost::shared_ptr canCal = // - boost::make_shared(1, 1, 0, 0, 0); - PinholeCamera canCam1(pose1, *canCal); - PinholeCamera canCam2(pose2, *canCal); - std::cout << "canCam1.project(landmark);" << canCam1.project(landmark) << std::endl; - std::cout << "canCam2.project(landmark);" << canCam2.project(landmark) << std::endl; + // 2. Test nonlinear triangulation + point = triangulateNonlinear(cameras, measurements, point); + EXPECT(assert_equal(landmark, point, 1e-7)); - CameraSet< PinholeCamera > canCameras; - canCameras.push_back(canCam1); - canCameras.push_back(canCam2); - - Point2Vector can_measurements; - can_measurements.push_back(canCam1.project(landmark)); - can_measurements.push_back(canCam2.project(landmark)); - - // construct projection matrices from poses & calibration - std::vector> can_projection_matrices = - getCameraProjectionMatrices< PinholeCamera >(canCameras); - std::cout <<"can_projection_matrices \n" << can_projection_matrices.at(0) < actual1 = // // triangulatePoint3(cameras, measurements, rank_tol, optimize); diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 9f60916e3..8bb8e4779 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -69,16 +69,13 @@ Point3 triangulateDLT( const std::vector& unit3measurements, double rank_tol) { Point2Vector measurements; - size_t i=0; for (const Unit3& pu : unit3measurements) { // get canonical pixel projection from Unit3 Point3 p = pu.point3(); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION if (p.z() <= 0) // TODO: maybe we should handle this more delicately throw(TriangulationCheiralityException()); +#endif measurements.push_back(Point2(p.x() / p.z(), p.y() / p.z())); - std::cout << "px" << Point2(pu.point3().x() / pu.point3().z(), - pu.point3().y() / pu.point3().z())<< std::endl; - std::cout << "projection_matrices \n "<< projection_matrices.at(i)<< std::endl; - i++; } return triangulateDLT(projection_matrices, measurements, rank_tol); } diff --git a/gtsam/slam/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h index f12053d29..0a15d6861 100644 --- a/gtsam/slam/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -129,7 +129,7 @@ public: << std::endl; if (throwCheirality_) throw e; - return Eigen::Matrix::dimension,1>::Constant(2.0 * camera_.calibration().fx()); + return camera_.defaultErrorWhenTriangulatingBehindCamera(); } }