From d2f0cf5cc7149a2ee096021b6ea1288d47245348 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Jan 2023 11:02:26 -0800 Subject: [PATCH] Inherit constructors for CameraSets to switch to initializer lists. --- gtsam/geometry/CameraSet.h | 80 +++++----- gtsam/geometry/tests/testTriangulation.cpp | 165 ++++++++------------- 2 files changed, 99 insertions(+), 146 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 143d4bc3c..0fbb50f02 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -35,56 +35,56 @@ template class CameraSet : public std::vector > { protected: + using Base = std::vector>; - /** - * 2D measurement and noise model for each of the m views - * The order is kept the same as the keys that we use to create the factor. - */ - typedef typename CAMERA::Measurement Z; - typedef typename CAMERA::MeasurementVector ZVector; + /** + * 2D measurement and noise model for each of the m views + * The order is kept the same as the keys that we use to create the factor. + */ + typedef typename CAMERA::Measurement Z; + typedef typename CAMERA::MeasurementVector ZVector; - static const int D = traits::dimension; ///< Camera dimension - static const int ZDim = traits::dimension; ///< Measurement dimension + static const int D = traits::dimension; ///< Camera dimension + static const int ZDim = traits::dimension; ///< Measurement dimension - /// Make a vector of re-projection errors - static Vector ErrorVector(const ZVector& predicted, - const ZVector& measured) { + /// Make a vector of re-projection errors + static Vector ErrorVector(const ZVector& predicted, const ZVector& measured) { + // Check size + size_t m = predicted.size(); + if (measured.size() != m) + throw std::runtime_error("CameraSet::errors: size mismatch"); - // Check size - size_t m = predicted.size(); - if (measured.size() != m) - throw std::runtime_error("CameraSet::errors: size mismatch"); - - // Project and fill error vector - Vector b(ZDim * m); - for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { - Vector bi = traits::Local(measured[i], predicted[i]); - if(ZDim==3 && std::isnan(bi(1))){ // if it is a stereo point and the right pixel is missing (nan) - bi(1) = 0; - } - b.segment(row) = bi; - } - return b; + // Project and fill error vector + Vector b(ZDim * m); + for (size_t i = 0, row = 0; i < m; i++, row += ZDim) { + Vector bi = traits::Local(measured[i], predicted[i]); + if (ZDim == 3 && std::isnan(bi(1))) { // if it is a stereo point and the + // right pixel is missing (nan) + bi(1) = 0; + } + b.segment(row) = bi; + } + return b; } public: + using Base::Base; // Inherit the vector constructors - /// Destructor - virtual ~CameraSet() = default; + /// Destructor + virtual ~CameraSet() = default; - /// Definitions for blocks of F - using MatrixZD = Eigen::Matrix; - using FBlocks = std::vector>; + /// Definitions for blocks of F + using MatrixZD = Eigen::Matrix; + using FBlocks = std::vector>; - /** - * print - * @param s optional string naming the factor - * @param keyFormatter optional formatter useful for printing Symbols - */ - virtual void print(const std::string& s = "") const { - std::cout << s << "CameraSet, cameras = \n"; - for (size_t k = 0; k < this->size(); ++k) - this->at(k).print(s); + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + virtual void print(const std::string& s = "") const { + std::cout << s << "CameraSet, cameras = \n"; + for (size_t k = 0; k < this->size(); ++k) this->at(k).print(s); } /// equals diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index bc843ad75..95397d5b3 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -30,12 +30,8 @@ #include #include -#include -#include - using namespace std; using namespace gtsam; -using namespace boost::assign; // Some common constants @@ -51,34 +47,34 @@ static const PinholeCamera kCamera1(kPose1, *kSharedCal); static const Pose3 kPose2 = kPose1 * Pose3(Rot3(), Point3(1, 0, 0)); static const PinholeCamera kCamera2(kPose2, *kSharedCal); -// landmark ~5 meters infront of camera +static const std::vector kPoses = {kPose1, kPose2}; + + +// landmark ~5 meters in front of camera static const Point3 kLandmark(5, 0.5, 1.2); // 1. Project two landmarks into two cameras and triangulate static const Point2 kZ1 = kCamera1.project(kLandmark); static const Point2 kZ2 = kCamera2.project(kLandmark); +static const Point2Vector kMeasurements{kZ1, kZ2}; //****************************************************************************** // Simple test with a well-behaved two camera situation TEST(triangulation, twoPoses) { - vector poses; - Point2Vector measurements; - - poses += kPose1, kPose2; - measurements += kZ1, kZ2; + Point2Vector measurements = kMeasurements; double rank_tol = 1e-9; // 1. Test simple DLT, perfect in no noise situation bool optimize = false; boost::optional actual1 = // - triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); + triangulatePoint3(kPoses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; boost::optional actual2 = // - triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); + triangulatePoint3(kPoses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); // 3. Add some noise and try again: result should be ~ (4.995, @@ -87,13 +83,13 @@ TEST(triangulation, twoPoses) { measurements.at(1) += Point2(-0.2, 0.3); optimize = false; boost::optional actual3 = // - triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); + triangulatePoint3(kPoses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4)); // 4. Now with optimization on optimize = true; boost::optional actual4 = // - triangulatePoint3(poses, kSharedCal, measurements, rank_tol, optimize); + triangulatePoint3(kPoses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } @@ -102,7 +98,7 @@ TEST(triangulation, twoCamerasUsingLOST) { cameras.push_back(kCamera1); cameras.push_back(kCamera2); - Point2Vector measurements = {kZ1, kZ2}; + Point2Vector measurements = kMeasurements; SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-4); double rank_tol = 1e-9; @@ -175,25 +171,21 @@ TEST(triangulation, twoPosesCal3DS2) { Point2 z1Distorted = camera1Distorted.project(kLandmark); Point2 z2Distorted = camera2Distorted.project(kLandmark); - vector poses; - Point2Vector measurements; - - poses += kPose1, kPose2; - measurements += z1Distorted, z2Distorted; + Point2Vector measurements{z1Distorted, z2Distorted}; double rank_tol = 1e-9; // 1. Test simple DLT, perfect in no noise situation bool optimize = false; boost::optional actual1 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, + triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; boost::optional actual2 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, + triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); @@ -203,14 +195,14 @@ TEST(triangulation, twoPosesCal3DS2) { measurements.at(1) += Point2(-0.2, 0.3); optimize = false; boost::optional actual3 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, + triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); // 4. Now with optimization on optimize = true; boost::optional actual4 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, + triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); } @@ -232,25 +224,21 @@ TEST(triangulation, twoPosesFisheye) { Point2 z1Distorted = camera1Distorted.project(kLandmark); Point2 z2Distorted = camera2Distorted.project(kLandmark); - vector poses; - Point2Vector measurements; - - poses += kPose1, kPose2; - measurements += z1Distorted, z2Distorted; + Point2Vector measurements{z1Distorted, z2Distorted}; double rank_tol = 1e-9; // 1. Test simple DLT, perfect in no noise situation bool optimize = false; boost::optional actual1 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, + triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; boost::optional actual2 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, + triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); @@ -260,14 +248,14 @@ TEST(triangulation, twoPosesFisheye) { measurements.at(1) += Point2(-0.2, 0.3); optimize = false; boost::optional actual3 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, + triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); // 4. Now with optimization on optimize = true; boost::optional actual4 = // - triangulatePoint3(poses, sharedDistortedCal, measurements, + triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); } @@ -284,17 +272,13 @@ TEST(triangulation, twoPosesBundler) { Point2 z1 = camera1.project(kLandmark); Point2 z2 = camera2.project(kLandmark); - vector poses; - Point2Vector measurements; - - poses += kPose1, kPose2; - measurements += z1, z2; + Point2Vector measurements{z1, z2}; bool optimize = true; double rank_tol = 1e-9; boost::optional actual = // - triangulatePoint3(poses, bundlerCal, measurements, rank_tol, + triangulatePoint3(kPoses, bundlerCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual, 1e-7)); @@ -303,19 +287,15 @@ TEST(triangulation, twoPosesBundler) { measurements.at(1) += Point2(-0.2, 0.3); boost::optional actual2 = // - triangulatePoint3(poses, bundlerCal, measurements, rank_tol, + triangulatePoint3(kPoses, bundlerCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-3)); } //****************************************************************************** TEST(triangulation, fourPoses) { - vector poses; - Point2Vector measurements; - - poses += kPose1, kPose2; - measurements += kZ1, kZ2; - + Pose3Vector poses = kPoses; + Point2Vector measurements = kMeasurements; boost::optional actual = triangulatePoint3(poses, kSharedCal, measurements); EXPECT(assert_equal(kLandmark, *actual, 1e-2)); @@ -334,8 +314,8 @@ TEST(triangulation, fourPoses) { PinholeCamera camera3(pose3, *kSharedCal); Point2 z3 = camera3.project(kLandmark); - poses += pose3; - measurements += z3 + Point2(0.1, -0.1); + poses.push_back(pose3); + measurements.push_back(z3 + Point2(0.1, -0.1)); boost::optional triangulated_3cameras = // triangulatePoint3(poses, kSharedCal, measurements); @@ -353,8 +333,8 @@ TEST(triangulation, fourPoses) { #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException); - poses += pose4; - measurements += Point2(400, 400); + poses.push_back(pose4); + measurements.emplace_back(400, 400); CHECK_EXCEPTION(triangulatePoint3(poses, kSharedCal, measurements), TriangulationCheiralityException); @@ -368,10 +348,8 @@ TEST(triangulation, threePoses_robustNoiseModel) { PinholeCamera camera3(pose3, *kSharedCal); Point2 z3 = camera3.project(kLandmark); - vector poses; - Point2Vector measurements; - poses += kPose1, kPose2, pose3; - measurements += kZ1, kZ2, z3; + const vector poses{kPose1, kPose2, pose3}; + Point2Vector measurements{kZ1, kZ2, z3}; // noise free, so should give exactly the landmark boost::optional actual = @@ -410,10 +388,9 @@ TEST(triangulation, fourPoses_robustNoiseModel) { PinholeCamera camera3(pose3, *kSharedCal); Point2 z3 = camera3.project(kLandmark); - vector poses; - Point2Vector measurements; - poses += kPose1, kPose1, kPose2, pose3; // 2 measurements from pose 1 - measurements += kZ1, kZ1, kZ2, z3; + const vector poses{kPose1, kPose1, kPose2, pose3}; + // 2 measurements from pose 1: + Point2Vector measurements{kZ1, kZ1, kZ2, z3}; // noise free, so should give exactly the landmark boost::optional actual = @@ -463,11 +440,8 @@ TEST(triangulation, fourPoses_distinct_Ks) { Point2 z1 = camera1.project(kLandmark); Point2 z2 = camera2.project(kLandmark); - CameraSet> cameras; - Point2Vector measurements; - - cameras += camera1, camera2; - measurements += z1, z2; + CameraSet> cameras{camera1, camera2}; + Point2Vector measurements{z1, z2}; boost::optional actual = // triangulatePoint3(cameras, measurements); @@ -488,8 +462,8 @@ TEST(triangulation, fourPoses_distinct_Ks) { PinholeCamera camera3(pose3, K3); Point2 z3 = camera3.project(kLandmark); - cameras += camera3; - measurements += z3 + Point2(0.1, -0.1); + cameras.push_back(camera3); + measurements.push_back(z3 + Point2(0.1, -0.1)); boost::optional triangulated_3cameras = // triangulatePoint3(cameras, measurements); @@ -508,8 +482,8 @@ TEST(triangulation, fourPoses_distinct_Ks) { #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION CHECK_EXCEPTION(camera4.project(kLandmark), CheiralityException); - cameras += camera4; - measurements += Point2(400, 400); + cameras.push_back(camera4); + measurements.emplace_back(400, 400); CHECK_EXCEPTION(triangulatePoint3(cameras, measurements), TriangulationCheiralityException); #endif @@ -529,11 +503,8 @@ TEST(triangulation, fourPoses_distinct_Ks_distortion) { Point2 z1 = camera1.project(kLandmark); Point2 z2 = camera2.project(kLandmark); - CameraSet> cameras; - Point2Vector measurements; - - cameras += camera1, camera2; - measurements += z1, z2; + const CameraSet> cameras{camera1, camera2}; + const Point2Vector measurements{z1, z2}; boost::optional actual = // triangulatePoint3(cameras, measurements); @@ -554,11 +525,8 @@ TEST(triangulation, outliersAndFarLandmarks) { Point2 z1 = camera1.project(kLandmark); Point2 z2 = camera2.project(kLandmark); - CameraSet> cameras; - Point2Vector measurements; - - cameras += camera1, camera2; - measurements += z1, z2; + CameraSet> cameras{camera1, camera2}; + Point2Vector measurements{z1, z2}; double landmarkDistanceThreshold = 10; // landmark is closer than that TriangulationParameters params( @@ -582,8 +550,8 @@ TEST(triangulation, outliersAndFarLandmarks) { PinholeCamera camera3(pose3, K3); Point2 z3 = camera3.project(kLandmark); - cameras += camera3; - measurements += z3 + Point2(10, -10); + cameras.push_back(camera3); + measurements.push_back(z3 + Point2(10, -10)); landmarkDistanceThreshold = 10; // landmark is closer than that double outlierThreshold = 100; // loose, the outlier is going to pass @@ -608,11 +576,8 @@ TEST(triangulation, twoIdenticalPoses) { // 1. Project two landmarks into two cameras and triangulate Point2 z1 = camera1.project(kLandmark); - vector poses; - Point2Vector measurements; - - poses += kPose1, kPose1; - measurements += z1, z1; + const vector poses{kPose1, kPose1}; + const Point2Vector measurements{z1, z1}; CHECK_EXCEPTION(triangulatePoint3(poses, kSharedCal, measurements), TriangulationUnderconstrainedException); @@ -623,22 +588,19 @@ TEST(triangulation, onePose) { // we expect this test to fail with a TriangulationUnderconstrainedException // because there's only one camera observation - vector poses; - Point2Vector measurements; - - poses += Pose3(); - measurements += Point2(0, 0); + const vector poses{Pose3()}; + const Point2Vector measurements {{0,0}}; CHECK_EXCEPTION(triangulatePoint3(poses, kSharedCal, measurements), TriangulationUnderconstrainedException); } //****************************************************************************** -TEST(triangulation, StereotriangulateNonlinear) { +TEST(triangulation, StereoTriangulateNonlinear) { auto stereoK = boost::make_shared(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612); - // two camera poses m1, m2 + // two camera kPoses m1, m2 Matrix4 m1, m2; m1 << 0.796888717, 0.603404026, -0.0295271487, 46.6673779, 0.592783835, -0.77156583, 0.230856632, 66.2186159, 0.116517574, -0.201470143, @@ -648,14 +610,12 @@ TEST(triangulation, StereotriangulateNonlinear) { 0.947083213, 0.131587097, 65.843136, -0.0206094928, 0.131334858, -0.991123524, -4.3525033, 0, 0, 0, 1; - typedef CameraSet Cameras; - Cameras cameras; - cameras.push_back(StereoCamera(Pose3(m1), stereoK)); - cameras.push_back(StereoCamera(Pose3(m2), stereoK)); + typedef CameraSet StereoCameras; + const StereoCameras cameras{{Pose3(m1), stereoK}, {Pose3(m2), stereoK}}; StereoPoint2Vector measurements; - measurements += StereoPoint2(226.936, 175.212, 424.469); - measurements += StereoPoint2(339.571, 285.547, 669.973); + measurements.push_back(StereoPoint2(226.936, 175.212, 424.469)); + measurements.push_back(StereoPoint2(339.571, 285.547, 669.973)); Point3 initial = Point3(46.0536958, 66.4621179, -6.56285929); // error: 96.5715555191 @@ -741,8 +701,6 @@ TEST(triangulation, StereotriangulateNonlinear) { //****************************************************************************** // Simple test with a well-behaved two camera situation TEST(triangulation, twoPoses_sphericalCamera) { - vector poses; - std::vector measurements; // Project landmark into two cameras and triangulate SphericalCamera cam1(kPose1); @@ -750,8 +708,7 @@ TEST(triangulation, twoPoses_sphericalCamera) { Unit3 u1 = cam1.project(kLandmark); Unit3 u2 = cam2.project(kLandmark); - poses += kPose1, kPose2; - measurements += u1, u2; + std::vector measurements{u1, u2}; CameraSet cameras; cameras.push_back(cam1); @@ -803,9 +760,6 @@ TEST(triangulation, twoPoses_sphericalCamera) { //****************************************************************************** TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) { - vector poses; - std::vector measurements; - // Project landmark into two cameras and triangulate Pose3 poseA = Pose3( Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), @@ -825,8 +779,7 @@ TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) { EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, -1.0)), u2, 1e-7)); // behind and to the right of PoseB - poses += kPose1, kPose2; - measurements += u1, u2; + const std::vector measurements{u1, u2}; CameraSet cameras; cameras.push_back(cam1);