From 7d80f52b1247204d910cdd07c91be79c68749dd2 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 26 Aug 2021 13:31:09 -0400 Subject: [PATCH] done with all tests --- gtsam/slam/SmartProjectionFactorP.h | 3 + .../slam/tests/testSmartProjectionFactorP.cpp | 271 +++++++++--------- 2 files changed, 137 insertions(+), 137 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index 559a294ac..929ec41f7 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -90,6 +90,9 @@ class SmartProjectionFactorP : public SmartProjectionFactor { const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params) { + // use only configuration that works with this factor + Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; + Base::params_.linearizationMode = gtsam::HESSIAN; } /** Virtual destructor */ diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionFactorP.cpp index a6a90dbbc..4591dc08e 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactorP.cpp @@ -950,144 +950,141 @@ TEST( SmartProjectionFactorP, hessianComparedToProjFactors_measurementsFromSameP EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); } -///* *************************************************************************/ -//TEST( SmartProjectionFactorP, optimization_3poses_measurementsFromSamePose ) { -// -// using namespace vanillaPoseRS; -// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); -// -// // create inputs -// std::vector> key_pairs; -// key_pairs.push_back(std::make_pair(x1,x2)); -// key_pairs.push_back(std::make_pair(x2,x3)); -// key_pairs.push_back(std::make_pair(x3,x1)); -// -// std::vector interp_factors; -// interp_factors.push_back(interp_factor1); -// interp_factors.push_back(interp_factor2); -// interp_factors.push_back(interp_factor3); -// -// // For first factor, we create redundant measurement (taken by the same keys as factor 1, to -// // make sure the redundancy in the keys does not create problems) -// Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; -// measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement -// std::vector> key_pairs_redundant = key_pairs; -// key_pairs_redundant.push_back(key_pairs.at(0)); // we readd the first pair of keys -// std::vector interp_factors_redundant = interp_factors; -// interp_factors_redundant.push_back(interp_factors.at(0));// we readd the first interp factor -// -// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); -// smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant, sharedK); -// -// SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); -// smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); -// smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); -// -// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); -// -// NonlinearFactorGraph graph; -// graph.push_back(smartFactor1); -// graph.push_back(smartFactor2); -// graph.push_back(smartFactor3); -// graph.addPrior(x1, level_pose, noisePrior); -// graph.addPrior(x2, pose_right, noisePrior); -// -// Values groundTruth; -// groundTruth.insert(x1, level_pose); -// groundTruth.insert(x2, pose_right); -// groundTruth.insert(x3, pose_above); -// DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); -// -// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below -// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.1, 0.1, 0.1)); // smaller noise -// Values values; -// values.insert(x1, level_pose); -// values.insert(x2, pose_right); -// // initialize third pose with some noise, we expect it to move back to original pose_above -// values.insert(x3, pose_above * noise_pose); -// EXPECT( // check that the pose is actually noisy -// assert_equal( -// Pose3( -// Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, -// -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), -// Point3(0.1, -0.1, 1.9)), values.at(x3))); -// -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// EXPECT(assert_equal(pose_above, result.at(x3), 1e-5)); -//} +/* *************************************************************************/ +TEST( SmartProjectionFactorP, optimization_3poses_measurementsFromSamePose ) { -//#ifndef DISABLE_TIMING -//#include -//// -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) -////| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: 0 max: 0) -////| -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 children, min: 0 max: 0) -///* *************************************************************************/ -//TEST( SmartProjectionPoseFactorRollingShutter, timing ) { -// -// using namespace vanillaPose; -// -// // Default cameras for simple derivatives -// static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); -// -// Rot3 R = Rot3::identity(); -// Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); -// Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); -// Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); -// Pose3 body_P_sensorId = Pose3::identity(); -// -// // one landmarks 1m in front of camera -// Point3 landmark1(0, 0, 10); -// -// Point2Vector measurements_lmk1; -// -// // Project 2 landmarks into 2 cameras -// measurements_lmk1.push_back(cam1.project(landmark1)); -// measurements_lmk1.push_back(cam2.project(landmark1)); -// -// size_t nrTests = 1000; -// -// for(size_t i = 0; iadd(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, -// body_P_sensorId); -// interp_factor = 1; // equivalent to measurement taken at pose 2 -// smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, -// body_P_sensorId); -// -// Values values; -// values.insert(x1, pose1); -// values.insert(x2, pose2); -// gttic_(SF_RS_LINEARIZE); -// smartFactorRS->linearize(values); -// gttoc_(SF_RS_LINEARIZE); -// } -// -// for(size_t i = 0; iadd(measurements_lmk1[0], x1); -// smartFactor->add(measurements_lmk1[1], x2); -// -// Values values; -// values.insert(x1, pose1); -// values.insert(x2, pose2); -// gttic_(RS_LINEARIZE); -// smartFactor->linearize(values); -// gttoc_(RS_LINEARIZE); -// } -// tictoc_print_(); -//} -//#endif + using namespace vanillaPose; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector keys; + keys.push_back(x1); + keys.push_back(x2); + keys.push_back(x3); + + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + // For first factor, we create redundant measurement (taken by the same keys as factor 1, to + // make sure the redundancy in the keys does not create problems) + Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; + measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement + std::vector keys_redundant = keys; + keys_redundant.push_back(keys.at(0)); // we readd the first key + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs_redundant = sharedKs; + sharedKs_redundant.push_back(sharedK);// we readd the first calibration + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); + smartFactor1->add(measurements_lmk1_redundant, keys_redundant, sharedKs_redundant); + + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); + smartFactor2->add(measurements_lmk2, keys, sharedKs); + + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); + smartFactor3->add(measurements_lmk3, keys, 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, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Values groundTruth; + groundTruth.insert(x1, level_pose); + groundTruth.insert(x2, pose_right); + groundTruth.insert(x3, pose_above); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-5)); +} + +#ifndef DISABLE_TIMING +#include +// this factor is actually slightly faster (but comparable) to original SmartProjectionPoseFactor +//-Total: 0 CPU (0 times, 0 wall, 0.01 children, min: 0 max: 0) +//| -SmartFactorP LINEARIZE: 0 CPU (1000 times, 0.005481 wall, 0 children, min: 0 max: 0) +//| -SmartPoseFactor LINEARIZE: 0.01 CPU (1000 times, 0.007318 wall, 0.01 children, min: 0 max: 0) +/* *************************************************************************/ +TEST( SmartProjectionFactorP, timing ) { + + using namespace vanillaPose; + + // Default cameras for simple derivatives + static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); + + Rot3 R = Rot3::identity(); + Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); + Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); + Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); + Pose3 body_P_sensorId = Pose3::identity(); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + Point2Vector measurements_lmk1; + + // Project 2 landmarks into 2 cameras + measurements_lmk1.push_back(cam1.project(landmark1)); + measurements_lmk1.push_back(cam2.project(landmark1)); + + size_t nrTests = 1000; + + for(size_t i = 0; iadd(measurements_lmk1[0], x1, sharedKSimple, body_P_sensorId); + smartFactorP->add(measurements_lmk1[1], x1, sharedKSimple, body_P_sensorId); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + gttic_(SmartFactorP_LINEARIZE); + smartFactorP->linearize(values); + gttoc_(SmartFactorP_LINEARIZE); + } + + for(size_t i = 0; iadd(measurements_lmk1[0], x1); + smartFactor->add(measurements_lmk1[1], x2); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + gttic_(SmartPoseFactor_LINEARIZE); + smartFactor->linearize(values); + gttoc_(SmartPoseFactor_LINEARIZE); + } + tictoc_print_(); +} +#endif /* ************************************************************************* */ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");