diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionFactorP.cpp index 554ce7873..7dd06c18e 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactorP.cpp @@ -1133,22 +1133,16 @@ TEST( SmartProjectionFactorP, optimization_3poses_sphericalCamera ) { 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 + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.5, 0.5)); // large noise - still works! + 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))); - DOUBLES_EQUAL(0.15734109864597129, graph.error(values), 1e-9); + DOUBLES_EQUAL(20.37690384646076, graph.error(values), 1e-9); Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); @@ -1218,6 +1212,153 @@ TEST( SmartProjectionFactorP, timing_sphericalCamera ) { } #endif +/* *************************************************************************/ +TEST( SmartProjectionFactorP, 2poses_rankTol ) { + Pose3 poseA = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0.0,0.0,0.0)); // with z pointing along x axis of global frame + Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0.0,-0.1,0.0)); // 10cm to the right of poseA + Point3 landmarkL = Point3(5.0,0.0,0.0); // 5m in front of poseA + + // triangulate from a stereo with 10cm baseline, assuming standard calibration + {// default rankTol = 1 gives a valid point (compare with calibrated and spherical cameras below) + using namespace vanillaPose; // pinhole with Cal3_S2 calibration + + Camera cam1(poseA,sharedK); + Camera cam2(poseB,sharedK); + + SmartProjectionParams params; + params.setRankTolerance(1); + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model,params)); + smartFactor1->add(cam1.project(landmarkL), x1, sharedK); + smartFactor1->add(cam2.project(landmarkL), x2, sharedK); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + + Values groundTruth; + groundTruth.insert(x1, poseA); + groundTruth.insert(x2, poseB); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // get point + TriangulationResult point = smartFactor1->point(); + EXPECT(point.valid()); // valid triangulation + EXPECT(assert_equal(landmarkL, *point, 1e-7)); + } + // triangulate from a stereo with 10cm baseline, assuming canonical calibration + {// default rankTol = 1 or 0.1 gives a degenerate point, which is undesirable for a point 5m away and 10cm baseline + using namespace vanillaPose; // pinhole with Cal3_S2 calibration + static Cal3_S2::shared_ptr canonicalK(new Cal3_S2(1.0,1.0,0.0,0.0,0.0)); //canonical camera + + Camera cam1(poseA,canonicalK); + Camera cam2(poseB,canonicalK); + + SmartProjectionParams params; + params.setRankTolerance(0.1); + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model,params)); + smartFactor1->add(cam1.project(landmarkL), x1, canonicalK); + smartFactor1->add(cam2.project(landmarkL), x2, canonicalK); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + + Values groundTruth; + groundTruth.insert(x1, poseA); + groundTruth.insert(x2, poseB); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // get point + TriangulationResult point = smartFactor1->point(); + EXPECT(point.degenerate()); // valid triangulation + } + // triangulate from a stereo with 10cm baseline, assuming canonical calibration + {// smaller rankTol = 0.01 gives a valid point (compare with calibrated and spherical cameras below) + using namespace vanillaPose; // pinhole with Cal3_S2 calibration + static Cal3_S2::shared_ptr canonicalK(new Cal3_S2(1.0,1.0,0.0,0.0,0.0)); //canonical camera + + Camera cam1(poseA,canonicalK); + Camera cam2(poseB,canonicalK); + + SmartProjectionParams params; + params.setRankTolerance(0.01); + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model,params)); + smartFactor1->add(cam1.project(landmarkL), x1, canonicalK); + smartFactor1->add(cam2.project(landmarkL), x2, canonicalK); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + + Values groundTruth; + groundTruth.insert(x1, poseA); + groundTruth.insert(x2, poseB); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // get point + TriangulationResult point = smartFactor1->point(); + EXPECT(point.valid()); // valid triangulation + EXPECT(assert_equal(landmarkL, *point, 1e-7)); + } +} + +/* *************************************************************************/ +TEST( SmartProjectionFactorP, 2poses_sphericalCamera_rankTol ) { + typedef SphericalCamera Camera; + typedef SmartProjectionFactorP SmartFactorP; + static EmptyCal::shared_ptr emptyK; + Pose3 poseA = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0.0,0.0,0.0)); // with z pointing along x axis of global frame + Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0.0,-0.1,0.0)); // 10cm to the right of poseA + Point3 landmarkL = Point3(5.0,0.0,0.0); // 5m in front of poseA + + Camera cam1(poseA); + Camera cam2(poseB); + + // TRIANGULATION TEST WITH DEFAULT RANK TOL + {// rankTol = 1 or 0.1 gives a degenerate point, which is undesirable for a point 5m away and 10cm baseline + SmartProjectionParams params; + params.setRankTolerance(0.1); + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model,params)); + smartFactor1->add(cam1.project(landmarkL), x1, emptyK); + smartFactor1->add(cam2.project(landmarkL), x2, emptyK); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + + Values groundTruth; + groundTruth.insert(x1, poseA); + groundTruth.insert(x2, poseB); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // get point + TriangulationResult point = smartFactor1->point(); + EXPECT(point.degenerate()); // not enough parallax + } + // SAME TEST WITH SMALLER RANK TOL + {// rankTol = 0.01 gives a valid point + SmartProjectionParams params; + params.setRankTolerance(0.01); + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model,params)); + smartFactor1->add(cam1.project(landmarkL), x1, emptyK); + smartFactor1->add(cam2.project(landmarkL), x2, emptyK); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + + Values groundTruth; + groundTruth.insert(x1, poseA); + groundTruth.insert(x2, poseB); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // get point + TriangulationResult point = smartFactor1->point(); + EXPECT(point.valid()); // valid triangulation + EXPECT(assert_equal(landmarkL, *point, 1e-7)); + } +} + /* ************************************************************************* */ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");