From 306393a18c1cbbcc9a53378f6552c2effc35d23c Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 21 Jul 2021 14:30:55 -0400 Subject: [PATCH] solidified add and equal --- .../SmartProjectionPoseFactorRollingShutter.h | 8 +- ...martProjectionPoseFactorRollingShutter.cpp | 1656 ++++++++--------- 2 files changed, 832 insertions(+), 832 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 13ba4e85e..e5730dcd6 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -99,8 +99,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< void add(const Point2& measured, const Key& world_P_body_key1, const Key& world_P_body_key2, const double& gamma, const boost::shared_ptr& K, const Pose3 body_P_sensor) { - // store measurements in base class (note: we only store the first key there) - Base::add(measured, world_P_body_key1); + // store measurements in base class (note: we manyally add keys below to make sure they are unique + this->measured_.push_back(measured); + // but we also store the extrinsic calibration keys in the same order world_P_body_key_pairs_.push_back( std::make_pair(world_P_body_key1, world_P_body_key2)); @@ -113,6 +114,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< == this->keys_.end()) this->keys_.push_back(world_P_body_key2); // add only unique keys + // store interpolation factors + gammas_.push_back(gamma); + // store fixed calibration K_all_.push_back(K); diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index fe662932c..87bcbee81 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -16,7 +16,7 @@ * @date July 2021 */ -//#include "gtsam/slam/tests/smartFactorScenarios.h" +#include "gtsam/slam/tests/smartFactorScenarios.h" #include #include #include @@ -44,8 +44,17 @@ using symbol_shorthand::L; static Symbol x1('X', 1); static Symbol x2('X', 2); static Symbol x3('X', 3); +static Symbol x4('X', 4); +static Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), + Point3(0.1, 0.0, 0.0)); static Point2 measurement1(323.0, 240.0); +static Point2 measurement2(200.0, 220.0); +static Point2 measurement3(320.0, 10.0); +static double interp_factor = 0.5; +static double interp_factor1 = 0.3; +static double interp_factor2 = 0.4; +static double interp_factor3 = 0.5; LevenbergMarquardtParams lmParams; typedef SmartProjectionPoseFactorRollingShutter SmartFactorRS; @@ -55,841 +64,828 @@ TEST( SmartProjectionPoseFactorRollingShutter, Constructor) { SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); } -/* ************************************************************************* * +/* ************************************************************************* */ TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) { - using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactor factor1(model, sharedK, params); -} - -/* ************************************************************************* * -TEST( SmartProjectionPoseFactorRollingShutter, Constructor3) { - using namespace vanillaPose; - SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); - factor1->add(measurement1, x1); -} - -/* ************************************************************************* * -TEST( SmartProjectionPoseFactorRollingShutter, Constructor4) { - using namespace vanillaPose; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartFactor factor1(model, sharedK, params); - factor1.add(measurement1, x1); -} - -/* ************************************************************************* * -TEST( SmartProjectionPoseFactorRollingShutter, params) { - using namespace vanillaPose; - SmartProjectionParams params; - double rt = params.getRetriangulationThreshold(); - EXPECT_DOUBLES_EQUAL(1e-5, rt, 1e-7); - params.setRetriangulationThreshold(1e-3); - rt = params.getRetriangulationThreshold(); - EXPECT_DOUBLES_EQUAL(1e-3, rt, 1e-7); -} - -/* ************************************************************************* * -TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { - using namespace vanillaPose; - SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); - factor1->add(measurement1, x1); - - SmartFactor::shared_ptr factor2(new SmartFactor(model,sharedK)); - factor2->add(measurement1, x1); - - CHECK(assert_equal(*factor1, *factor2)); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, 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); - - SmartFactor factor(model, sharedK); - factor.add(level_uv, x1); - factor.add(level_uv_right, x2); - - Values values; // it's a pose factor, hence these are poses - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - - double actualError = factor.error(values); - double expectedError = 0.0; - EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); - - SmartFactor::Cameras cameras = factor.cameras(values); - double actualError2 = factor.totalReprojectionError(cameras); - EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); - - // Calculate expected derivative for point (easiest to check) - std::function f = // - std::bind(&SmartFactor::whitenedError, factor, cameras, std::placeholders::_1); - - // Calculate using computeEP - Matrix actualE; - factor.triangulateAndComputeE(actualE, values); - - // get point - boost::optional point = factor.point(); - CHECK(point); - - // calculate numerical derivative with triangulated point - Matrix expectedE = sigma * numericalDerivative11(f, *point); - EXPECT(assert_equal(expectedE, actualE, 1e-7)); - - // Calculate using reprojectionError - SmartFactor::Cameras::FBlocks F; - Matrix E; - Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); - EXPECT(assert_equal(expectedE, E, 1e-7)); - - EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); - - // Calculate using computeJacobians - Vector b; - SmartFactor::FBlocks Fs; - factor.computeJacobians(Fs, E, b, cameras, *point); - double actualError3 = b.squaredNorm(); - EXPECT(assert_equal(expectedE, E, 1e-7)); - EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, noisy ) { - - using namespace vanillaPose; - - // Project two landmarks into two cameras - Point2 pixelError(0.2, 0.2); - Point2 level_uv = level_camera.project(landmark1) + pixelError; - Point2 level_uv_right = level_camera_right.project(landmark1); - - 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)); - values.insert(x2, pose_right.compose(noise_pose)); - - SmartFactor::shared_ptr factor(new SmartFactor(model, sharedK)); - factor->add(level_uv, x1); - factor->add(level_uv_right, x2); - - double actualError1 = factor->error(values); - - SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK)); - Point2Vector measurements; - measurements.push_back(level_uv); - measurements.push_back(level_uv_right); - - KeyVector views {x1, x2}; - - factor2->add(measurements, views); - double actualError2 = factor2->error(values); - DOUBLES_EQUAL(actualError1, actualError2, 1e-7); -} - -/* ************************************************************************* -TEST(SmartProjectionPoseFactorRollingShutter, 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)); - - // These are the poses we want to estimate, from camera measurements - const Pose3 sensor_T_body = body_T_sensor.inverse(); - Pose3 wTb1 = cam1.pose() * sensor_T_body; - Pose3 wTb2 = cam2.pose() * sensor_T_body; - Pose3 wTb3 = cam3.pose() * sensor_T_body; - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0); - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 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); - - // Create smart factors - KeyVector views {x1, x2, x3}; - - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setDegeneracyMode(IGNORE_DEGENERACY); - params.setEnableEPI(false); - - SmartFactor smartFactor1(model, sharedK, body_T_sensor, params); - smartFactor1.add(measurements_cam1, views); - - SmartFactor smartFactor2(model, sharedK, body_T_sensor, params); - smartFactor2.add(measurements_cam2, views); - - SmartFactor smartFactor3(model, sharedK, body_T_sensor, params); - smartFactor3.add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - // Put all factors in factor graph, adding priors - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, wTb1, noisePrior); - graph.addPrior(x2, wTb2, noisePrior); - - // Check errors at ground truth poses - Values gtValues; - gtValues.insert(x1, wTb1); - gtValues.insert(x2, wTb2); - gtValues.insert(x3, wTb3); - double actualError = graph.error(gtValues); - 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)); - Values values; - values.insert(x1, wTb1); - values.insert(x2, wTb2); - // initialize third pose with some noise, we expect it to move back to - // original pose3 - values.insert(x3, wTb3 * noise_pose); - - LevenbergMarquardtParams lmParams; - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(wTb3, result.at(x3))); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) { - - using namespace vanillaPose2; - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 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; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2)); - smartFactor3->add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); - - Values groundTruth; - groundTruth.insert(x1, cam1.pose()); - groundTruth.insert(x2, cam2.pose()); - 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 / 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 - 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))); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, 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); - - // one landmarks 1m in front of camera - Point3 landmark1(0, 0, 10); - - Point2Vector measurements_cam1; - - // Project 2 landmarks into 2 cameras - measurements_cam1.push_back(cam1.project(landmark1)); - measurements_cam1.push_back(cam2.project(landmark1)); - - // Create smart factors - KeyVector views {x1, x2}; - - SmartFactor::shared_ptr smartFactor1 = boost::make_shared(model, sharedK); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::Cameras cameras; - cameras.push_back(cam1); - cameras.push_back(cam2); - - // Make sure triangulation works - CHECK(smartFactor1->triangulateSafe(cameras)); - CHECK(!smartFactor1->isDegenerate()); - CHECK(!smartFactor1->isPointBehindCamera()); - boost::optional p = smartFactor1->point(); - CHECK(p); - EXPECT(assert_equal(landmark1, *p)); - - VectorValues zeroDelta; - Vector6 delta; - delta.setZero(); - zeroDelta.insert(x1, delta); - zeroDelta.insert(x2, delta); - - VectorValues perturbedDelta; - delta.setOnes(); - perturbedDelta.insert(x1, delta); - perturbedDelta.insert(x2, delta); - double expectedError = 2500; - - // 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; - A1 *= 10. / sigma; - A2 *= 10. / sigma; - Matrix expectedInformation; // filled below - { - // createHessianFactor - Matrix66 G11 = 0.5 * A1.transpose() * A1; - Matrix66 G12 = 0.5 * A1.transpose() * A2; - Matrix66 G22 = 0.5 * A2.transpose() * A2; - - Vector6 g1; - g1.setZero(); - Vector6 g2; - g2.setZero(); - - double f = 0; - - RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); - expectedInformation = expected.information(); - - boost::shared_ptr > actual = - smartFactor1->createHessianFactor(cameras, 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); - EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); - } - - { - Matrix26 F1; - F1.setZero(); - F1(0, 1) = -100; - F1(0, 3) = -10; - F1(1, 0) = 100; - F1(1, 4) = -10; - Matrix26 F2; - F2.setZero(); - F2(0, 1) = -101; - F2(0, 3) = -10; - F2(0, 5) = -1; - F2(1, 0) = 100; - F2(1, 2) = 10; - F2(1, 4) = -10; - Matrix E(4, 3); - E.setZero(); - E(0, 0) = 10; - E(1, 1) = 10; - E(2, 0) = 10; - E(2, 2) = 1; - E(3, 1) = 10; - SmartFactor::FBlocks Fs = list_of(F1)(F2); - Vector b(4); - b.setZero(); - - // Create smart factors - KeyVector keys; - keys.push_back(x1); - keys.push_back(x2); - - // createJacobianQFactor - SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); - Matrix3 P = (E.transpose() * E).inverse(); - JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n); - EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-6)); - - boost::shared_ptr > actualQ = - smartFactor1->createJacobianQFactor(cameras, 0.0); - CHECK(actualQ); - EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-6)); - EXPECT(assert_equal(expectedQ, *actualQ)); - EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-6); - EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-6); - - // Whiten for RegularImplicitSchurFactor (does not have noise model) - model->WhitenSystem(E, b); - Matrix3 whiteP = (E.transpose() * E).inverse(); - Fs[0] = model->Whiten(Fs[0]); - Fs[1] = model->Whiten(Fs[1]); - - // createRegularImplicitSchurFactor - RegularImplicitSchurFactor expected(keys, Fs, E, whiteP, b); - - boost::shared_ptr > actual = - smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); - CHECK(actual); - EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); - EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); - EXPECT(assert_equal(expected, *actual)); - EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); - EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); - } - - { - // createJacobianSVDFactor - Vector1 b; - b.setZero(); - double s = sigma * sin(M_PI_4); - SharedIsotropic n = noiseModel::Isotropic::Sigma(4 - 3, sigma); - JacobianFactor expected(x1, s * A1, x2, s * A2, b, n); - EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); - - boost::shared_ptr actual = - smartFactor1->createJacobianSVDFactor(cameras, 0.0); - CHECK(actual); - EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); - EXPECT(assert_equal(expected, *actual)); - EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); - EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); - } -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection_factor ) { - - using namespace vanillaPose; - - KeyVector views {x1, x2, x3}; - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 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); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK)); - smartFactor3->add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - 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 / 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 - 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))); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, jacobianSVD ) { - - using namespace vanillaPose; - - KeyVector views {x1, x2, x3}; - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 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); - - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::JACOBIAN_SVD); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); - params.setEnableEPI(false); - SmartFactor factor1(model, sharedK, params); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, params)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, params)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, params)); - smartFactor3->add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - 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 / 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()); - values.insert(x3, pose_above * noise_pose); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { - - using namespace vanillaPose; - - double excludeLandmarksFutherThanDist = 2; - - KeyVector views {x1, x2, x3}; - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 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); - - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::JACOBIAN_SVD); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setEnableEPI(false); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, params)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, params)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, params)); - smartFactor3->add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - 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 / 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()); - values.insert(x3, pose_above * noise_pose); - - // All factors are disabled and pose should remain where it is - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(values.at(x3), result.at(x3))); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) { - - using namespace vanillaPose; - - double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - - KeyVector views {x1, x2, x3}; - - // add fourth landmark - Point3 landmark4(5, -0.5, 1); - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, - measurements_cam4; - - // Project 4 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); - projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); - measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier - - SmartProjectionParams params; - params.setLinearizationMode(gtsam::JACOBIAN_SVD); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, params)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, params)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, params)); - smartFactor3->add(measurements_cam3, views); - - SmartFactor::shared_ptr smartFactor4( - new SmartFactor(model, sharedK, params)); - smartFactor4->add(measurements_cam4, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.push_back(smartFactor4); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.addPrior(x2, cam2.pose(), noisePrior); - - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, cam3.pose()); - - // All factors are disabled and pose should remain where it is - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(cam3.pose(), result.at(x3))); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) { - - using namespace vanillaPose2; - - KeyVector views {x1, x2, x3}; - - typedef GenericProjectionFactor ProjectionFactor; - NonlinearFactorGraph graph; - - // Project three landmarks into three cameras - graph.emplace_shared(cam1.project(landmark1), model, x1, L(1), sharedK2); - graph.emplace_shared(cam2.project(landmark1), model, x2, L(1), sharedK2); - graph.emplace_shared(cam3.project(landmark1), model, x3, L(1), sharedK2); - - graph.emplace_shared(cam1.project(landmark2), model, x1, L(2), sharedK2); - graph.emplace_shared(cam2.project(landmark2), model, x2, L(2), sharedK2); - graph.emplace_shared(cam3.project(landmark2), model, x3, L(2), sharedK2); - - graph.emplace_shared(cam1.project(landmark3), model, x1, L(3), sharedK2); - graph.emplace_shared(cam2.project(landmark3), model, x2, L(3), sharedK2); - graph.emplace_shared(cam3.project(landmark3), model, x3, L(3), sharedK2); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - graph.addPrior(x1, level_pose, noisePrior); - graph.addPrior(x2, pose_right, noisePrior); - - 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, level_pose); - values.insert(x2, pose_right); - values.insert(x3, pose_above * noise_pose); - values.insert(L(1), landmark1); - values.insert(L(2), landmark2); - values.insert(L(3), landmark3); - - DOUBLES_EQUAL(48406055, graph.error(values), 1); - - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - Values result = optimizer.optimize(); - - DOUBLES_EQUAL(0, graph.error(result), 1e-9); - - EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); -} - -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, 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 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); - Camera cam2(pose2, sharedK); - Camera cam3(pose3, sharedK); - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 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); - - SmartProjectionParams params; - params.setRankTolerance(10); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, params)); // HESSIAN, by default - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, params)); // HESSIAN, by default - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, params)); // HESSIAN, by default - smartFactor3->add(measurements_cam3, views); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - 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 / 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 - 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))); - - 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(); - - 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(); - - // Check 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( SmartProjectionPoseFactorRollingShutter, Hessian ) { - - using namespace vanillaPose2; - - KeyVector views {x1, x2}; - - // Project three landmarks into 2 cameras - Point2 cam1_uv1 = cam1.project(landmark1); - Point2 cam2_uv1 = cam2.project(landmark1); - Point2Vector measurements_cam1; - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); - smartFactor1->add(measurements_cam1, views); - - 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()); - - boost::shared_ptr factor = smartFactor1->linearize(values); - - // 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] + SmartFactorRS factor1(model, params); } /* ************************************************************************* */ +TEST( SmartProjectionPoseFactorRollingShutter, add) { + using namespace vanillaPose; + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor, sharedK, body_P_sensor); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { + using namespace vanillaPose; + + // create fake measurements + std::vector measurements; + measurements.push_back(measurement1); + measurements.push_back(measurement2); + measurements.push_back(measurement3); + + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1,x2)); + key_pairs.push_back(std::make_pair(x2,x3)); + key_pairs.push_back(std::make_pair(x3,x4)); + + std::vector> intrinsicCalibrations; + intrinsicCalibrations.push_back(sharedK); + intrinsicCalibrations.push_back(sharedK); + intrinsicCalibrations.push_back(sharedK); + + std::vector extrinsicCalibrations; + extrinsicCalibrations.push_back(body_P_sensor); + extrinsicCalibrations.push_back(body_P_sensor); + extrinsicCalibrations.push_back(body_P_sensor); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + // create by adding a batch of measurements with a bunch of calibrations + SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model)); + factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, extrinsicCalibrations); + + // create by adding a batch of measurements with a single calibrations + SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model)); + factor3->add(measurements, key_pairs, interp_factors, sharedK, body_P_sensor); + + { // create equal factors and show equal returns true + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor); + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + CHECK(assert_equal(*factor1, *factor2)); + CHECK(assert_equal(*factor1, *factor3)); + } + { // create slightly different factors (different keys) and show equal returns false + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x2, interp_factor2, sharedK, body_P_sensor); // different! + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + CHECK(!assert_equal(*factor1, *factor2)); + CHECK(!assert_equal(*factor1, *factor3)); + } + { // create slightly different factors (different extrinsics) and show equal returns false + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor*body_P_sensor); // different! + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + CHECK(!assert_equal(*factor1, *factor2)); + CHECK(!assert_equal(*factor1, *factor3)); + } + { // create slightly different factors (different interp factors) and show equal returns false + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x3, interp_factor1, sharedK, body_P_sensor); // different! + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + CHECK(!assert_equal(*factor1, *factor2)); + CHECK(!assert_equal(*factor1, *factor3)); + } +} + +/* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, 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); + + SmartFactor factor(model, sharedK); + factor.add(level_uv, x1); + factor.add(level_uv_right, x2); + + Values values; // it's a pose factor, hence these are poses + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + + double actualError = factor.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + + SmartFactor::Cameras cameras = factor.cameras(values); + double actualError2 = factor.totalReprojectionError(cameras); + EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); + + // Calculate expected derivative for point (easiest to check) + std::function f = // + std::bind(&SmartFactor::whitenedError, factor, cameras, std::placeholders::_1); + + // Calculate using computeEP + Matrix actualE; + factor.triangulateAndComputeE(actualE, values); + + // get point + boost::optional point = factor.point(); + CHECK(point); + + // calculate numerical derivative with triangulated point + Matrix expectedE = sigma * numericalDerivative11(f, *point); + EXPECT(assert_equal(expectedE, actualE, 1e-7)); + + // Calculate using reprojectionError + SmartFactor::Cameras::FBlocks F; + Matrix E; + Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); + EXPECT(assert_equal(expectedE, E, 1e-7)); + + EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); + + // Calculate using computeJacobians + Vector b; + SmartFactor::FBlocks Fs; + factor.computeJacobians(Fs, E, b, cameras, *point); + double actualError3 = b.squaredNorm(); + EXPECT(assert_equal(expectedE, E, 1e-7)); + EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); + } + + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, noisy ) { + + using namespace vanillaPose; + + // Project two landmarks into two cameras + Point2 pixelError(0.2, 0.2); + Point2 level_uv = level_camera.project(landmark1) + pixelError; + Point2 level_uv_right = level_camera_right.project(landmark1); + + 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)); + values.insert(x2, pose_right.compose(noise_pose)); + + SmartFactor::shared_ptr factor(new SmartFactor(model, sharedK)); + factor->add(level_uv, x1); + factor->add(level_uv_right, x2); + + double actualError1 = factor->error(values); + + SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK)); + Point2Vector measurements; + measurements.push_back(level_uv); + measurements.push_back(level_uv_right); + + KeyVector views {x1, x2}; + + factor2->add(measurements, views); + double actualError2 = factor2->error(values); + DOUBLES_EQUAL(actualError1, actualError2, 1e-7); + } + + /* ************************************************************************* + TEST(SmartProjectionPoseFactorRollingShutter, 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)); + + // These are the poses we want to estimate, from camera measurements + const Pose3 sensor_T_body = body_T_sensor.inverse(); + Pose3 wTb1 = cam1.pose() * sensor_T_body; + Pose3 wTb2 = cam2.pose() * sensor_T_body; + Pose3 wTb3 = cam3.pose() * sensor_T_body; + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 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); + + // Create smart factors + KeyVector views {x1, x2, x3}; + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setDegeneracyMode(IGNORE_DEGENERACY); + params.setEnableEPI(false); + + SmartFactor smartFactor1(model, sharedK, body_T_sensor, params); + smartFactor1.add(measurements_cam1, views); + + SmartFactor smartFactor2(model, sharedK, body_T_sensor, params); + smartFactor2.add(measurements_cam2, views); + + SmartFactor smartFactor3(model, sharedK, body_T_sensor, params); + smartFactor3.add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + // Put all factors in factor graph, adding priors + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, wTb1, noisePrior); + graph.addPrior(x2, wTb2, noisePrior); + + // Check errors at ground truth poses + Values gtValues; + gtValues.insert(x1, wTb1); + gtValues.insert(x2, wTb2); + gtValues.insert(x3, wTb3); + double actualError = graph.error(gtValues); + 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)); + Values values; + values.insert(x1, wTb1); + values.insert(x2, wTb2); + // initialize third pose with some noise, we expect it to move back to + // original pose3 + values.insert(x3, wTb3 * noise_pose); + + LevenbergMarquardtParams lmParams; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(wTb3, result.at(x3))); + } + + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) { + + using namespace vanillaPose2; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 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; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + Values groundTruth; + groundTruth.insert(x1, cam1.pose()); + groundTruth.insert(x2, cam2.pose()); + 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 / 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 + 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))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); + } + + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, 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); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + Point2Vector measurements_cam1; + + // Project 2 landmarks into 2 cameras + measurements_cam1.push_back(cam1.project(landmark1)); + measurements_cam1.push_back(cam2.project(landmark1)); + + // Create smart factors + KeyVector views {x1, x2}; + + SmartFactor::shared_ptr smartFactor1 = boost::make_shared(model, sharedK); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + // Make sure triangulation works + CHECK(smartFactor1->triangulateSafe(cameras)); + CHECK(!smartFactor1->isDegenerate()); + CHECK(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + CHECK(p); + EXPECT(assert_equal(landmark1, *p)); + + VectorValues zeroDelta; + Vector6 delta; + delta.setZero(); + zeroDelta.insert(x1, delta); + zeroDelta.insert(x2, delta); + + VectorValues perturbedDelta; + delta.setOnes(); + perturbedDelta.insert(x1, delta); + perturbedDelta.insert(x2, delta); + double expectedError = 2500; + + // 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; + A1 *= 10. / sigma; + A2 *= 10. / sigma; + Matrix expectedInformation; // filled below + { + // createHessianFactor + Matrix66 G11 = 0.5 * A1.transpose() * A1; + Matrix66 G12 = 0.5 * A1.transpose() * A2; + Matrix66 G22 = 0.5 * A2.transpose() * A2; + + Vector6 g1; + g1.setZero(); + Vector6 g2; + g2.setZero(); + + double f = 0; + + RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); + expectedInformation = expected.information(); + + boost::shared_ptr > actual = + smartFactor1->createHessianFactor(cameras, 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); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); + } + + { + Matrix26 F1; + F1.setZero(); + F1(0, 1) = -100; + F1(0, 3) = -10; + F1(1, 0) = 100; + F1(1, 4) = -10; + Matrix26 F2; + F2.setZero(); + F2(0, 1) = -101; + F2(0, 3) = -10; + F2(0, 5) = -1; + F2(1, 0) = 100; + F2(1, 2) = 10; + F2(1, 4) = -10; + Matrix E(4, 3); + E.setZero(); + E(0, 0) = 10; + E(1, 1) = 10; + E(2, 0) = 10; + E(2, 2) = 1; + E(3, 1) = 10; + SmartFactor::FBlocks Fs = list_of(F1)(F2); + Vector b(4); + b.setZero(); + + // Create smart factors + KeyVector keys; + keys.push_back(x1); + keys.push_back(x2); + + // createJacobianQFactor + SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); + Matrix3 P = (E.transpose() * E).inverse(); + JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n); + EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-6)); + + boost::shared_ptr > actualQ = + smartFactor1->createJacobianQFactor(cameras, 0.0); + CHECK(actualQ); + EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-6)); + EXPECT(assert_equal(expectedQ, *actualQ)); + EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-6); + + // Whiten for RegularImplicitSchurFactor (does not have noise model) + model->WhitenSystem(E, b); + Matrix3 whiteP = (E.transpose() * E).inverse(); + Fs[0] = model->Whiten(Fs[0]); + Fs[1] = model->Whiten(Fs[1]); + + // createRegularImplicitSchurFactor + RegularImplicitSchurFactor expected(keys, Fs, E, whiteP, b); + + boost::shared_ptr > actual = + smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); + CHECK(actual); + EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); + EXPECT(assert_equal(expected, *actual)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); + } + + { + // createJacobianSVDFactor + Vector1 b; + b.setZero(); + double s = sigma * sin(M_PI_4); + SharedIsotropic n = noiseModel::Isotropic::Sigma(4 - 3, sigma); + JacobianFactor expected(x1, s * A1, x2, s * A2, b, n); + EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); + + boost::shared_ptr actual = + smartFactor1->createJacobianSVDFactor(cameras, 0.0); + CHECK(actual); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); + EXPECT(assert_equal(expected, *actual)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); + } + } + + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection_factor ) { + + using namespace vanillaPose; + + KeyVector views {x1, x2, x3}; + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 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); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + 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 / 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 + 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))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); + } + + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { + + using namespace vanillaPose; + + double excludeLandmarksFutherThanDist = 2; + + KeyVector views {x1, x2, x3}; + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 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); + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(false); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, params)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, params)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, params)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + 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 / 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()); + values.insert(x3, pose_above * noise_pose); + + // All factors are disabled and pose should remain where it is + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(values.at(x3), result.at(x3))); + } + + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) { + + using namespace vanillaPose; + + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + + KeyVector views {x1, x2, x3}; + + // add fourth landmark + Point3 landmark4(5, -0.5, 1); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4; + + // Project 4 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); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); + measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier + + SmartProjectionParams params; + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, params)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, params)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, params)); + smartFactor3->add(measurements_cam3, views); + + SmartFactor::shared_ptr smartFactor4( + new SmartFactor(model, sharedK, params)); + smartFactor4->add(measurements_cam4, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(smartFactor4); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); + + // All factors are disabled and pose should remain where it is + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(cam3.pose(), result.at(x3))); + } + + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) { + + using namespace vanillaPose2; + + KeyVector views {x1, x2, x3}; + + typedef GenericProjectionFactor ProjectionFactor; + NonlinearFactorGraph graph; + + // Project three landmarks into three cameras + graph.emplace_shared(cam1.project(landmark1), model, x1, L(1), sharedK2); + graph.emplace_shared(cam2.project(landmark1), model, x2, L(1), sharedK2); + graph.emplace_shared(cam3.project(landmark1), model, x3, L(1), sharedK2); + + graph.emplace_shared(cam1.project(landmark2), model, x1, L(2), sharedK2); + graph.emplace_shared(cam2.project(landmark2), model, x2, L(2), sharedK2); + graph.emplace_shared(cam3.project(landmark2), model, x3, L(2), sharedK2); + + graph.emplace_shared(cam1.project(landmark3), model, x1, L(3), sharedK2); + graph.emplace_shared(cam2.project(landmark3), model, x2, L(3), sharedK2); + graph.emplace_shared(cam3.project(landmark3), model, x3, L(3), sharedK2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + 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, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above * noise_pose); + values.insert(L(1), landmark1); + values.insert(L(2), landmark2); + values.insert(L(3), landmark3); + + DOUBLES_EQUAL(48406055, graph.error(values), 1); + + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + Values result = optimizer.optimize(); + + DOUBLES_EQUAL(0, graph.error(result), 1e-9); + + EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); + } + + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, 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 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Camera cam2(pose2, sharedK); + Camera cam3(pose3, sharedK); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // 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); + + SmartProjectionParams params; + params.setRankTolerance(10); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, params)); // HESSIAN, by default + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, params)); // HESSIAN, by default + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, params)); // HESSIAN, by default + smartFactor3->add(measurements_cam3, views); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + 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 / 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 + 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))); + + 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(); + + 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(); + + // Check 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( SmartProjectionPoseFactorRollingShutter, Hessian ) { + + using namespace vanillaPose2; + + KeyVector views {x1, x2}; + + // Project three landmarks into 2 cameras + Point2 cam1_uv1 = cam1.project(landmark1); + Point2 cam2_uv1 = cam2.project(landmark1); + Point2Vector measurements_cam1; + measurements_cam1.push_back(cam1_uv1); + measurements_cam1.push_back(cam2_uv1); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); + smartFactor1->add(measurements_cam1, views); + + 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()); + + boost::shared_ptr factor = smartFactor1->linearize(values); + + // 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] + } + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);