From 4fd6c2cb5d12cec2ff7e9b9184d2e2fe1fd78b15 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 5 Oct 2021 22:23:16 -0400 Subject: [PATCH] bug fix - finalizing last few tests --- .../SmartProjectionPoseFactorRollingShutter.h | 3 +- ...martProjectionPoseFactorRollingShutter.cpp | 1133 ++++++++--------- 2 files changed, 567 insertions(+), 569 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index cf1526673..09d13f0e5 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -178,7 +178,8 @@ class SmartProjectionPoseFactorRollingShutter if (world_P_body_key_pairs.size() != measurements.size() || world_P_body_key_pairs.size() != alphas.size() - || world_P_body_key_pairs.size() != cameraIds.size()) { + || (world_P_body_key_pairs.size() != cameraIds.size() + && cameraIds.size() != 0)) { // cameraIds.size()=0 is default throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " "trying to add inconsistent inputs"); } diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 6c28cbda5..e93c00ba8 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -135,41 +135,41 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model, cameraRig)); factor3->add(measurements, key_pairs, interp_factors, cameraIds); -// { // create equal factors and show equal returns true -// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); -// factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); -// factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); -// factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); -// -// EXPECT(factor1->equals(*factor2)); -// EXPECT(factor1->equals(*factor3)); -// } -// { // create equal factors and show equal returns true (use default cameraId) -// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); -// factor1->add(measurement1, x1, x2, interp_factor1); -// factor1->add(measurement2, x2, x3, interp_factor2); -// factor1->add(measurement3, x3, x4, interp_factor3); -// -// EXPECT(factor1->equals(*factor2)); -// EXPECT(factor1->equals(*factor3)); -// } -// { // 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); -// -// EXPECT(factor1->equals(*factor2)); -// EXPECT(factor1->equals(*factor3)); -// } -// { // create slightly different factors (different keys) and show equal -// // returns false -// 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(measurement3, x3, x4, interp_factor3, cameraId1); -// -// EXPECT(!factor1->equals(*factor2)); -// EXPECT(!factor1->equals(*factor3)); -// } + { // create equal factors and show equal returns true + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); + factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); + factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); + + EXPECT(factor1->equals(*factor2)); + EXPECT(factor1->equals(*factor3)); + } + { // create equal factors and show equal returns true (use default cameraId) + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + factor1->add(measurement1, x1, x2, interp_factor1); + factor1->add(measurement2, x2, x3, interp_factor2); + factor1->add(measurement3, x3, x4, interp_factor3); + + EXPECT(factor1->equals(*factor2)); + EXPECT(factor1->equals(*factor3)); + } + { // 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); + + EXPECT(factor1->equals(*factor2)); + EXPECT(factor1->equals(*factor3)); + } + { // create slightly different factors (different keys) and show equal + // 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(measurement3, x3, x4, interp_factor3, cameraId1); + + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); + } { // create slightly different factors (different extrinsics) and show equal // returns false Cameras cameraRig2; @@ -194,539 +194,536 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { } } -//static const int DimBlock = 12; ///< size of the variable stacking 2 poses from -// ///< which the observation pose is interpolated -//static const int ZDim = 2; ///< Measurement dimension (Point2) -//typedef Eigen::Matrix -// MatrixZD; // F blocks (derivatives wrt camera) -//typedef std::vector> -// FBlocks; // vector of F blocks -// -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { -// using namespace vanillaPoseRS; -// -// // Project two landmarks into two cameras -// Point2 level_uv = cam1.project(landmark1); -// Point2 level_uv_right = cam2.project(landmark1); -// Pose3 body_P_sensorId = Pose3::identity(); -// -// SmartFactorRS factor(model); -// factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); -// factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); -// -// Values values; // it's a pose factor, hence these are poses -// values.insert(x1, level_pose); -// values.insert(x2, pose_right); -// values.insert(x3, pose_above); -// -// double actualError = factor.error(values); -// double expectedError = 0.0; -// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -// -// // Check triangulation -// factor.triangulateSafe(factor.cameras(values)); -// TriangulationResult point = factor.point(); -// EXPECT(point.valid()); // check triangulated point is valid -// EXPECT(assert_equal( -// landmark1, -// *point)); // check triangulation result matches expected 3D landmark -// -// // Check Jacobians -// // -- actual Jacobians -// FBlocks actualFs; -// Matrix actualE; -// Vector actualb; -// factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, -// values); -// EXPECT(actualE.rows() == 4); -// EXPECT(actualE.cols() == 3); -// EXPECT(actualb.rows() == 4); -// EXPECT(actualb.cols() == 1); -// EXPECT(actualFs.size() == 2); -// -// // -- expected Jacobians from ProjectionFactorsRollingShutter -// ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, -// x2, l0, sharedK, body_P_sensorId); -// Matrix expectedF11, expectedF12, expectedE1; -// Vector expectedb1 = factor1.evaluateError( -// level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); -// EXPECT( -// assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); -// EXPECT( -// assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); -// EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); -// // by definition computeJacobiansWithTriangulatedPoint returns minus -// // reprojectionError -// EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); -// -// ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, -// x2, x3, l0, sharedK, body_P_sensorId); -// Matrix expectedF21, expectedF22, expectedE2; -// Vector expectedb2 = factor2.evaluateError( -// pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); -// EXPECT( -// assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); -// EXPECT( -// assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); -// EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); -// // by definition computeJacobiansWithTriangulatedPoint returns minus -// // reprojectionError -// EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); -//} -// -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { -// // also includes non-identical extrinsic calibration -// using namespace vanillaPoseRS; -// -// // Project two landmarks into two cameras -// Point2 pixelError(0.5, 1.0); -// Point2 level_uv = cam1.project(landmark1) + pixelError; -// Point2 level_uv_right = cam2.project(landmark1); -// Pose3 body_P_sensorNonId = body_P_sensor; -// -// SmartFactorRS factor(model); -// factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId); -// factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, -// body_P_sensorNonId); -// -// Values values; // it's a pose factor, hence these are poses -// values.insert(x1, level_pose); -// values.insert(x2, pose_right); -// values.insert(x3, pose_above); -// -// // Perform triangulation -// factor.triangulateSafe(factor.cameras(values)); -// TriangulationResult point = factor.point(); -// EXPECT(point.valid()); // check triangulated point is valid -// Point3 landmarkNoisy = *point; -// -// // Check Jacobians -// // -- actual Jacobians -// FBlocks actualFs; -// Matrix actualE; -// Vector actualb; -// factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, -// values); -// EXPECT(actualE.rows() == 4); -// EXPECT(actualE.cols() == 3); -// EXPECT(actualb.rows() == 4); -// EXPECT(actualb.cols() == 1); -// EXPECT(actualFs.size() == 2); -// -// // -- expected Jacobians from ProjectionFactorsRollingShutter -// ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, -// x2, l0, sharedK, body_P_sensorNonId); -// Matrix expectedF11, expectedF12, expectedE1; -// Vector expectedb1 = -// factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, -// expectedF12, expectedE1); -// EXPECT( -// assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); -// EXPECT( -// assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); -// EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); -// // by definition computeJacobiansWithTriangulatedPoint returns minus -// // reprojectionError -// EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); -// -// ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, -// x2, x3, l0, sharedK, -// body_P_sensorNonId); -// Matrix expectedF21, expectedF22, expectedE2; -// Vector expectedb2 = -// factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, -// expectedF22, expectedE2); -// EXPECT( -// assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); -// EXPECT( -// assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); -// EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); -// // by definition computeJacobiansWithTriangulatedPoint returns minus -// // reprojectionError -// EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); -// -// // Check errors -// double actualError = factor.error(values); // from smart factor -// NonlinearFactorGraph nfg; -// nfg.add(factor1); -// nfg.add(factor2); -// values.insert(l0, landmarkNoisy); -// double expectedError = nfg.error(values); -// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -//} -// -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { -// using namespace vanillaPoseRS; -// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); -// -// // create inputs -// 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, x1)); -// -// std::vector interp_factors; -// interp_factors.push_back(interp_factor1); -// interp_factors.push_back(interp_factor2); -// interp_factors.push_back(interp_factor3); -// -// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); -// smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); -// smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); -// smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); -// -// 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, level_pose, noisePrior); -// graph.addPrior(x2, pose_right, noisePrior); -// -// Values groundTruth; -// groundTruth.insert(x1, level_pose); -// groundTruth.insert(x2, pose_right); -// 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 / 100, 0., -M_PI / 100), -// 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 -// 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))); -// -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -//} -// -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { -// // here we replicate a test in SmartProjectionPoseFactor by setting -// // interpolation factors to 0 and 1 (such that the rollingShutter measurements -// // falls back to standard pixel measurements) Note: this is a quite extreme -// // test since in typical camera you would not have more than 1 measurement per -// // landmark at each interpolated pose -// using namespace vanillaPose; -// -// // Default cameras for simple derivatives -// static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); -// -// Rot3 R = Rot3::identity(); -// Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); -// Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); -// Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); -// Pose3 body_P_sensorId = Pose3::identity(); -// -// // one landmarks 1m in front of camera -// Point3 landmark1(0, 0, 10); -// -// Point2Vector measurements_lmk1; -// -// // Project 2 landmarks into 2 cameras -// measurements_lmk1.push_back(cam1.project(landmark1)); -// measurements_lmk1.push_back(cam2.project(landmark1)); -// -// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); -// double interp_factor = 0; // equivalent to measurement taken at pose 1 -// smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, -// body_P_sensorId); -// interp_factor = 1; // equivalent to measurement taken at pose 2 -// smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, -// body_P_sensorId); -// -// SmartFactor::Cameras cameras; -// cameras.push_back(cam1); -// cameras.push_back(cam2); -// -// // Make sure triangulation works -// EXPECT(smartFactor1->triangulateSafe(cameras)); -// EXPECT(!smartFactor1->isDegenerate()); -// EXPECT(!smartFactor1->isPointBehindCamera()); -// boost::optional p = smartFactor1->point(); -// EXPECT(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(); -// -// Values values; -// values.insert(x1, pose1); -// values.insert(x2, pose2); -// boost::shared_ptr> actual = -// smartFactor1->createHessianFactor(values); -// 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); -//} -// -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { -// using namespace vanillaPoseRS; -// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); -// -// // create inputs -// 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, x1)); -// -// std::vector interp_factors; -// interp_factors.push_back(interp_factor1); -// interp_factors.push_back(interp_factor2); -// interp_factors.push_back(interp_factor3); -// -// double excludeLandmarksFutherThanDist = 1e10; // very large -// SmartProjectionParams params; -// params.setRankTolerance(1.0); -// params.setLinearizationMode(gtsam::HESSIAN); -// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); -// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); -// params.setEnableEPI(true); -// -// SmartFactorRS smartFactor1(model, params); -// smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS smartFactor2(model, params); -// smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS smartFactor3(model, params); -// smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); -// -// 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, level_pose, noisePrior); -// graph.addPrior(x2, pose_right, noisePrior); -// -// 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, level_pose); -// values.insert(x2, pose_right); -// // initialize third pose with some noise, we expect it to move back to -// // original pose_above -// values.insert(x3, pose_above * noise_pose); -// -// // Optimization should correct 3rd pose -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -//} -// -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, -// optimization_3poses_landmarkDistance) { -// using namespace vanillaPoseRS; -// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); -// -// // create inputs -// 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, x1)); -// -// std::vector interp_factors; -// interp_factors.push_back(interp_factor1); -// interp_factors.push_back(interp_factor2); -// interp_factors.push_back(interp_factor3); -// -// double excludeLandmarksFutherThanDist = 2; -// SmartProjectionParams params; -// params.setRankTolerance(1.0); -// params.setLinearizationMode(gtsam::HESSIAN); -// params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); -// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); -// params.setEnableEPI(false); -// -// SmartFactorRS smartFactor1(model, params); -// smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS smartFactor2(model, params); -// smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS smartFactor3(model, params); -// smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); -// -// 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, level_pose, noisePrior); -// graph.addPrior(x2, pose_right, noisePrior); -// -// 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, level_pose); -// values.insert(x2, pose_right); -// // initialize third pose with some noise, we expect it to move back to -// // original pose_above -// values.insert(x3, pose_above * noise_pose); -// -// // All factors are disabled (due to the distance threshold) 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, -// optimization_3poses_dynamicOutlierRejection) { -// using namespace vanillaPoseRS; -// // add fourth landmark -// Point3 landmark4(5, -0.5, 1); -// -// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3, -// measurements_lmk4; -// // Project 4 landmarks into cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); -// projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4); -// measurements_lmk4.at(0) = -// measurements_lmk4.at(0) + Point2(10, 10); // add outlier -// -// // create inputs -// 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, x1)); -// -// std::vector interp_factors; -// interp_factors.push_back(interp_factor1); -// interp_factors.push_back(interp_factor2); -// interp_factors.push_back(interp_factor3); -// -// double excludeLandmarksFutherThanDist = 1e10; -// double dynamicOutlierRejectionThreshold = -// 3; // max 3 pixel of average reprojection error -// -// SmartProjectionParams params; -// params.setRankTolerance(1.0); -// params.setLinearizationMode(gtsam::HESSIAN); -// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); -// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); -// params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); -// params.setEnableEPI(false); -// -// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, params)); -// smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, params)); -// smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, params)); -// smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model, params)); -// smartFactor4->add(measurements_lmk4, key_pairs, interp_factors, sharedK); -// -// 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, level_pose, noisePrior); -// graph.addPrior(x2, pose_right, noisePrior); -// -// Pose3 noise_pose = Pose3( -// Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.01, 0.01, -// 0.01)); // smaller noise, otherwise outlier rejection will kick in -// 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 -// values.insert(x3, pose_above * noise_pose); -// -// // Optimization should correct 3rd pose -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -//} -// +static const int DimBlock = 12; ///< size of the variable stacking 2 poses from + ///< which the observation pose is interpolated +static const int ZDim = 2; ///< Measurement dimension (Point2) +typedef Eigen::Matrix + MatrixZD; // F blocks (derivatives wrt camera) +typedef std::vector> + FBlocks; // vector of F blocks + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { + using namespace vanillaPoseRS; + + // Project two landmarks into two cameras + Point2 level_uv = cam1.project(landmark1); + Point2 level_uv_right = cam2.project(landmark1); + Pose3 body_P_sensorId = Pose3::identity(); + + SmartFactorRS factor(model, Camera(body_P_sensorId, sharedK)); + factor.add(level_uv, x1, x2, interp_factor1); + factor.add(level_uv_right, x2, x3, interp_factor2); + + Values values; // it's a pose factor, hence these are poses + values.insert(x1, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above); + + double actualError = factor.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + + // Check triangulation + factor.triangulateSafe(factor.cameras(values)); + TriangulationResult point = factor.point(); + EXPECT(point.valid()); // check triangulated point is valid + EXPECT(assert_equal( + landmark1, + *point)); // check triangulation result matches expected 3D landmark + + // Check Jacobians + // -- actual Jacobians + FBlocks actualFs; + Matrix actualE; + Vector actualb; + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, + values); + EXPECT(actualE.rows() == 4); + EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); + EXPECT(actualb.cols() == 1); + EXPECT(actualFs.size() == 2); + + // -- expected Jacobians from ProjectionFactorsRollingShutter + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, + x2, l0, sharedK, body_P_sensorId); + Matrix expectedF11, expectedF12, expectedE1; + Vector expectedb1 = factor1.evaluateError( + level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); + EXPECT( + assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, + x2, x3, l0, sharedK, body_P_sensorId); + Matrix expectedF21, expectedF22, expectedE2; + Vector expectedb2 = factor2.evaluateError( + pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); + EXPECT( + assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { + // also includes non-identical extrinsic calibration + using namespace vanillaPoseRS; + + // Project two landmarks into two cameras + Point2 pixelError(0.5, 1.0); + Point2 level_uv = cam1.project(landmark1) + pixelError; + Point2 level_uv_right = cam2.project(landmark1); + Pose3 body_P_sensorNonId = body_P_sensor; + + SmartFactorRS factor(model, Camera(body_P_sensorNonId, sharedK)); + factor.add(level_uv, x1, x2, interp_factor1); + factor.add(level_uv_right, x2, x3, interp_factor2); + + Values values; // it's a pose factor, hence these are poses + values.insert(x1, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above); + + // Perform triangulation + factor.triangulateSafe(factor.cameras(values)); + TriangulationResult point = factor.point(); + EXPECT(point.valid()); // check triangulated point is valid + Point3 landmarkNoisy = *point; + + // Check Jacobians + // -- actual Jacobians + FBlocks actualFs; + Matrix actualE; + Vector actualb; + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, + values); + EXPECT(actualE.rows() == 4); + EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); + EXPECT(actualb.cols() == 1); + EXPECT(actualFs.size() == 2); + + // -- expected Jacobians from ProjectionFactorsRollingShutter + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, + x2, l0, sharedK, body_P_sensorNonId); + Matrix expectedF11, expectedF12, expectedE1; + Vector expectedb1 = + factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, + expectedF12, expectedE1); + EXPECT( + assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, + x2, x3, l0, sharedK, + body_P_sensorNonId); + Matrix expectedF21, expectedF22, expectedE2; + Vector expectedb2 = + factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, + expectedF22, expectedE2); + EXPECT( + assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); + + // Check errors + double actualError = factor.error(values); // from smart factor + NonlinearFactorGraph nfg; + nfg.add(factor1); + nfg.add(factor2); + values.insert(l0, landmarkNoisy); + double expectedError = nfg.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + 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, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + 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))); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); + + 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); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Values groundTruth; + groundTruth.insert(x1, level_pose); + groundTruth.insert(x2, pose_right); + 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 / 100, 0., -M_PI / 100), + 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 + 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))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { + // here we replicate a test in SmartProjectionPoseFactor by setting + // interpolation factors to 0 and 1 (such that the rollingShutter measurements + // falls back to standard pixel measurements) Note: this is a quite extreme + // test since in typical camera you would not have more than 1 measurement per + // landmark at each interpolated pose + using namespace vanillaPose; + + // Default cameras for simple derivatives + static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); + + Rot3 R = Rot3::identity(); + Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); + Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); + Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); + Pose3 body_P_sensorId = Pose3::identity(); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + Point2Vector measurements_lmk1; + + // Project 2 landmarks into 2 cameras + 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))); + 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 + smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor); + + SmartFactor::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + // Make sure triangulation works + EXPECT(smartFactor1->triangulateSafe(cameras)); + EXPECT(!smartFactor1->isDegenerate()); + EXPECT(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + EXPECT(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(); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + boost::shared_ptr> actual = + smartFactor1->createHessianFactor(values); + 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); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + 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, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + double excludeLandmarksFutherThanDist = 1e10; // very large + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(true); + + SmartFactorRS smartFactor1(model, Camera(Pose3::identity(),sharedK), params); + smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); + + SmartFactorRS smartFactor2(model, Camera(Pose3::identity(),sharedK), params); + smartFactor2.add(measurements_lmk2, key_pairs, interp_factors); + + 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); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + 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, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to + // original pose_above + values.insert(x3, pose_above * noise_pose); + + // Optimization should correct 3rd pose + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_landmarkDistance) { + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + 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, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + double excludeLandmarksFutherThanDist = 2; + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(false); + + SmartFactorRS smartFactor1(model, Camera(Pose3::identity(),sharedK), params); + smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); + + SmartFactorRS smartFactor2(model, Camera(Pose3::identity(),sharedK), params); + smartFactor2.add(measurements_lmk2, key_pairs, interp_factors); + + 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); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + 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, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to + // original pose_above + values.insert(x3, pose_above * noise_pose); + + // All factors are disabled (due to the distance threshold) 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, + optimization_3poses_dynamicOutlierRejection) { + using namespace vanillaPoseRS; + // add fourth landmark + Point3 landmark4(5, -0.5, 1); + + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3, + measurements_lmk4; + // Project 4 landmarks into cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4); + measurements_lmk4.at(0) = + measurements_lmk4.at(0) + Point2(10, 10); // add outlier + + // create inputs + 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, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = + 3; // max 3 pixel of average reprojection error + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + params.setEnableEPI(false); + + 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)); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); + + 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)); + smartFactor4->add(measurements_lmk4, key_pairs, interp_factors); + + 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, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Pose3 noise_pose = Pose3( + Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.01, 0.01, + 0.01)); // smaller noise, otherwise outlier rejection will kick in + 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 + values.insert(x3, pose_above * noise_pose); + + // Optimization should correct 3rd pose + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + ///* *************************************************************************/ //TEST(SmartProjectionPoseFactorRollingShutter, // hessianComparedToProjFactorsRollingShutter) {