From c4cd2b5080d2a20bb2c77037821def2a4d21b157 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 6 Nov 2021 18:05:58 -0400 Subject: [PATCH] fixed formatting (plus small fix: std::vector -> fastVector) --- .../tests/testSmartProjectionRigFactor.cpp | 525 ++++++++++-------- ...martProjectionPoseFactorRollingShutter.cpp | 121 ++-- 2 files changed, 350 insertions(+), 296 deletions(-) diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index a2a30e89a..8e6735dbd 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -19,15 +19,17 @@ * @date August 2021 */ -#include "smartFactorScenarios.h" -#include -#include +#include #include #include -#include +#include +#include + #include #include +#include "smartFactorScenarios.h" + using namespace boost::assign; using namespace std::placeholders; @@ -37,15 +39,15 @@ static const double sigma = 0.1; static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); // Convenience for named keys -using symbol_shorthand::X; using symbol_shorthand::L; +using symbol_shorthand::X; // tests data static Symbol x1('X', 1); static Symbol x2('X', 2); static Symbol x3('X', 3); -Key cameraId1 = 0; // first camera +Key cameraId1 = 0; // first camera Key cameraId2 = 1; Key cameraId3 = 2; @@ -56,15 +58,15 @@ LevenbergMarquardtParams lmParams; // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor) { +TEST(SmartProjectionRigFactor, Constructor) { using namespace vanillaPose; Cameras cameraRig; - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); } /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor2) { +TEST(SmartProjectionRigFactor, Constructor2) { using namespace vanillaPose; Cameras cameraRig; SmartProjectionParams params; @@ -73,19 +75,19 @@ TEST( SmartProjectionRigFactor, Constructor2) { } /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor3) { +TEST(SmartProjectionRigFactor, Constructor3) { using namespace vanillaPose; Cameras cameraRig; - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); factor1->add(measurement1, x1, cameraId1); } /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor4) { +TEST(SmartProjectionRigFactor, Constructor4) { using namespace vanillaPose; Cameras cameraRig; - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); SmartProjectionParams params; params.setRankTolerance(rankTol); SmartRigFactor factor1(model, cameraRig, params); @@ -93,7 +95,7 @@ TEST( SmartProjectionRigFactor, Constructor4) { } /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor5) { +TEST(SmartProjectionRigFactor, Constructor5) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); @@ -102,10 +104,10 @@ TEST( SmartProjectionRigFactor, Constructor5) { } /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Equals ) { +TEST(SmartProjectionRigFactor, Equals) { using namespace vanillaPose; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); factor1->add(measurement1, x1, cameraId1); @@ -115,23 +117,23 @@ TEST( SmartProjectionRigFactor, Equals ) { CHECK(assert_equal(*factor1, *factor2)); - SmartRigFactor::shared_ptr factor3(new SmartRigFactor(model, Camera(Pose3::identity(), sharedK))); - factor3->add(measurement1, x1); // now use default + SmartRigFactor::shared_ptr factor3( + new SmartRigFactor(model, Camera(Pose3::identity(), sharedK))); + factor3->add(measurement1, x1); // now use default CHECK(assert_equal(*factor1, *factor3)); } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, noiseless ) { - +TEST(SmartProjectionRigFactor, noiseless) { using namespace vanillaPose; // Project two landmarks into two cameras Point2 level_uv = level_camera.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1); - SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK) ); - factor.add(level_uv, x1); // both taken from the same camera + SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK)); + factor.add(level_uv, x1); // both taken from the same camera factor.add(level_uv_right, x2); Values values; // it's a pose factor, hence these are poses @@ -181,12 +183,11 @@ TEST( SmartProjectionRigFactor, noiseless ) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, noisy ) { - +TEST(SmartProjectionRigFactor, noisy) { using namespace vanillaPose; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); // Project two landmarks into two cameras Point2 pixelError(0.2, 0.2); @@ -195,25 +196,25 @@ TEST( SmartProjectionRigFactor, noisy ) { Values values; values.insert(x1, cam1.pose()); - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), - Point3(0.5, 0.1, 0.3)); + Pose3 noise_pose = + Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); values.insert(x2, pose_right.compose(noise_pose)); - SmartRigFactor::shared_ptr factor(new SmartRigFactor(model,cameraRig)); + SmartRigFactor::shared_ptr factor(new SmartRigFactor(model, cameraRig)); factor->add(level_uv, x1, cameraId1); factor->add(level_uv_right, x2, cameraId1); double actualError1 = factor->error(values); // create other factor by passing multiple measurements - SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model,cameraRig)); + SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model, cameraRig)); Point2Vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - KeyVector views { x1, x2 }; - FastVector cameraIds { 0, 0 }; + KeyVector views{x1, x2}; + FastVector cameraIds{0, 0}; factor2->add(measurements, views, cameraIds); double actualError2 = factor2->error(values); @@ -225,10 +226,10 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { using namespace vanillaPose; // create arbitrary body_T_sensor (transforms from sensor to body) - Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), - Point3(1, 1, 1)); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(body_T_sensor, sharedK) ); + Pose3 body_T_sensor = + Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(body_T_sensor, sharedK)); // These are the poses we want to estimate, from camera measurements const Pose3 sensor_T_body = body_T_sensor.inverse(); @@ -247,8 +248,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // Create smart factors - KeyVector views { x1, x2, x3 }; - FastVector cameraIds { 0, 0, 0 }; + KeyVector views{x1, x2, x3}; + FastVector cameraIds{0, 0, 0}; SmartProjectionParams params; params.setRankTolerance(1.0); @@ -256,7 +257,9 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { params.setEnableEPI(false); SmartRigFactor smartFactor1(model, cameraRig, params); - smartFactor1.add(measurements_cam1, views); // use default CameraIds since we have a single camera + smartFactor1.add( + measurements_cam1, + views); // use default CameraIds since we have a single camera SmartRigFactor smartFactor2(model, cameraRig, params); smartFactor2.add(measurements_cam2, views); @@ -283,8 +286,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { double expectedError = 0.0; DOUBLES_EQUAL(expectedError, actualError, 1e-7) - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); + Pose3 noise_pose = + Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); Values values; values.insert(x1, wTb1); values.insert(x2, wTb2); @@ -304,16 +307,16 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { using namespace vanillaPose; // create arbitrary body_T_sensor (transforms from sensor to body) - Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), - Point3(1, 1, 1)); - Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), - Point3(0, 0, 1)); + Pose3 body_T_sensor1 = + Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); + Pose3 body_T_sensor2 = + Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Point3(0, 0, 1)); Pose3 body_T_sensor3 = Pose3::identity(); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(body_T_sensor1, sharedK) ); - cameraRig.push_back( Camera(body_T_sensor2, sharedK) ); - cameraRig.push_back( Camera(body_T_sensor3, sharedK) ); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(body_T_sensor1, sharedK)); + cameraRig.push_back(Camera(body_T_sensor2, sharedK)); + cameraRig.push_back(Camera(body_T_sensor3, sharedK)); // These are the poses we want to estimate, from camera measurements const Pose3 sensor_T_body1 = body_T_sensor1.inverse(); @@ -334,8 +337,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // Create smart factors - KeyVector views { x1, x2, x3 }; - FastVector cameraIds { 0, 1, 2 }; + KeyVector views{x1, x2, x3}; + FastVector cameraIds{0, 1, 2}; SmartProjectionParams params; params.setRankTolerance(1.0); @@ -370,8 +373,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { double expectedError = 0.0; DOUBLES_EQUAL(expectedError, actualError, 1e-7) - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); + Pose3 noise_pose = + Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); Values values; values.insert(x1, wTb1); values.insert(x2, wTb2); @@ -387,29 +390,29 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { - +TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) { using namespace vanillaPose2; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK2) ); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK2)); // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - KeyVector views {x1,x2,x3}; - FastVector cameraIds{0,0,0};// 3 measurements from the same camera in the rig + KeyVector views{x1, x2, x3}; + FastVector cameraIds{ + 0, 0, 0}; // 3 measurements from the same camera in the rig - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model,cameraRig)); + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model,cameraRig)); + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model,cameraRig)); + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -427,21 +430,21 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { groundTruth.insert(x3, cam3.pose()); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), - values.at(x3))); + EXPECT(assert_equal( + Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); @@ -450,15 +453,14 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, Factors ) { - +TEST(SmartProjectionRigFactor, Factors) { using namespace vanillaPose; // Default cameras for simple derivatives Rot3 R; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); - Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( - Pose3(R, Point3(1, 0, 0)), sharedK); + Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), + cam2(Pose3(R, Point3(1, 0, 0)), sharedK); // one landmarks 1m in front of camera Point3 landmark1(0, 0, 10); @@ -470,12 +472,13 @@ TEST( SmartProjectionRigFactor, Factors ) { measurements_cam1.push_back(cam2.project(landmark1)); // Create smart factors - KeyVector views { x1, x2 }; - FastVector cameraIds { 0, 0 }; + KeyVector views{x1, x2}; + FastVector cameraIds{0, 0}; - SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared < SmartRigFactor - > (model, Camera(Pose3::identity(), sharedK)); - smartFactor1->add(measurements_cam1, views); // we have a single camera so use default cameraIds + SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared( + model, Camera(Pose3::identity(), sharedK)); + smartFactor1->add(measurements_cam1, + views); // we have a single camera so use default cameraIds SmartRigFactor::Cameras cameras; cameras.push_back(cam1); @@ -501,7 +504,8 @@ TEST( SmartProjectionRigFactor, Factors ) { perturbedDelta.insert(x2, delta); double expectedError = 2500; - // After eliminating the point, A1 and A2 contain 2-rank information on cameras: + // After eliminating the point, A1 and A2 contain 2-rank information on + // cameras: Matrix16 A1, A2; A1 << -10, 0, 0, 0, 1, 0; A2 << 10, 0, 1, 0, -1, 0; @@ -528,8 +532,8 @@ TEST( SmartProjectionRigFactor, Factors ) { values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); - boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 - ->createHessianFactor(values, 0.0); + boost::shared_ptr> actual = + smartFactor1->createHessianFactor(values, 0.0); EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); EXPECT(assert_equal(expected, *actual, 1e-6)); EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); @@ -538,11 +542,10 @@ TEST( SmartProjectionRigFactor, Factors ) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { - +TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { using namespace vanillaPose; - KeyVector views { x1, x2, x3 }; + KeyVector views{x1, x2, x3}; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -551,15 +554,15 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; + std::vector> sharedKs; sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); // create smart factor - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - FastVector cameraIds { 0, 0, 0}; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); @@ -578,22 +581,21 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { graph.addPrior(x1, cam1.pose(), noisePrior); graph.addPrior(x2, cam2.pose(), noisePrior); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656, - -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, - -0.0313952598), - Point3(0.1, -0.1, 1.9)), - values.at(x3))); + EXPECT(assert_equal(Pose3(Rot3(1.11022302e-16, -0.0314107591, 0.99950656, + -0.99950656, -0.0313952598, -0.000986635786, + 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); @@ -602,13 +604,12 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, landmarkDistance ) { - +TEST(SmartProjectionRigFactor, landmarkDistance) { using namespace vanillaPose; double excludeLandmarksFutherThanDist = 2; - KeyVector views { x1, x2, x3 }; + KeyVector views{x1, x2, x3}; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -624,17 +625,20 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - FastVector cameraIds { 0, 0, 0}; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + FastVector cameraIds{0, 0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -646,7 +650,8 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) { graph.addPrior(x1, cam1.pose(), noisePrior); graph.addPrior(x2, cam2.pose(), noisePrior); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; @@ -662,14 +667,14 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { - +TEST(SmartProjectionRigFactor, dynamicOutlierRejection) { using namespace vanillaPose; double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + double dynamicOutlierRejectionThreshold = + 1; // max 1 pixel of average reprojection error - KeyVector views { x1, x2, x3 }; + KeyVector views{x1, x2, x3}; // add fourth landmark Point3 landmark4(5, -0.5, 1); @@ -682,7 +687,8 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); - measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier + measurements_cam4.at(0) = + measurements_cam4.at(0) + Point2(10, 10); // add outlier SmartProjectionParams params; params.setLinearizationMode(gtsam::HESSIAN); @@ -690,20 +696,24 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - FastVector cameraIds { 0, 0, 0}; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + FastVector cameraIds{0, 0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor4(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor4( + new SmartRigFactor(model, cameraRig, params)); smartFactor4->add(measurements_cam4, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -729,15 +739,14 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, CheckHessian) { - - KeyVector views { x1, x2, x3 }; +TEST(SmartProjectionRigFactor, CheckHessian) { + KeyVector views{x1, x2, x3}; using namespace vanillaPose; // Two slightly different cameras - Pose3 pose2 = level_pose - * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose2 = + level_pose * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Camera cam2(pose2, sharedK); Camera cam3(pose3, sharedK); @@ -753,17 +762,20 @@ TEST( SmartProjectionRigFactor, CheckHessian) { params.setRankTolerance(10); params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - FastVector cameraIds { 0, 0, 0}; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + FastVector cameraIds{0, 0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default smartFactor3->add(measurements_cam3, views, cameraIds); NonlinearFactorGraph graph; @@ -771,53 +783,53 @@ TEST( SmartProjectionRigFactor, CheckHessian) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose3 * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, - -0.130426831, -0.0115837907, 0.130819108, -0.98278564, - -0.130455917), - Point3(0.0897734171, -0.110201006, 0.901022872)), - values.at(x3))); + EXPECT(assert_equal(Pose3(Rot3(0.00563056869, -0.130848107, 0.991386438, + -0.991390265, -0.130426831, -0.0115837907, + 0.130819108, -0.98278564, -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); boost::shared_ptr factor1 = smartFactor1->linearize(values); boost::shared_ptr factor2 = smartFactor2->linearize(values); boost::shared_ptr factor3 = smartFactor3->linearize(values); - Matrix CumulativeInformation = factor1->information() + factor2->information() - + factor3->information(); + Matrix CumulativeInformation = + factor1->information() + factor2->information() + factor3->information(); - boost::shared_ptr GaussianGraph = graph.linearize( - values); + boost::shared_ptr GaussianGraph = + graph.linearize(values); Matrix GraphInformation = GaussianGraph->hessian().first; // Check Hessian EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6)); - Matrix AugInformationMatrix = factor1->augmentedInformation() - + factor2->augmentedInformation() + factor3->augmentedInformation(); + Matrix AugInformationMatrix = factor1->augmentedInformation() + + factor2->augmentedInformation() + + factor3->augmentedInformation(); // Check Information vector - Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector + Vector InfoVector = AugInformationMatrix.block( + 0, 18, 18, 1); // 18x18 Hessian + information vector // Check Hessian EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6)); } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, Hessian ) { - +TEST(SmartProjectionRigFactor, Hessian) { using namespace vanillaPose2; - KeyVector views { x1, x2 }; + KeyVector views{x1, x2}; // Project three landmarks into 2 cameras Point2 cam1_uv1 = cam1.project(landmark1); @@ -826,15 +838,15 @@ TEST( SmartProjectionRigFactor, Hessian ) { measurements_cam1.push_back(cam1_uv1); measurements_cam1.push_back(cam2_uv1); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK2) ); - FastVector cameraIds { 0, 0 }; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK2)); + FastVector cameraIds{0, 0}; SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), - Point3(0.5, 0.1, 0.3)); + Pose3 noise_pose = + Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Values values; values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); @@ -843,15 +855,16 @@ TEST( SmartProjectionRigFactor, Hessian ) { // compute triangulation from linearization point // compute reprojection errors (sum squared) - // compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) - // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] + // compare with factor.info(): the bottom right element is the squared sum of + // the reprojection errors (normalized by the covariance) check that it is + // correctly scaled when using noiseProjection = [1/4 0; 0 1/4] } /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { +TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) { using namespace bundlerPose; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedBundlerK) ); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK)); SmartProjectionParams params; params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); @@ -860,8 +873,7 @@ TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, Cal3Bundler ) { - +TEST(SmartProjectionRigFactor, Cal3Bundler) { using namespace bundlerPose; // three landmarks ~5 meters in front of camera @@ -874,11 +886,11 @@ TEST( SmartProjectionRigFactor, Cal3Bundler ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - KeyVector views { x1, x2, x3 }; + KeyVector views{x1, x2, x3}; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedBundlerK) ); - FastVector cameraIds { 0, 0, 0 }; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK)); + FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); @@ -898,21 +910,21 @@ TEST( SmartProjectionRigFactor, Cal3Bundler ) { graph.addPrior(x1, cam1.pose(), noisePrior); graph.addPrior(x2, cam2.pose(), noisePrior); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - EXPECT( - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), - values.at(x3))); + EXPECT(assert_equal( + Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); @@ -924,9 +936,11 @@ TEST( SmartProjectionRigFactor, Cal3Bundler ) { typedef GenericProjectionFactor TestProjectionFactor; static Symbol l0('L', 0); /* *************************************************************************/ -TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSamePose) { - // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark - // at a single pose, a setup that occurs in multi-camera systems +TEST(SmartProjectionRigFactor, + hessianComparedToProjFactors_measurementsFromSamePose) { + // in this test we make sure the fact works even if we have multiple pixel + // measurements of the same landmark at a single pose, a setup that occurs in + // multi-camera systems using namespace vanillaPose; Point2Vector measurements_lmk1; @@ -936,14 +950,15 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam // create redundant measurements: Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; - measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement // create inputs - std::vector keys { x1, x2, x3, x1}; + KeyVector keys{x1, x2, x3, x1}; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - FastVector cameraIds { 0, 0, 0, 0 }; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + FastVector cameraIds{0, 0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_lmk1_redundant, keys, cameraIds); @@ -953,10 +968,15 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise to get a nontrivial linearization point + // initialize third pose with some noise to get a nontrivial linearization + // point values.insert(x3, pose_above * noise_pose); EXPECT( // check that the pose is actually noisy - assert_equal( Pose3( Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), Point3(0.1, -0.1, 1.9)), values.at(x3))); + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); // linearization point for the poses Pose3 pose1 = level_pose; @@ -965,8 +985,8 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam // ==== check Hessian of smartFactor1 ===== // -- compute actual Hessian - boost::shared_ptr linearfactor1 = smartFactor1->linearize( - values); + boost::shared_ptr linearfactor1 = + smartFactor1->linearize(values); Matrix actualHessian = linearfactor1->information(); // -- compute expected Hessian from manual Schur complement from Jacobians @@ -984,30 +1004,31 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0, sharedK); Matrix HPoseActual, HEActual; - // note: b is minus the reprojection error, cf the smart factor jacobian computation - b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual, - HEActual); + // note: b is minus the reprojection error, cf the smart factor jacobian + // computation + b.segment<2>(0) = + -factor11.evaluateError(pose1, *point, HPoseActual, HEActual); F.block<2, 6>(0, 0) = HPoseActual; E.block<2, 3>(0, 0) = HEActual; TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0, sharedK); - b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual, - HEActual); + b.segment<2>(2) = + -factor12.evaluateError(pose2, *point, HPoseActual, HEActual); F.block<2, 6>(2, 6) = HPoseActual; E.block<2, 3>(2, 0) = HEActual; TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0, sharedK); - b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual, - HEActual); + b.segment<2>(4) = + -factor13.evaluateError(pose3, *point, HPoseActual, HEActual); F.block<2, 6>(4, 12) = HPoseActual; E.block<2, 3>(4, 0) = HEActual; TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0, sharedK); - b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual, - HEActual); + b.segment<2>(6) = + -factor11.evaluateError(pose1, *point, HPoseActual, HEActual); F.block<2, 6>(6, 0) = HPoseActual; E.block<2, 3>(6, 0) = HEActual; @@ -1017,20 +1038,22 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam b = (1 / sigma) * b; //* G = F' * F - F' * E * P * E' * F Matrix P = (E.transpose() * E).inverse(); - Matrix expectedHessian = F.transpose() * F - - (F.transpose() * E * P * E.transpose() * F); + Matrix expectedHessian = + F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); // ==== check Information vector of smartFactor1 ===== GaussianFactorGraph gfg; gfg.add(linearfactor1); Matrix actualHessian_v2 = gfg.hessian().first; - EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian + EXPECT(assert_equal(actualHessian_v2, actualHessian, + 1e-6)); // sanity check on hessian // -- compute actual information vector Vector actualInfoVector = gfg.hessian().second; - // -- compute expected information vector from manual Schur complement from Jacobians + // -- compute expected information vector from manual Schur complement from + // Jacobians //* g = F' * (b - E * P * E' * b) Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); @@ -1049,8 +1072,7 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { - +TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { using namespace vanillaPose; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -1060,21 +1082,24 @@ TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); // create inputs - std::vector keys { x1, x2, x3 }; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - FastVector cameraIds { 0, 0, 0 }; - FastVector cameraIdsRedundant { 0, 0, 0, 0 }; + KeyVector keys{x1, x2, x3}; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + FastVector cameraIds{0, 0, 0}; + FastVector cameraIdsRedundant{0, 0, 0, 0}; - // For first factor, we create redundant measurement (taken by the same keys as factor 1, to - // make sure the redundancy in the keys does not create problems) + // For first factor, we create redundant measurement (taken by the same keys + // as factor 1, to make sure the redundancy in the keys does not create + // problems) Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; - measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement - std::vector keys_redundant = keys; - keys_redundant.push_back(keys.at(0)); // we readd the first key + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement + KeyVector keys_redundant = keys; + keys_redundant.push_back(keys.at(0)); // we readd the first key SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); - smartFactor1->add(measurements_lmk1_redundant, keys_redundant, cameraIdsRedundant); + smartFactor1->add(measurements_lmk1_redundant, keys_redundant, + cameraIdsRedundant); SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); smartFactor2->add(measurements_lmk2, keys, cameraIds); @@ -1097,20 +1122,22 @@ TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { groundTruth.insert(x3, pose_above); DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), + // Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); // smaller noise + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - EXPECT( // check that the pose is actually noisy - assert_equal( - Pose3( - Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, - -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), - Point3(0.1, -0.1, 1.9)), values.at(x3))); + EXPECT( // check that the pose is actually noisy + assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, + -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), + values.at(x3))); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); @@ -1120,13 +1147,14 @@ TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { #ifndef DISABLE_TIMING #include -// this factor is slightly slower (but comparable) to original SmartProjectionPoseFactor +// this factor is slightly slower (but comparable) to original +// SmartProjectionPoseFactor //-Total: 0 CPU (0 times, 0 wall, 0.17 children, min: 0 max: 0) -//| -SmartRigFactor LINEARIZE: 0.11 CPU (10000 times, 0.086311 wall, 0.11 children, min: 0 max: 0) -//| -SmartPoseFactor LINEARIZE: 0.06 CPU (10000 times, 0.065103 wall, 0.06 children, min: 0 max: 0) +//| -SmartRigFactor LINEARIZE: 0.11 CPU (10000 times, 0.086311 wall, 0.11 +// children, min: 0 max: 0) | -SmartPoseFactor LINEARIZE: 0.06 CPU (10000 +// times, 0.065103 wall, 0.06 children, min: 0 max: 0) /* *************************************************************************/ -TEST( SmartProjectionRigFactor, timing ) { - +TEST(SmartProjectionRigFactor, timing) { using namespace vanillaPose; // Default cameras for simple derivatives @@ -1138,8 +1166,8 @@ TEST( SmartProjectionRigFactor, timing ) { Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); Pose3 body_P_sensorId = Pose3::identity(); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(body_P_sensorId, sharedKSimple) ); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(body_P_sensorId, sharedKSimple)); // one landmarks 1m in front of camera Point3 landmark1(0, 0, 10); @@ -1152,8 +1180,9 @@ TEST( SmartProjectionRigFactor, timing ) { size_t nrTests = 10000; - for(size_t i = 0; iadd(measurements_lmk1[0], x1, cameraId1); smartFactorP->add(measurements_lmk1[1], x1, cameraId1); @@ -1165,7 +1194,7 @@ TEST( SmartProjectionRigFactor, timing ) { gttoc_(SmartRigFactor_LINEARIZE); } - for(size_t i = 0; iadd(measurements_lmk1[0], x1); smartFactor->add(measurements_lmk1[1], x2); @@ -1181,16 +1210,21 @@ TEST( SmartProjectionRigFactor, timing ) { } #endif -///* ************************************************************************* */ -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); -//BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -//BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +///* ************************************************************************* +///*/ +// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, +// "gtsam_noiseModel_Constrained"); +// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, +// "gtsam_noiseModel_Diagonal"); +// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, +// "gtsam_noiseModel_Gaussian"); +// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, +// "gtsam_noiseModel_Isotropic"); +// BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +// BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); // -//TEST(SmartProjectionRigFactor, serialize) { +// TEST(SmartProjectionRigFactor, serialize) { // using namespace vanillaPose; // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params; @@ -1235,4 +1269,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 6a3e31dd7..09a16e6fb 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -84,7 +84,8 @@ typedef SmartProjectionPoseFactorRollingShutter> /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { using namespace vanillaPoseRS; - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); } /* ************************************************************************* */ @@ -98,7 +99,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, add) { using namespace vanillaPoseRS; - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); factor1->add(measurement1, x1, x2, interp_factor); } @@ -122,10 +124,10 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - std::vector cameraIds{0, 0, 0}; + FastVector cameraIds{0, 0, 0}; Cameras cameraRig; - cameraRig.push_back( Camera(body_P_sensor, sharedK) ); + cameraRig.push_back(Camera(body_P_sensor, sharedK)); // create by adding a batch of measurements with a bunch of calibrations SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model, cameraRig)); @@ -153,7 +155,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { EXPECT(factor1->equals(*factor2)); EXPECT(factor1->equals(*factor3)); } - { // create equal factors and show equal returns true (use default cameraId) + { // create equal factors and show equal returns true (use default cameraId) SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); factor1->add(measurements, key_pairs, interp_factors); @@ -164,7 +166,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { // returns false (use default cameraIds) SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); - factor1->add(measurement2, x2, x2, interp_factor2, cameraId1); // different! + factor1->add(measurement2, x2, x2, interp_factor2, + cameraId1); // different! factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); EXPECT(!factor1->equals(*factor2)); @@ -173,10 +176,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { { // create slightly different factors (different extrinsics) and show equal // returns false Cameras cameraRig2; - cameraRig2.push_back( Camera(body_P_sensor * body_P_sensor, sharedK) ); + cameraRig2.push_back(Camera(body_P_sensor * body_P_sensor, sharedK)); SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig2)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); - factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); // different! + factor1->add(measurement2, x2, x3, interp_factor2, + cameraId1); // different! factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); EXPECT(!factor1->equals(*factor2)); @@ -186,7 +190,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { // equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); - factor1->add(measurement2, x2, x3, interp_factor1, cameraId1); // different! + factor1->add(measurement2, x2, x3, interp_factor1, + cameraId1); // different! factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); EXPECT(!factor1->equals(*factor2)); @@ -377,13 +382,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + SmartFactorRS::shared_ptr smartFactor2( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + SmartFactorRS::shared_ptr smartFactor3( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -450,13 +458,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) { cameraRig.push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig)); - smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {1,1,1}); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {1, 1, 1}); SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig)); - smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {1,1,1}); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {1, 1, 1}); SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig)); - smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {1,1,1}); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {1, 1, 1}); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -470,7 +478,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) { Values groundTruth; groundTruth.insert(x1, level_pose); groundTruth.insert(x2, pose_right); - groundTruth.insert(x3, pose_above); // pose above is the pose of the camera + groundTruth.insert(x3, pose_above); // pose above is the pose of the camera DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), @@ -503,20 +511,21 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) { Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; // create arbitrary body_T_sensor (transforms from sensor to body) - Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), - Point3(1, 1, 1)); - Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), - Point3(0, 0, 1)); - Pose3 body_T_sensor3 = Pose3::identity(); + Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-0.03, 0., 0.01), Point3(1, 1, 1)); + Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-0.1, 0., 0.05), Point3(0, 0, 1)); + Pose3 body_T_sensor3 = Pose3(Rot3::Ypr(-0.3, 0., -0.05), Point3(0, 1, 1)); - Camera camera1(interp_pose1*body_T_sensor1, sharedK); - Camera camera2(interp_pose2*body_T_sensor2, sharedK); - Camera camera3(interp_pose3*body_T_sensor3, sharedK); + Camera camera1(interp_pose1 * body_T_sensor1, sharedK); + Camera camera2(interp_pose2 * body_T_sensor2, sharedK); + Camera camera3(interp_pose3 * body_T_sensor3, sharedK); // Project three landmarks into three cameras - projectToMultipleCameras(camera1, camera2, camera3, landmark1, measurements_lmk1); - projectToMultipleCameras(camera1, camera2, camera3, landmark2, measurements_lmk2); - projectToMultipleCameras(camera1, camera2, camera3, landmark3, measurements_lmk3); + projectToMultipleCameras(camera1, camera2, camera3, landmark1, + measurements_lmk1); + projectToMultipleCameras(camera1, camera2, camera3, landmark2, + measurements_lmk2); + projectToMultipleCameras(camera1, camera2, camera3, landmark3, + measurements_lmk3); // create inputs std::vector> key_pairs; @@ -535,13 +544,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) { cameraRig.push_back(Camera(body_T_sensor3, sharedK)); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig)); - smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {0,1,2}); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {0, 1, 2}); SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig)); - smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {0,1,2}); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {0, 1, 2}); SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig)); - smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0,1,2}); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0, 1, 2}); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -555,7 +564,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) { Values groundTruth; groundTruth.insert(x1, level_pose); groundTruth.insert(x2, pose_right); - groundTruth.insert(x3, pose_above); // pose above is the pose of the camera + groundTruth.insert(x3, pose_above); // pose above is the pose of the camera DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), @@ -608,7 +617,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { measurements_lmk1.push_back(cam1.project(landmark1)); measurements_lmk1.push_back(cam2.project(landmark1)); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(body_P_sensorId,sharedKSimple))); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple))); double interp_factor = 0; // equivalent to measurement taken at pose 1 smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor); interp_factor = 1; // equivalent to measurement taken at pose 2 @@ -702,13 +712,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(true); - SmartFactorRS smartFactor1(model, Camera(Pose3::identity(),sharedK), params); + SmartFactorRS smartFactor1(model, Camera(Pose3::identity(), sharedK), params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); - SmartFactorRS smartFactor2(model, Camera(Pose3::identity(),sharedK), params); + SmartFactorRS smartFactor2(model, Camera(Pose3::identity(), sharedK), params); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors); - SmartFactorRS smartFactor3(model, Camera(Pose3::identity(),sharedK), params); + SmartFactorRS smartFactor3(model, Camera(Pose3::identity(), sharedK), params); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -766,13 +776,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - SmartFactorRS smartFactor1(model, Camera(Pose3::identity(),sharedK), params); + SmartFactorRS smartFactor1(model, Camera(Pose3::identity(), sharedK), params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); - SmartFactorRS smartFactor2(model, Camera(Pose3::identity(),sharedK), params); + SmartFactorRS smartFactor2(model, Camera(Pose3::identity(), sharedK), params); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors); - SmartFactorRS smartFactor3(model, Camera(Pose3::identity(),sharedK), params); + SmartFactorRS smartFactor3(model, Camera(Pose3::identity(), sharedK), params); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -841,16 +851,20 @@ TEST(SmartProjectionPoseFactorRollingShutter, params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setEnableEPI(false); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + SmartFactorRS::shared_ptr smartFactor2( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + SmartFactorRS::shared_ptr smartFactor3( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); - SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + SmartFactorRS::shared_ptr smartFactor4( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor4->add(measurements_lmk4, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -901,7 +915,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1038,7 +1053,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor1); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1193,14 +1209,17 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors_redundant.push_back( interp_factors.at(0)); // we readd the first interp factor - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant); - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + SmartFactorRS::shared_ptr smartFactor2( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + SmartFactorRS::shared_ptr smartFactor3( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1244,8 +1263,9 @@ TEST(SmartProjectionPoseFactorRollingShutter, #ifndef DISABLE_TIMING #include //-Total: 0 CPU (0 times, 0 wall, 0.21 children, min: 0 max: 0) -//| -SF RS LINEARIZE: 0.15 CPU (10000 times, 0.125521 wall, 0.15 children, min: 0 max: 0) -//| -RS LINEARIZE: 0.06 CPU (10000 times, 0.06311 wall, 0.06 children, min: 0 max: 0) +//| -SF RS LINEARIZE: 0.15 CPU (10000 times, 0.125521 wall, 0.15 children, +// min: 0 max: 0) | -RS LINEARIZE: 0.06 CPU (10000 times, 0.06311 wall, 0.06 +// children, min: 0 max: 0) /* *************************************************************************/ TEST(SmartProjectionPoseFactorRollingShutter, timing) { using namespace vanillaPose; @@ -1271,7 +1291,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) { size_t nrTests = 10000; for (size_t i = 0; i < nrTests; i++) { - SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model, Camera(body_P_sensorId,sharedKSimple))); + SmartFactorRS::shared_ptr smartFactorRS( + new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple))); double interp_factor = 0; // equivalent to measurement taken at pose 1 smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor); interp_factor = 1; // equivalent to measurement taken at pose 2