From 050d64bdca2e05e85bee15c82971a6cb8dc03fa2 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 26 Aug 2021 12:30:52 -0400 Subject: [PATCH] all tests pass except one on serialization --- gtsam/slam/SmartProjectionFactorP.h | 1 + gtsam/slam/tests/smartFactorScenarios.h | 2 + .../slam/tests/testSmartProjectionFactorP.cpp | 1658 ++++++----------- 3 files changed, 576 insertions(+), 1085 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index ccc67df44..559a294ac 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -340,6 +340,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(K_all_); + ar & BOOST_SERIALIZATION_NVP(nonUniqueKeys_); ar & BOOST_SERIALIZATION_NVP(body_P_sensors_); } diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index 1a16485e0..7421f73af 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -84,6 +84,7 @@ Camera cam3(pose_above, sharedK); namespace vanillaPose2 { typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionFactorP SmartFactorP; static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); Camera level_camera(level_pose, sharedK2); Camera level_camera_right(pose_right, sharedK2); @@ -113,6 +114,7 @@ typedef GeneralSFMFactor SFMFactor; namespace bundlerPose { typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionFactorP SmartFactorP; static boost::shared_ptr sharedBundlerK( new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); Camera level_camera(level_pose, sharedBundlerK); diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionFactorP.cpp index 816ba6b8a..9ce3e56ac 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactorP.cpp @@ -275,1101 +275,589 @@ TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { EXPECT(assert_equal(wTb3, result.at(x3))); } -///* *************************************************************************/ -//TEST( SmartProjectionFactorP, 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); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, sharedK2)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, sharedK2)); -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(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( SmartProjectionFactorP, 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}; -// -// SmartFactorP::shared_ptr smartFactor1 = boost::make_shared(model, sharedK); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::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; -// SmartFactorP::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( SmartProjectionFactorP, 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); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, sharedK)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, sharedK)); -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(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( SmartProjectionFactorP, 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); -// SmartFactorP factor1(model, sharedK, params); -// -// SmartFactorP::shared_ptr smartFactor1( -// new SmartFactorP(model, sharedK, params)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2( -// new SmartFactorP(model, sharedK, params)); -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3( -// new SmartFactorP(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( SmartProjectionFactorP, 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); -// -// SmartFactorP::shared_ptr smartFactor1( -// new SmartFactorP(model, sharedK, params)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2( -// new SmartFactorP(model, sharedK, params)); -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3( -// new SmartFactorP(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( SmartProjectionFactorP, 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); -// -// SmartFactorP::shared_ptr smartFactor1( -// new SmartFactorP(model, sharedK, params)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2( -// new SmartFactorP(model, sharedK, params)); -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3( -// new SmartFactorP(model, sharedK, params)); -// smartFactor3->add(measurements_cam3, views); -// -// SmartFactorP::shared_ptr smartFactor4( -// new SmartFactorP(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( SmartProjectionFactorP, jacobianQ ) { -// -// 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.setLinearizationMode(gtsam::JACOBIAN_Q); -// -// SmartFactorP::shared_ptr smartFactor1( -// new SmartFactorP(model, sharedK, params)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2( -// new SmartFactorP(model, sharedK, params)); -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3( -// new SmartFactorP(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 / 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( SmartProjectionFactorP, 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( SmartProjectionFactorP, 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); -// -// SmartFactorP::shared_ptr smartFactor1( -// new SmartFactorP(model, sharedK, params)); // HESSIAN, by default -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2( -// new SmartFactorP(model, sharedK, params)); // HESSIAN, by default -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3( -// new SmartFactorP(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( SmartProjectionFactorP, 3poses_2land_rotation_only_smart_projection_factor ) { -// using namespace vanillaPose2; -// -// KeyVector views {x1, x2, x3}; -// -// // Two different cameras, at the same position, but different rotations -// 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, sharedK2); -// Camera cam3(pose3, sharedK2); -// -// Point2Vector measurements_cam1, measurements_cam2; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); -// -// SmartProjectionParams params; -// params.setRankTolerance(50); -// params.setDegeneracyMode(gtsam::HANDLE_INFINITY); -// -// SmartFactorP::shared_ptr smartFactor1( -// new SmartFactorP(model, sharedK2, params)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2( -// new SmartFactorP(model, sharedK2, params)); -// smartFactor2->add(measurements_cam2, views); -// -// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -// const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); -// Point3 positionPrior = Point3(0, 0, 1); -// -// NonlinearFactorGraph graph; -// graph.push_back(smartFactor1); -// graph.push_back(smartFactor2); -// graph.addPrior(x1, cam1.pose(), noisePrior); -// graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); -// graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); -// -// 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, pose2 * noise_pose); -// values.insert(x3, pose3 * noise_pose); -// -// // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// Values result = optimizer.optimize(); -// EXPECT(assert_equal(pose3, result.at(x3))); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionFactorP, 3poses_rotation_only_smart_projection_factor ) { -// -// // this test considers a condition in which the cheirality constraint is triggered -// using namespace vanillaPose; -// -// KeyVector views {x1, x2, x3}; -// -// // Two different cameras, at the same position, but different rotations -// 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); -// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); -// -// SmartFactorP::shared_ptr smartFactor1( -// new SmartFactorP(model, sharedK, params)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2( -// new SmartFactorP(model, sharedK, params)); -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3( -// new SmartFactorP(model, sharedK, params)); -// smartFactor3->add(measurements_cam3, views); -// -// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -// const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, -// 0.10); -// Point3 positionPrior = Point3(0, 0, 1); -// -// NonlinearFactorGraph graph; -// graph.push_back(smartFactor1); -// graph.push_back(smartFactor2); -// graph.push_back(smartFactor3); -// graph.addPrior(x1, cam1.pose(), noisePrior); -// graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); -// graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); -// -// // 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, 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))); -// -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// -// // Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY) -// // rotation remains the same as the initial guess, but position is fixed by PoseTranslationPrior -//#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION -// EXPECT(assert_equal(Pose3(values.at(x3).rotation(), -// Point3(0,0,1)), result.at(x3))); -//#else -// // if the check is disabled, no cheirality exception if thrown and the pose converges to the right rotation -// // with modest accuracy since the configuration is essentially degenerate without the translation due to noise (noise_pose) -// EXPECT(assert_equal(pose3, result.at(x3),1e-3)); -//#endif -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionFactorP, 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); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(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] -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionFactorP, HessianWithRotation ) { -// // cout << " ************************ SmartProjectionFactorP: rotated Hessian **********************" << endl; -// -// using namespace vanillaPose; -// -// KeyVector views {x1, x2, x3}; -// -// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; -// -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); -// -// SmartFactorP::shared_ptr smartFactorInstance(new SmartFactorP(model, sharedK)); -// smartFactorInstance->add(measurements_cam1, views); -// -// Values values; -// values.insert(x1, cam1.pose()); -// values.insert(x2, cam2.pose()); -// values.insert(x3, cam3.pose()); -// -// boost::shared_ptr factor = smartFactorInstance->linearize( -// values); -// -// Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); -// -// Values rotValues; -// rotValues.insert(x1, poseDrift.compose(level_pose)); -// rotValues.insert(x2, poseDrift.compose(pose_right)); -// rotValues.insert(x3, poseDrift.compose(pose_above)); -// -// boost::shared_ptr factorRot = smartFactorInstance->linearize( -// rotValues); -// -// // Hessian is invariant to rotations in the nondegenerate case -// EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); -// -// Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), -// Point3(10, -4, 5)); -// -// Values tranValues; -// tranValues.insert(x1, poseDrift2.compose(level_pose)); -// tranValues.insert(x2, poseDrift2.compose(pose_right)); -// tranValues.insert(x3, poseDrift2.compose(pose_above)); -// -// boost::shared_ptr factorRotTran = -// smartFactorInstance->linearize(tranValues); -// -// // Hessian is invariant to rotations and translations in the nondegenerate case -// EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7)); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionFactorP, HessianWithRotationDegenerate ) { -// -// using namespace vanillaPose2; -// -// KeyVector views {x1, x2, x3}; -// -// // All cameras have the same pose so will be degenerate ! -// Camera cam2(level_pose, sharedK2); -// Camera cam3(level_pose, sharedK2); -// -// Point2Vector measurements_cam1; -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); -// -// SmartFactorP::shared_ptr smartFactor(new SmartFactorP(model, sharedK2)); -// smartFactor->add(measurements_cam1, views); -// -// Values values; -// values.insert(x1, cam1.pose()); -// values.insert(x2, cam2.pose()); -// values.insert(x3, cam3.pose()); -// -// boost::shared_ptr factor = smartFactor->linearize(values); -// -// Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); -// -// Values rotValues; -// rotValues.insert(x1, poseDrift.compose(level_pose)); -// rotValues.insert(x2, poseDrift.compose(level_pose)); -// rotValues.insert(x3, poseDrift.compose(level_pose)); -// -// boost::shared_ptr factorRot = // -// smartFactor->linearize(rotValues); -// -// // Hessian is invariant to rotations in the nondegenerate case -// EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); -// -// Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), -// Point3(10, -4, 5)); -// -// Values tranValues; -// tranValues.insert(x1, poseDrift2.compose(level_pose)); -// tranValues.insert(x2, poseDrift2.compose(level_pose)); -// tranValues.insert(x3, poseDrift2.compose(level_pose)); -// -// boost::shared_ptr factorRotTran = smartFactor->linearize( -// tranValues); -// -// // Hessian is invariant to rotations and translations in the nondegenerate case -// EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7)); -//} -// -///* ************************************************************************* */ -//TEST( SmartProjectionFactorP, ConstructorWithCal3Bundler) { -// using namespace bundlerPose; -// SmartProjectionParams params; -// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); -// SmartFactorP factor(model, sharedBundlerK, params); -// factor.add(measurement1, x1); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionFactorP, Cal3Bundler ) { -// -// using namespace bundlerPose; -// -// // three landmarks ~5 meters in front of camera -// Point3 landmark3(3, 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); -// -// KeyVector views {x1, x2, x3}; -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, sharedBundlerK)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, sharedBundlerK)); -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, sharedBundlerK)); -// 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(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(cam3.pose(), result.at(x3), 1e-6)); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionFactorP, Cal3BundlerRotationOnly ) { -// -// using namespace bundlerPose; -// -// KeyVector views {x1, x2, x3}; -// -// // Two 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, sharedBundlerK); -// Camera cam3(pose3, sharedBundlerK); -// -// // landmark3 at 3 meters now -// Point3 landmark3(3, 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); -// -// SmartProjectionParams params; -// params.setRankTolerance(10); -// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); -// -// SmartFactorP::shared_ptr smartFactor1( -// new SmartFactorP(model, sharedBundlerK, params)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2( -// new SmartFactorP(model, sharedBundlerK, params)); -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3( -// new SmartFactorP(model, sharedBundlerK, params)); -// smartFactor3->add(measurements_cam3, views); -// -// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -// const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, -// 0.10); -// Point3 positionPrior = Point3(0, 0, 1); -// -// NonlinearFactorGraph graph; -// graph.push_back(smartFactor1); -// graph.push_back(smartFactor2); -// graph.push_back(smartFactor3); -// graph.addPrior(x1, cam1.pose(), noisePrior); -// graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); -// graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); -// -// // 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))); -// -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// -// 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_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -//BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); -//BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -//BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); -// -//TEST(SmartProjectionFactorP, serialize) { -// using namespace vanillaPose; -// using namespace gtsam::serializationTestHelpers; -// SmartProjectionParams params; -// params.setRankTolerance(rankTol); -// SmartFactorP factor(model, sharedK, params); -// -// EXPECT(equalsObj(factor)); -// EXPECT(equalsXML(factor)); -// EXPECT(equalsBinary(factor)); -//} -// +/* *************************************************************************/ +TEST( SmartProjectionFactorP, 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); + + std::vector> sharedK2s; + sharedK2s.push_back(sharedK2); + sharedK2s.push_back(sharedK2); + sharedK2s.push_back(sharedK2); + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); + smartFactor1->add(measurements_cam1, views, sharedK2s); + + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); + smartFactor2->add(measurements_cam2, views, sharedK2s); + + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); + smartFactor3->add(measurements_cam3, views, sharedK2s); + + 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( SmartProjectionFactorP, 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}; + + std::vector> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + SmartFactorP::shared_ptr smartFactor1 = boost::make_shared(model); + smartFactor1->add(measurements_cam1, views, sharedKs); + + SmartFactorP::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(); + + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + + boost::shared_ptr > actual = + smartFactor1->createHessianFactor(values, 0.0); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); + EXPECT(assert_equal(expected, *actual, 1e-6)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); + } +} + +/* *************************************************************************/ +TEST( SmartProjectionFactorP, 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); + + std::vector> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); + smartFactor1->add(measurements_cam1, views, sharedKs); + + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); + smartFactor2->add(measurements_cam2, views, sharedKs); + + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); + smartFactor3->add(measurements_cam3, views, sharedKs); + + 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( SmartProjectionFactorP, 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); + + std::vector> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(false); + + SmartFactorP::shared_ptr smartFactor1( + new SmartFactorP(model, params)); + smartFactor1->add(measurements_cam1, views, sharedKs); + + SmartFactorP::shared_ptr smartFactor2( + new SmartFactorP(model, params)); + smartFactor2->add(measurements_cam2, views, sharedKs); + + SmartFactorP::shared_ptr smartFactor3( + new SmartFactorP(model, params)); + smartFactor3->add(measurements_cam3, views, sharedKs); + + 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( SmartProjectionFactorP, dynamicOutlierRejection ) { + + using namespace vanillaPose; + + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + + KeyVector views {x1, x2, x3}; + + std::vector> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + // 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::HESSIAN); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + + SmartFactorP::shared_ptr smartFactor1( + new SmartFactorP(model, params)); + smartFactor1->add(measurements_cam1, views, sharedKs); + + SmartFactorP::shared_ptr smartFactor2( + new SmartFactorP(model, params)); + smartFactor2->add(measurements_cam2, views, sharedKs); + + SmartFactorP::shared_ptr smartFactor3( + new SmartFactorP(model, params)); + smartFactor3->add(measurements_cam3, views, sharedKs); + + SmartFactorP::shared_ptr smartFactor4( + new SmartFactorP(model, params)); + smartFactor4->add(measurements_cam4, views, sharedKs); + + 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( SmartProjectionFactorP, 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); + + std::vector> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + SmartProjectionParams params; + params.setRankTolerance(10); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + + SmartFactorP::shared_ptr smartFactor1( + new SmartFactorP(model, params)); // HESSIAN, by default + smartFactor1->add(measurements_cam1, views, sharedKs); + + SmartFactorP::shared_ptr smartFactor2( + new SmartFactorP(model, params)); // HESSIAN, by default + smartFactor2->add(measurements_cam2, views, sharedKs); + + SmartFactorP::shared_ptr smartFactor3( + new SmartFactorP(model, params)); // HESSIAN, by default + smartFactor3->add(measurements_cam3, views, sharedKs); + + 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( SmartProjectionFactorP, 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); + + std::vector> sharedK2s; + sharedK2s.push_back(sharedK2); + sharedK2s.push_back(sharedK2); + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); + smartFactor1->add(measurements_cam1, views, sharedK2s); + + 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] +} + +/* ************************************************************************* */ +TEST( SmartProjectionFactorP, ConstructorWithCal3Bundler) { + using namespace bundlerPose; + SmartProjectionParams params; + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + SmartFactorP factor(model, params); + factor.add(measurement1, x1, sharedBundlerK); +} + +/* *************************************************************************/ +TEST( SmartProjectionFactorP, Cal3Bundler ) { + + using namespace bundlerPose; + + // three landmarks ~5 meters in front of camera + Point3 landmark3(3, 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); + + KeyVector views {x1, x2, x3}; + + std::vector> sharedBundlerKs; + sharedBundlerKs.push_back(sharedBundlerK); + sharedBundlerKs.push_back(sharedBundlerK); + sharedBundlerKs.push_back(sharedBundlerK); + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); + smartFactor1->add(measurements_cam1, views, sharedBundlerKs); + + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); + smartFactor2->add(measurements_cam2, views, sharedBundlerKs); + + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); + smartFactor3->add(measurements_cam3, views, sharedBundlerKs); + + 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(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(cam3.pose(), result.at(x3), 1e-6)); +} + +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +TEST(SmartProjectionFactorP, serialize) { + using namespace vanillaPose; + using namespace gtsam::serializationTestHelpers; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactorP factor(model, params); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +// SERIALIZATION TEST FAILS: to be fixed //TEST(SmartProjectionFactorP, serialize2) { // using namespace vanillaPose; // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params; // params.setRankTolerance(rankTol); -// Pose3 bts; -// SmartFactorP factor(model, sharedK, bts, params); +// SmartFactorP factor(model, params); // -// // insert some measurments +// // insert some measurements // KeyVector key_view; // Point2Vector meas_view; +// std::vector> sharedKs; +// +// // key_view.push_back(Symbol('x', 1)); // meas_view.push_back(Point2(10, 10)); -// factor.add(meas_view, key_view); +// sharedKs.push_back(sharedK); +// factor.add(meas_view, key_view, sharedKs); // // EXPECT(equalsObj(factor)); // EXPECT(equalsXML(factor));