12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fixed bug

release/4.3a0
lcarlone 2021-08-25 21:24:37 -04:00
parent 37dfb0c3da
commit df82ca1b0c
3 changed files with 169 additions and 178 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/slam/SmartProjectionFactorP.h
View File

@ -186,10 +186,11 @@ public:
*/ */
typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras(const Values& values) const override {
typename Base::Cameras cameras; typename Base::Cameras cameras;
for (const Key& i : this->keys_) { for (size_t i = 0; i < this->keys_.size(); i++) {
const Pose3& body_P_cam = body_P_sensors_[i]; const Pose3& body_P_cam_i = body_P_sensors_[i];
const Pose3 world_P_sensor_k = values.at<Pose3>(i) * body_P_cam; const Pose3 world_P_sensor_i = values.at<Pose3>(this->keys_[i])
cameras.emplace_back(world_P_sensor_k, K_all_[i]); * body_P_cam_i;
cameras.emplace_back(world_P_sensor_i, K_all_[i]);
} }
return cameras; return cameras;
} }

1
gtsam/slam/tests/smartFactorScenarios.h
View File

@ -70,6 +70,7 @@ SmartProjectionParams params;
namespace vanillaPose { namespace vanillaPose {
typedef PinholePose<Cal3_S2> Camera; typedef PinholePose<Cal3_S2> Camera;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor; typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
typedef SmartProjectionFactorP<Camera> SmartFactorP;
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
Camera level_camera(level_pose, sharedK); Camera level_camera(level_pose, sharedK);
Camera level_camera_right(pose_right, sharedK); Camera level_camera_right(pose_right, sharedK);

337
gtsam/slam/tests/testSmartProjectionFactorP.cpp
View File

@ -51,87 +51,76 @@ LevenbergMarquardtParams lmParams;
// Make more verbose like so (in tests): // Make more verbose like so (in tests):
// params.verbosityLM = LevenbergMarquardtParams::SUMMARY; // params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
///* ************************************************************************* */ /* ************************************************************************* */
//TEST( SmartProjectionPoseFactor, Constructor) { TEST( SmartProjectionFactorP, Constructor) {
// using namespace vanillaPose; using namespace vanillaPose;
// SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); SmartFactorP::shared_ptr factor1(new SmartFactorP(model));
//} }
//
///* ************************************************************************* */ /* ************************************************************************* */
//TEST( SmartProjectionPoseFactor, Constructor2) { TEST( SmartProjectionFactorP, Constructor2) {
// using namespace vanillaPose; using namespace vanillaPose;
// SmartProjectionParams params; SmartProjectionParams params;
// params.setRankTolerance(rankTol); params.setRankTolerance(rankTol);
// SmartFactor factor1(model, sharedK, params); SmartFactorP factor1(model, params);
//} }
//
///* ************************************************************************* */ /* ************************************************************************* */
//TEST( SmartProjectionPoseFactor, Constructor3) { TEST( SmartProjectionFactorP, Constructor3) {
// using namespace vanillaPose; using namespace vanillaPose;
// SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); SmartFactorP::shared_ptr factor1(new SmartFactorP(model));
// factor1->add(measurement1, x1); factor1->add(measurement1, x1, sharedK);
//} }
//
///* ************************************************************************* */ /* ************************************************************************* */
//TEST( SmartProjectionPoseFactor, Constructor4) { TEST( SmartProjectionFactorP, Constructor4) {
// using namespace vanillaPose; using namespace vanillaPose;
// SmartProjectionParams params; SmartProjectionParams params;
// params.setRankTolerance(rankTol); params.setRankTolerance(rankTol);
// SmartFactor factor1(model, sharedK, params); SmartFactorP factor1(model, params);
// factor1.add(measurement1, x1); factor1.add(measurement1, x1, sharedK);
//} }
//
///* ************************************************************************* */ /* ************************************************************************* */
//TEST( SmartProjectionPoseFactor, params) { TEST( SmartProjectionFactorP, Equals ) {
// using namespace vanillaPose; using namespace vanillaPose;
// SmartProjectionParams params; SmartFactorP::shared_ptr factor1(new SmartFactorP(model));
// double rt = params.getRetriangulationThreshold(); factor1->add(measurement1, x1, sharedK);
// EXPECT_DOUBLES_EQUAL(1e-5, rt, 1e-7);
// params.setRetriangulationThreshold(1e-3); SmartFactorP::shared_ptr factor2(new SmartFactorP(model));
// rt = params.getRetriangulationThreshold(); factor2->add(measurement1, x1, sharedK);
// EXPECT_DOUBLES_EQUAL(1e-3, rt, 1e-7);
//} CHECK(assert_equal(*factor1, *factor2));
// }
///* ************************************************************************* */
//TEST( SmartProjectionPoseFactor, Equals ) { /* *************************************************************************/
// using namespace vanillaPose; TEST( SmartProjectionFactorP, noiseless ) {
// SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
// factor1->add(measurement1, x1); using namespace vanillaPose;
//
// SmartFactor::shared_ptr factor2(new SmartFactor(model,sharedK)); // Project two landmarks into two cameras
// factor2->add(measurement1, x1); Point2 level_uv = level_camera.project(landmark1);
// Point2 level_uv_right = level_camera_right.project(landmark1);
// CHECK(assert_equal(*factor1, *factor2));
//} SmartFactorP factor(model);
// factor.add(level_uv, x1, sharedK);
///* *************************************************************************/ factor.add(level_uv_right, x2, sharedK);
//TEST( SmartProjectionPoseFactor, noiseless ) {
// Values values; // it's a pose factor, hence these are poses
// using namespace vanillaPose; values.insert(x1, cam1.pose());
// values.insert(x2, cam2.pose());
// // Project two landmarks into two cameras
// Point2 level_uv = level_camera.project(landmark1); double actualError = factor.error(values);
// Point2 level_uv_right = level_camera_right.project(landmark1); double expectedError = 0.0;
// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
// SmartFactor factor(model, sharedK);
// factor.add(level_uv, x1); // SmartFactorP::Cameras cameras = factor.cameras(values);
// factor.add(level_uv_right, x2);
//
// Values values; // it's a pose factor, hence these are poses
// values.insert(x1, cam1.pose());
// values.insert(x2, cam2.pose());
//
// double actualError = factor.error(values);
// double expectedError = 0.0;
// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
//
// SmartFactor::Cameras cameras = factor.cameras(values);
// double actualError2 = factor.totalReprojectionError(cameras); // double actualError2 = factor.totalReprojectionError(cameras);
// EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); // EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
// //
// // Calculate expected derivative for point (easiest to check) // // Calculate expected derivative for point (easiest to check)
// std::function<Vector(Point3)> f = // // std::function<Vector(Point3)> f = //
// std::bind(&SmartFactor::whitenedError<Point3>, factor, cameras, std::placeholders::_1); // std::bind(&SmartFactorP::whitenedError<Point3>, factor, cameras, std::placeholders::_1);
// //
// // Calculate using computeEP // // Calculate using computeEP
// Matrix actualE; // Matrix actualE;
@ -146,7 +135,7 @@ LevenbergMarquardtParams lmParams;
// EXPECT(assert_equal(expectedE, actualE, 1e-7)); // EXPECT(assert_equal(expectedE, actualE, 1e-7));
// //
// // Calculate using reprojectionError // // Calculate using reprojectionError
// SmartFactor::Cameras::FBlocks F; // SmartFactorP::Cameras::FBlocks F;
// Matrix E; // Matrix E;
// Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); // Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E);
// EXPECT(assert_equal(expectedE, E, 1e-7)); // EXPECT(assert_equal(expectedE, E, 1e-7));
@ -155,15 +144,15 @@ LevenbergMarquardtParams lmParams;
// //
// // Calculate using computeJacobians // // Calculate using computeJacobians
// Vector b; // Vector b;
// SmartFactor::FBlocks Fs; // SmartFactorP::FBlocks Fs;
// factor.computeJacobians(Fs, E, b, cameras, *point); // factor.computeJacobians(Fs, E, b, cameras, *point);
// double actualError3 = b.squaredNorm(); // double actualError3 = b.squaredNorm();
// EXPECT(assert_equal(expectedE, E, 1e-7)); // EXPECT(assert_equal(expectedE, E, 1e-7));
// EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); // EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6);
//} }
//
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionPoseFactor, noisy ) { //TEST( SmartProjectionFactorP, noisy ) {
// //
// using namespace vanillaPose; // using namespace vanillaPose;
// //
@ -178,13 +167,13 @@ LevenbergMarquardtParams lmParams;
// Point3(0.5, 0.1, 0.3)); // Point3(0.5, 0.1, 0.3));
// values.insert(x2, pose_right.compose(noise_pose)); // values.insert(x2, pose_right.compose(noise_pose));
// //
// SmartFactor::shared_ptr factor(new SmartFactor(model, sharedK)); // SmartFactorP::shared_ptr factor(new SmartFactorP(model, sharedK));
// factor->add(level_uv, x1); // factor->add(level_uv, x1);
// factor->add(level_uv_right, x2); // factor->add(level_uv_right, x2);
// //
// double actualError1 = factor->error(values); // double actualError1 = factor->error(values);
// //
// SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK)); // SmartFactorP::shared_ptr factor2(new SmartFactorP(model, sharedK));
// Point2Vector measurements; // Point2Vector measurements;
// measurements.push_back(level_uv); // measurements.push_back(level_uv);
// measurements.push_back(level_uv_right); // measurements.push_back(level_uv_right);
@ -197,7 +186,7 @@ LevenbergMarquardtParams lmParams;
//} //}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { //TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) {
// using namespace vanillaPose; // using namespace vanillaPose;
// //
// // create arbitrary body_T_sensor (transforms from sensor to body) // // create arbitrary body_T_sensor (transforms from sensor to body)
@ -227,13 +216,13 @@ LevenbergMarquardtParams lmParams;
// params.setDegeneracyMode(IGNORE_DEGENERACY); // params.setDegeneracyMode(IGNORE_DEGENERACY);
// params.setEnableEPI(false); // params.setEnableEPI(false);
// //
// SmartFactor smartFactor1(model, sharedK, body_T_sensor, params); // SmartFactorP smartFactor1(model, sharedK, body_T_sensor, params);
// smartFactor1.add(measurements_cam1, views); // smartFactor1.add(measurements_cam1, views);
// //
// SmartFactor smartFactor2(model, sharedK, body_T_sensor, params); // SmartFactorP smartFactor2(model, sharedK, body_T_sensor, params);
// smartFactor2.add(measurements_cam2, views); // smartFactor2.add(measurements_cam2, views);
// //
// SmartFactor smartFactor3(model, sharedK, body_T_sensor, params); // SmartFactorP smartFactor3(model, sharedK, body_T_sensor, params);
// smartFactor3.add(measurements_cam3, views); // smartFactor3.add(measurements_cam3, views);
// //
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -272,7 +261,7 @@ LevenbergMarquardtParams lmParams;
//} //}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { //TEST( SmartProjectionFactorP, 3poses_smart_projection_factor ) {
// //
// using namespace vanillaPose2; // using namespace vanillaPose2;
// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; // Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
@ -287,13 +276,13 @@ LevenbergMarquardtParams lmParams;
// views.push_back(x2); // views.push_back(x2);
// views.push_back(x3); // views.push_back(x3);
// //
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); // SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, sharedK2));
// smartFactor1->add(measurements_cam1, views); // smartFactor1->add(measurements_cam1, views);
// //
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2)); // SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, sharedK2));
// smartFactor2->add(measurements_cam2, views); // smartFactor2->add(measurements_cam2, views);
// //
// SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2)); // SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, sharedK2));
// smartFactor3->add(measurements_cam3, views); // smartFactor3->add(measurements_cam3, views);
// //
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -333,7 +322,7 @@ LevenbergMarquardtParams lmParams;
//} //}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionPoseFactor, Factors ) { //TEST( SmartProjectionFactorP, Factors ) {
// //
// using namespace vanillaPose; // using namespace vanillaPose;
// //
@ -355,10 +344,10 @@ LevenbergMarquardtParams lmParams;
// // Create smart factors // // Create smart factors
// KeyVector views {x1, x2}; // KeyVector views {x1, x2};
// //
// SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>(model, sharedK); // SmartFactorP::shared_ptr smartFactor1 = boost::make_shared<SmartFactorP>(model, sharedK);
// smartFactor1->add(measurements_cam1, views); // smartFactor1->add(measurements_cam1, views);
// //
// SmartFactor::Cameras cameras; // SmartFactorP::Cameras cameras;
// cameras.push_back(cam1); // cameras.push_back(cam1);
// cameras.push_back(cam2); // cameras.push_back(cam2);
// //
@ -435,7 +424,7 @@ LevenbergMarquardtParams lmParams;
// E(2, 0) = 10; // E(2, 0) = 10;
// E(2, 2) = 1; // E(2, 2) = 1;
// E(3, 1) = 10; // E(3, 1) = 10;
// SmartFactor::FBlocks Fs = list_of<Matrix>(F1)(F2); // SmartFactorP::FBlocks Fs = list_of<Matrix>(F1)(F2);
// Vector b(4); // Vector b(4);
// b.setZero(); // b.setZero();
// //
@ -497,7 +486,7 @@ LevenbergMarquardtParams lmParams;
//} //}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { //TEST( SmartProjectionFactorP, 3poses_iterative_smart_projection_factor ) {
// //
// using namespace vanillaPose; // using namespace vanillaPose;
// //
@ -510,13 +499,13 @@ LevenbergMarquardtParams lmParams;
// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); // projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// //
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK)); // SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, sharedK));
// smartFactor1->add(measurements_cam1, views); // smartFactor1->add(measurements_cam1, views);
// //
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK)); // SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, sharedK));
// smartFactor2->add(measurements_cam2, views); // smartFactor2->add(measurements_cam2, views);
// //
// SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK)); // SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, sharedK));
// smartFactor3->add(measurements_cam3, views); // smartFactor3->add(measurements_cam3, views);
// //
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -551,7 +540,7 @@ LevenbergMarquardtParams lmParams;
//} //}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionPoseFactor, jacobianSVD ) { //TEST( SmartProjectionFactorP, jacobianSVD ) {
// //
// using namespace vanillaPose; // using namespace vanillaPose;
// //
@ -569,18 +558,18 @@ LevenbergMarquardtParams lmParams;
// params.setLinearizationMode(gtsam::JACOBIAN_SVD); // params.setLinearizationMode(gtsam::JACOBIAN_SVD);
// params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); // params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
// params.setEnableEPI(false); // params.setEnableEPI(false);
// SmartFactor factor1(model, sharedK, params); // SmartFactorP factor1(model, sharedK, params);
// //
// SmartFactor::shared_ptr smartFactor1( // SmartFactorP::shared_ptr smartFactor1(
// new SmartFactor(model, sharedK, params)); // new SmartFactorP(model, sharedK, params));
// smartFactor1->add(measurements_cam1, views); // smartFactor1->add(measurements_cam1, views);
// //
// SmartFactor::shared_ptr smartFactor2( // SmartFactorP::shared_ptr smartFactor2(
// new SmartFactor(model, sharedK, params)); // new SmartFactorP(model, sharedK, params));
// smartFactor2->add(measurements_cam2, views); // smartFactor2->add(measurements_cam2, views);
// //
// SmartFactor::shared_ptr smartFactor3( // SmartFactorP::shared_ptr smartFactor3(
// new SmartFactor(model, sharedK, params)); // new SmartFactorP(model, sharedK, params));
// smartFactor3->add(measurements_cam3, views); // smartFactor3->add(measurements_cam3, views);
// //
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -607,7 +596,7 @@ LevenbergMarquardtParams lmParams;
//} //}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionPoseFactor, landmarkDistance ) { //TEST( SmartProjectionFactorP, landmarkDistance ) {
// //
// using namespace vanillaPose; // using namespace vanillaPose;
// //
@ -629,16 +618,16 @@ LevenbergMarquardtParams lmParams;
// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); // params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
// params.setEnableEPI(false); // params.setEnableEPI(false);
// //
// SmartFactor::shared_ptr smartFactor1( // SmartFactorP::shared_ptr smartFactor1(
// new SmartFactor(model, sharedK, params)); // new SmartFactorP(model, sharedK, params));
// smartFactor1->add(measurements_cam1, views); // smartFactor1->add(measurements_cam1, views);
// //
// SmartFactor::shared_ptr smartFactor2( // SmartFactorP::shared_ptr smartFactor2(
// new SmartFactor(model, sharedK, params)); // new SmartFactorP(model, sharedK, params));
// smartFactor2->add(measurements_cam2, views); // smartFactor2->add(measurements_cam2, views);
// //
// SmartFactor::shared_ptr smartFactor3( // SmartFactorP::shared_ptr smartFactor3(
// new SmartFactor(model, sharedK, params)); // new SmartFactorP(model, sharedK, params));
// smartFactor3->add(measurements_cam3, views); // smartFactor3->add(measurements_cam3, views);
// //
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -666,7 +655,7 @@ LevenbergMarquardtParams lmParams;
//} //}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { //TEST( SmartProjectionFactorP, dynamicOutlierRejection ) {
// //
// using namespace vanillaPose; // using namespace vanillaPose;
// //
@ -693,20 +682,20 @@ LevenbergMarquardtParams lmParams;
// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); // params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
// params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); // params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
// //
// SmartFactor::shared_ptr smartFactor1( // SmartFactorP::shared_ptr smartFactor1(
// new SmartFactor(model, sharedK, params)); // new SmartFactorP(model, sharedK, params));
// smartFactor1->add(measurements_cam1, views); // smartFactor1->add(measurements_cam1, views);
// //
// SmartFactor::shared_ptr smartFactor2( // SmartFactorP::shared_ptr smartFactor2(
// new SmartFactor(model, sharedK, params)); // new SmartFactorP(model, sharedK, params));
// smartFactor2->add(measurements_cam2, views); // smartFactor2->add(measurements_cam2, views);
// //
// SmartFactor::shared_ptr smartFactor3( // SmartFactorP::shared_ptr smartFactor3(
// new SmartFactor(model, sharedK, params)); // new SmartFactorP(model, sharedK, params));
// smartFactor3->add(measurements_cam3, views); // smartFactor3->add(measurements_cam3, views);
// //
// SmartFactor::shared_ptr smartFactor4( // SmartFactorP::shared_ptr smartFactor4(
// new SmartFactor(model, sharedK, params)); // new SmartFactorP(model, sharedK, params));
// smartFactor4->add(measurements_cam4, views); // smartFactor4->add(measurements_cam4, views);
// //
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -732,7 +721,7 @@ LevenbergMarquardtParams lmParams;
//} //}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionPoseFactor, jacobianQ ) { //TEST( SmartProjectionFactorP, jacobianQ ) {
// //
// using namespace vanillaPose; // using namespace vanillaPose;
// //
@ -748,16 +737,16 @@ LevenbergMarquardtParams lmParams;
// SmartProjectionParams params; // SmartProjectionParams params;
// params.setLinearizationMode(gtsam::JACOBIAN_Q); // params.setLinearizationMode(gtsam::JACOBIAN_Q);
// //
// SmartFactor::shared_ptr smartFactor1( // SmartFactorP::shared_ptr smartFactor1(
// new SmartFactor(model, sharedK, params)); // new SmartFactorP(model, sharedK, params));
// smartFactor1->add(measurements_cam1, views); // smartFactor1->add(measurements_cam1, views);
// //
// SmartFactor::shared_ptr smartFactor2( // SmartFactorP::shared_ptr smartFactor2(
// new SmartFactor(model, sharedK, params)); // new SmartFactorP(model, sharedK, params));
// smartFactor2->add(measurements_cam2, views); // smartFactor2->add(measurements_cam2, views);
// //
// SmartFactor::shared_ptr smartFactor3( // SmartFactorP::shared_ptr smartFactor3(
// new SmartFactor(model, sharedK, params)); // new SmartFactorP(model, sharedK, params));
// smartFactor3->add(measurements_cam3, views); // smartFactor3->add(measurements_cam3, views);
// //
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -783,7 +772,7 @@ LevenbergMarquardtParams lmParams;
//} //}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { //TEST( SmartProjectionFactorP, 3poses_projection_factor ) {
// //
// using namespace vanillaPose2; // using namespace vanillaPose2;
// //
@ -830,7 +819,7 @@ LevenbergMarquardtParams lmParams;
//} //}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionPoseFactor, CheckHessian) { //TEST( SmartProjectionFactorP, CheckHessian) {
// //
// KeyVector views {x1, x2, x3}; // KeyVector views {x1, x2, x3};
// //
@ -853,16 +842,16 @@ LevenbergMarquardtParams lmParams;
// SmartProjectionParams params; // SmartProjectionParams params;
// params.setRankTolerance(10); // params.setRankTolerance(10);
// //
// SmartFactor::shared_ptr smartFactor1( // SmartFactorP::shared_ptr smartFactor1(
// new SmartFactor(model, sharedK, params)); // HESSIAN, by default // new SmartFactorP(model, sharedK, params)); // HESSIAN, by default
// smartFactor1->add(measurements_cam1, views); // smartFactor1->add(measurements_cam1, views);
// //
// SmartFactor::shared_ptr smartFactor2( // SmartFactorP::shared_ptr smartFactor2(
// new SmartFactor(model, sharedK, params)); // HESSIAN, by default // new SmartFactorP(model, sharedK, params)); // HESSIAN, by default
// smartFactor2->add(measurements_cam2, views); // smartFactor2->add(measurements_cam2, views);
// //
// SmartFactor::shared_ptr smartFactor3( // SmartFactorP::shared_ptr smartFactor3(
// new SmartFactor(model, sharedK, params)); // HESSIAN, by default // new SmartFactorP(model, sharedK, params)); // HESSIAN, by default
// smartFactor3->add(measurements_cam3, views); // smartFactor3->add(measurements_cam3, views);
// //
// NonlinearFactorGraph graph; // NonlinearFactorGraph graph;
@ -912,7 +901,7 @@ LevenbergMarquardtParams lmParams;
//} //}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) { //TEST( SmartProjectionFactorP, 3poses_2land_rotation_only_smart_projection_factor ) {
// using namespace vanillaPose2; // using namespace vanillaPose2;
// //
// KeyVector views {x1, x2, x3}; // KeyVector views {x1, x2, x3};
@ -933,12 +922,12 @@ LevenbergMarquardtParams lmParams;
// params.setRankTolerance(50); // params.setRankTolerance(50);
// params.setDegeneracyMode(gtsam::HANDLE_INFINITY); // params.setDegeneracyMode(gtsam::HANDLE_INFINITY);
// //
// SmartFactor::shared_ptr smartFactor1( // SmartFactorP::shared_ptr smartFactor1(
// new SmartFactor(model, sharedK2, params)); // new SmartFactorP(model, sharedK2, params));
// smartFactor1->add(measurements_cam1, views); // smartFactor1->add(measurements_cam1, views);
// //
// SmartFactor::shared_ptr smartFactor2( // SmartFactorP::shared_ptr smartFactor2(
// new SmartFactor(model, sharedK2, params)); // new SmartFactorP(model, sharedK2, params));
// smartFactor2->add(measurements_cam2, views); // smartFactor2->add(measurements_cam2, views);
// //
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -966,7 +955,7 @@ LevenbergMarquardtParams lmParams;
//} //}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { //TEST( SmartProjectionFactorP, 3poses_rotation_only_smart_projection_factor ) {
// //
// // this test considers a condition in which the cheirality constraint is triggered // // this test considers a condition in which the cheirality constraint is triggered
// using namespace vanillaPose; // using namespace vanillaPose;
@ -991,16 +980,16 @@ LevenbergMarquardtParams lmParams;
// params.setRankTolerance(10); // params.setRankTolerance(10);
// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); // params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
// //
// SmartFactor::shared_ptr smartFactor1( // SmartFactorP::shared_ptr smartFactor1(
// new SmartFactor(model, sharedK, params)); // new SmartFactorP(model, sharedK, params));
// smartFactor1->add(measurements_cam1, views); // smartFactor1->add(measurements_cam1, views);
// //
// SmartFactor::shared_ptr smartFactor2( // SmartFactorP::shared_ptr smartFactor2(
// new SmartFactor(model, sharedK, params)); // new SmartFactorP(model, sharedK, params));
// smartFactor2->add(measurements_cam2, views); // smartFactor2->add(measurements_cam2, views);
// //
// SmartFactor::shared_ptr smartFactor3( // SmartFactorP::shared_ptr smartFactor3(
// new SmartFactor(model, sharedK, params)); // new SmartFactorP(model, sharedK, params));
// smartFactor3->add(measurements_cam3, views); // smartFactor3->add(measurements_cam3, views);
// //
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -1049,7 +1038,7 @@ LevenbergMarquardtParams lmParams;
//} //}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionPoseFactor, Hessian ) { //TEST( SmartProjectionFactorP, Hessian ) {
// //
// using namespace vanillaPose2; // using namespace vanillaPose2;
// //
@ -1062,7 +1051,7 @@ LevenbergMarquardtParams lmParams;
// measurements_cam1.push_back(cam1_uv1); // measurements_cam1.push_back(cam1_uv1);
// measurements_cam1.push_back(cam2_uv1); // measurements_cam1.push_back(cam2_uv1);
// //
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); // SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, sharedK2));
// smartFactor1->add(measurements_cam1, views); // smartFactor1->add(measurements_cam1, views);
// //
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
@ -1080,8 +1069,8 @@ LevenbergMarquardtParams lmParams;
//} //}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionPoseFactor, HessianWithRotation ) { //TEST( SmartProjectionFactorP, HessianWithRotation ) {
// // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl; // // cout << " ************************ SmartProjectionFactorP: rotated Hessian **********************" << endl;
// //
// using namespace vanillaPose; // using namespace vanillaPose;
// //
@ -1091,7 +1080,7 @@ LevenbergMarquardtParams lmParams;
// //
// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); // projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
// //
// SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(model, sharedK)); // SmartFactorP::shared_ptr smartFactorInstance(new SmartFactorP(model, sharedK));
// smartFactorInstance->add(measurements_cam1, views); // smartFactorInstance->add(measurements_cam1, views);
// //
// Values values; // Values values;
@ -1131,7 +1120,7 @@ LevenbergMarquardtParams lmParams;
//} //}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { //TEST( SmartProjectionFactorP, HessianWithRotationDegenerate ) {
// //
// using namespace vanillaPose2; // using namespace vanillaPose2;
// //
@ -1144,7 +1133,7 @@ LevenbergMarquardtParams lmParams;
// Point2Vector measurements_cam1; // Point2Vector measurements_cam1;
// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); // projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
// //
// SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2)); // SmartFactorP::shared_ptr smartFactor(new SmartFactorP(model, sharedK2));
// smartFactor->add(measurements_cam1, views); // smartFactor->add(measurements_cam1, views);
// //
// Values values; // Values values;
@ -1183,16 +1172,16 @@ LevenbergMarquardtParams lmParams;
//} //}
// //
///* ************************************************************************* */ ///* ************************************************************************* */
//TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { //TEST( SmartProjectionFactorP, ConstructorWithCal3Bundler) {
// using namespace bundlerPose; // using namespace bundlerPose;
// SmartProjectionParams params; // SmartProjectionParams params;
// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); // params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
// SmartFactor factor(model, sharedBundlerK, params); // SmartFactorP factor(model, sharedBundlerK, params);
// factor.add(measurement1, x1); // factor.add(measurement1, x1);
//} //}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionPoseFactor, Cal3Bundler ) { //TEST( SmartProjectionFactorP, Cal3Bundler ) {
// //
// using namespace bundlerPose; // using namespace bundlerPose;
// //
@ -1208,13 +1197,13 @@ LevenbergMarquardtParams lmParams;
// //
// KeyVector views {x1, x2, x3}; // KeyVector views {x1, x2, x3};
// //
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK)); // SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, sharedBundlerK));
// smartFactor1->add(measurements_cam1, views); // smartFactor1->add(measurements_cam1, views);
// //
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedBundlerK)); // SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, sharedBundlerK));
// smartFactor2->add(measurements_cam2, views); // smartFactor2->add(measurements_cam2, views);
// //
// SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedBundlerK)); // SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, sharedBundlerK));
// smartFactor3->add(measurements_cam3, views); // smartFactor3->add(measurements_cam3, views);
// //
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -1248,7 +1237,7 @@ LevenbergMarquardtParams lmParams;
//} //}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { //TEST( SmartProjectionFactorP, Cal3BundlerRotationOnly ) {
// //
// using namespace bundlerPose; // using namespace bundlerPose;
// //
@ -1275,16 +1264,16 @@ LevenbergMarquardtParams lmParams;
// params.setRankTolerance(10); // params.setRankTolerance(10);
// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); // params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
// //
// SmartFactor::shared_ptr smartFactor1( // SmartFactorP::shared_ptr smartFactor1(
// new SmartFactor(model, sharedBundlerK, params)); // new SmartFactorP(model, sharedBundlerK, params));
// smartFactor1->add(measurements_cam1, views); // smartFactor1->add(measurements_cam1, views);
// //
// SmartFactor::shared_ptr smartFactor2( // SmartFactorP::shared_ptr smartFactor2(
// new SmartFactor(model, sharedBundlerK, params)); // new SmartFactorP(model, sharedBundlerK, params));
// smartFactor2->add(measurements_cam2, views); // smartFactor2->add(measurements_cam2, views);
// //
// SmartFactor::shared_ptr smartFactor3( // SmartFactorP::shared_ptr smartFactor3(
// new SmartFactor(model, sharedBundlerK, params)); // new SmartFactorP(model, sharedBundlerK, params));
// smartFactor3->add(measurements_cam3, views); // smartFactor3->add(measurements_cam3, views);
// //
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -1340,25 +1329,25 @@ LevenbergMarquardtParams lmParams;
//BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); //BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
//BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); //BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
// //
//TEST(SmartProjectionPoseFactor, serialize) { //TEST(SmartProjectionFactorP, serialize) {
// using namespace vanillaPose; // using namespace vanillaPose;
// using namespace gtsam::serializationTestHelpers; // using namespace gtsam::serializationTestHelpers;
// SmartProjectionParams params; // SmartProjectionParams params;
// params.setRankTolerance(rankTol); // params.setRankTolerance(rankTol);
// SmartFactor factor(model, sharedK, params); // SmartFactorP factor(model, sharedK, params);
// //
// EXPECT(equalsObj(factor)); // EXPECT(equalsObj(factor));
// EXPECT(equalsXML(factor)); // EXPECT(equalsXML(factor));
// EXPECT(equalsBinary(factor)); // EXPECT(equalsBinary(factor));
//} //}
// //
//TEST(SmartProjectionPoseFactor, serialize2) { //TEST(SmartProjectionFactorP, serialize2) {
// using namespace vanillaPose; // using namespace vanillaPose;
// using namespace gtsam::serializationTestHelpers; // using namespace gtsam::serializationTestHelpers;
// SmartProjectionParams params; // SmartProjectionParams params;
// params.setRankTolerance(rankTol); // params.setRankTolerance(rankTol);
// Pose3 bts; // Pose3 bts;
// SmartFactor factor(model, sharedK, bts, params); // SmartFactorP factor(model, sharedK, bts, params);
// //
// // insert some measurments // // insert some measurments
// KeyVector key_view; // KeyVector key_view;