diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionFactorP.cpp index 9ce3e56ac..a6a90dbbc 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactorP.cpp @@ -106,7 +106,7 @@ TEST( SmartProjectionFactorP, noiseless ) { factor.add(level_uv, x1, sharedK); factor.add(level_uv_right, x2, sharedK); - Values values; // it's a pose factor, hence these are poses + Values values; // it's a pose factor, hence these are poses values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); @@ -119,8 +119,9 @@ TEST( SmartProjectionFactorP, noiseless ) { EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); // Calculate expected derivative for point (easiest to check) - std::function f = // - std::bind(&SmartFactorP::whitenedError, factor, cameras, std::placeholders::_1); + std::function f = // + std::bind(&SmartFactorP::whitenedError, factor, cameras, + std::placeholders::_1); // Calculate using computeEP Matrix actualE; @@ -164,7 +165,7 @@ TEST( SmartProjectionFactorP, noisy ) { Values values; values.insert(x1, cam1.pose()); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), - Point3(0.5, 0.1, 0.3)); + Point3(0.5, 0.1, 0.3)); values.insert(x2, pose_right.compose(noise_pose)); SmartFactorP::shared_ptr factor(new SmartFactorP(model)); @@ -178,11 +179,11 @@ TEST( SmartProjectionFactorP, noisy ) { measurements.push_back(level_uv); measurements.push_back(level_uv_right); - std::vector> sharedKs; + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); - KeyVector views {x1, x2}; + KeyVector views { x1, x2 }; factor2->add(measurements, views, sharedKs); double actualError2 = factor2->error(values); @@ -194,7 +195,8 @@ TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { using namespace vanillaPose; // create arbitrary body_T_sensor (transforms from sensor to body) - Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); + Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(1, 1, 1)); // These are the poses we want to estimate, from camera measurements const Pose3 sensor_T_body = body_T_sensor.inverse(); @@ -213,14 +215,14 @@ TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // Create smart factors - KeyVector views {x1, x2, x3}; + KeyVector views { x1, x2, x3 }; SmartProjectionParams params; params.setRankTolerance(1.0); params.setDegeneracyMode(IGNORE_DEGENERACY); params.setEnableEPI(false); - std::vector> sharedKs; + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); @@ -237,7 +239,8 @@ TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { smartFactor2.add(measurements_cam2, views, sharedKs, body_T_sensors); SmartFactorP smartFactor3(model, params); - smartFactor3.add(measurements_cam3, views, sharedKs, body_T_sensors);; + smartFactor3.add(measurements_cam3, views, sharedKs, body_T_sensors); + ; const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -291,7 +294,7 @@ TEST( SmartProjectionFactorP, 3poses_smart_projection_factor ) { views.push_back(x2); views.push_back(x3); - std::vector> sharedK2s; + std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s; sharedK2s.push_back(sharedK2); sharedK2s.push_back(sharedK2); sharedK2s.push_back(sharedK2); @@ -322,7 +325,7 @@ TEST( SmartProjectionFactorP, 3poses_smart_projection_factor ) { // 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 + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); @@ -332,8 +335,9 @@ TEST( SmartProjectionFactorP, 3poses_smart_projection_factor ) { 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))); + -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); @@ -362,13 +366,14 @@ TEST( SmartProjectionFactorP, Factors ) { measurements_cam1.push_back(cam2.project(landmark1)); // Create smart factors - KeyVector views {x1, x2}; + KeyVector views { x1, x2 }; - std::vector> sharedKs; + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); - SmartFactorP::shared_ptr smartFactor1 = boost::make_shared(model); + SmartFactorP::shared_ptr smartFactor1 = boost::make_shared < SmartFactorP + > (model); smartFactor1->add(measurements_cam1, views, sharedKs); SmartFactorP::Cameras cameras; @@ -401,7 +406,7 @@ TEST( SmartProjectionFactorP, Factors ) { A2 << 10, 0, 1, 0, -1, 0; A1 *= 10. / sigma; A2 *= 10. / sigma; - Matrix expectedInformation; // filled below + Matrix expectedInformation; // filled below { // createHessianFactor Matrix66 G11 = 0.5 * A1.transpose() * A1; @@ -422,8 +427,8 @@ TEST( SmartProjectionFactorP, Factors ) { values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); - boost::shared_ptr > actual = - smartFactor1->createHessianFactor(values, 0.0); + boost::shared_ptr < RegularHessianFactor<6> > 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); @@ -436,7 +441,7 @@ TEST( SmartProjectionFactorP, 3poses_iterative_smart_projection_factor ) { using namespace vanillaPose; - KeyVector views {x1, x2, x3}; + KeyVector views { x1, x2, x3 }; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -445,7 +450,7 @@ TEST( SmartProjectionFactorP, 3poses_iterative_smart_projection_factor ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - std::vector> sharedKs; + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); @@ -470,7 +475,7 @@ TEST( SmartProjectionFactorP, 3poses_iterative_smart_projection_factor ) { // 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 + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); @@ -480,8 +485,9 @@ TEST( SmartProjectionFactorP, 3poses_iterative_smart_projection_factor ) { 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)), + -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, + -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); Values result; @@ -497,7 +503,7 @@ TEST( SmartProjectionFactorP, landmarkDistance ) { double excludeLandmarksFutherThanDist = 2; - KeyVector views {x1, x2, x3}; + KeyVector views { x1, x2, x3 }; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -506,7 +512,7 @@ TEST( SmartProjectionFactorP, landmarkDistance ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - std::vector> sharedKs; + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); @@ -518,16 +524,13 @@ TEST( SmartProjectionFactorP, landmarkDistance ) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - SmartFactorP::shared_ptr smartFactor1( - new SmartFactorP(model, params)); + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); smartFactor1->add(measurements_cam1, views, sharedKs); - SmartFactorP::shared_ptr smartFactor2( - new SmartFactorP(model, params)); + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); smartFactor2->add(measurements_cam2, views, sharedKs); - SmartFactorP::shared_ptr smartFactor3( - new SmartFactorP(model, params)); + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); smartFactor3->add(measurements_cam3, views, sharedKs); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -541,7 +544,7 @@ TEST( SmartProjectionFactorP, landmarkDistance ) { // 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 + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); @@ -560,11 +563,11 @@ TEST( SmartProjectionFactorP, dynamicOutlierRejection ) { using namespace vanillaPose; double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - KeyVector views {x1, x2, x3}; + KeyVector views { x1, x2, x3 }; - std::vector> sharedKs; + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); @@ -580,7 +583,7 @@ TEST( SmartProjectionFactorP, dynamicOutlierRejection ) { 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 + measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier SmartProjectionParams params; params.setLinearizationMode(gtsam::HESSIAN); @@ -588,20 +591,16 @@ TEST( SmartProjectionFactorP, dynamicOutlierRejection ) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); - SmartFactorP::shared_ptr smartFactor1( - new SmartFactorP(model, params)); + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); smartFactor1->add(measurements_cam1, views, sharedKs); - SmartFactorP::shared_ptr smartFactor2( - new SmartFactorP(model, params)); + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); smartFactor2->add(measurements_cam2, views, sharedKs); - SmartFactorP::shared_ptr smartFactor3( - new SmartFactorP(model, params)); + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); smartFactor3->add(measurements_cam3, views, sharedKs); - SmartFactorP::shared_ptr smartFactor4( - new SmartFactorP(model, params)); + SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, params)); smartFactor4->add(measurements_cam4, views, sharedKs); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -629,7 +628,7 @@ TEST( SmartProjectionFactorP, dynamicOutlierRejection ) { /* *************************************************************************/ TEST( SmartProjectionFactorP, CheckHessian) { - KeyVector views {x1, x2, x3}; + KeyVector views { x1, x2, x3 }; using namespace vanillaPose; @@ -647,7 +646,7 @@ TEST( SmartProjectionFactorP, CheckHessian) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - std::vector> sharedKs; + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); @@ -656,16 +655,13 @@ TEST( SmartProjectionFactorP, CheckHessian) { params.setRankTolerance(10); params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - SmartFactorP::shared_ptr smartFactor1( - new SmartFactorP(model, params)); // HESSIAN, by default + 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 + 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 + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); // HESSIAN, by default smartFactor3->add(measurements_cam3, views, sharedKs); NonlinearFactorGraph graph; @@ -675,7 +671,7 @@ TEST( SmartProjectionFactorP, CheckHessian) { // 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 + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); @@ -685,8 +681,8 @@ TEST( SmartProjectionFactorP, CheckHessian) { assert_equal( Pose3( Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, - -0.130426831, -0.0115837907, 0.130819108, -0.98278564, - -0.130455917), + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), Point3(0.0897734171, -0.110201006, 0.901022872)), values.at(x3))); @@ -708,7 +704,7 @@ TEST( SmartProjectionFactorP, CheckHessian) { + factor2->augmentedInformation() + factor3->augmentedInformation(); // Check Information vector - Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + 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)); @@ -719,7 +715,7 @@ TEST( SmartProjectionFactorP, Hessian ) { using namespace vanillaPose2; - KeyVector views {x1, x2}; + KeyVector views { x1, x2 }; // Project three landmarks into 2 cameras Point2 cam1_uv1 = cam1.project(landmark1); @@ -728,7 +724,7 @@ TEST( SmartProjectionFactorP, Hessian ) { measurements_cam1.push_back(cam1_uv1); measurements_cam1.push_back(cam2_uv1); - std::vector> sharedK2s; + std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s; sharedK2s.push_back(sharedK2); sharedK2s.push_back(sharedK2); @@ -736,7 +732,7 @@ TEST( SmartProjectionFactorP, Hessian ) { 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)); + Point3(0.5, 0.1, 0.3)); Values values; values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); @@ -773,9 +769,9 @@ TEST( SmartProjectionFactorP, Cal3Bundler ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - KeyVector views {x1, x2, x3}; + KeyVector views { x1, x2, x3 }; - std::vector> sharedBundlerKs; + std::vector < boost::shared_ptr < Cal3Bundler >> sharedBundlerKs; sharedBundlerKs.push_back(sharedBundlerK); sharedBundlerKs.push_back(sharedBundlerK); sharedBundlerKs.push_back(sharedBundlerK); @@ -800,7 +796,7 @@ TEST( SmartProjectionFactorP, Cal3Bundler ) { // 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 + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); @@ -810,8 +806,9 @@ TEST( SmartProjectionFactorP, Cal3Bundler ) { 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))); + -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); @@ -819,6 +816,279 @@ TEST( SmartProjectionFactorP, Cal3Bundler ) { EXPECT(assert_equal(cam3.pose(), result.at(x3), 1e-6)); } +#include +typedef GenericProjectionFactor TestProjectionFactor; +static Symbol l0('L', 0); +/* *************************************************************************/ +TEST( SmartProjectionFactorP, hessianComparedToProjFactors_measurementsFromSamePose) { + // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark + // at a single pose, a setup that occurs in multi-camera systems + + using namespace vanillaPose; + Point2Vector measurements_lmk1; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + + // create redundant measurements: + Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; + measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement + + // create inputs + std::vector keys; + keys.push_back(x1); + keys.push_back(x2); + keys.push_back(x3); + keys.push_back(x1); + + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); + smartFactor1->add(measurements_lmk1_redundant, keys, sharedKs); + + 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 to get a nontrivial linearization point + 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))); + + // linearization point for the poses + Pose3 pose1 = level_pose; + Pose3 pose2 = pose_right; + Pose3 pose3 = pose_above * noise_pose; + + // ==== check Hessian of smartFactor1 ===== + // -- compute actual Hessian + boost::shared_ptr linearfactor1 = smartFactor1->linearize( + values); + Matrix actualHessian = linearfactor1->information(); + + // -- compute expected Hessian from manual Schur complement from Jacobians + // linearization point for the 3D point + smartFactor1->triangulateSafe(smartFactor1->cameras(values)); + TriangulationResult point = smartFactor1->point(); + EXPECT(point.valid()); // check triangulated point is valid + + // Use standard ProjectionFactor factor to calculate the Jacobians + Matrix F = Matrix::Zero(2 * 4, 6 * 3); + Matrix E = Matrix::Zero(2 * 4, 3); + Vector b = Vector::Zero(2 * 4); + + // create projection factors rolling shutter + TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0, + sharedK); + Matrix HPoseActual, HEActual; + // note: b is minus the reprojection error, cf the smart factor jacobian computation + b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual, + HEActual); + F.block<2, 6>(0, 0) = HPoseActual; + E.block<2, 3>(0, 0) = HEActual; + + TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0, + sharedK); + b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual, + HEActual); + F.block<2, 6>(2, 6) = HPoseActual; + E.block<2, 3>(2, 0) = HEActual; + + TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0, + sharedK); + b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual, + HEActual); + F.block<2, 6>(4, 12) = HPoseActual; + E.block<2, 3>(4, 0) = HEActual; + + TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0, + sharedK); + b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual, + HEActual); + F.block<2, 6>(6, 0) = HPoseActual; + E.block<2, 3>(6, 0) = HEActual; + + // whiten + F = (1 / sigma) * F; + E = (1 / sigma) * E; + b = (1 / sigma) * b; + //* G = F' * F - F' * E * P * E' * F + Matrix P = (E.transpose() * E).inverse(); + Matrix expectedHessian = F.transpose() * F + - (F.transpose() * E * P * E.transpose() * F); + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); + + // ==== check Information vector of smartFactor1 ===== + GaussianFactorGraph gfg; + gfg.add(linearfactor1); + Matrix actualHessian_v2 = gfg.hessian().first; + EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian + + // -- compute actual information vector + Vector actualInfoVector = gfg.hessian().second; + + // -- compute expected information vector from manual Schur complement from Jacobians + //* g = F' * (b - E * P * E' * b) + Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); + EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); + + // ==== check error of smartFactor1 (again) ===== + NonlinearFactorGraph nfg_projFactors; + nfg_projFactors.add(factor11); + nfg_projFactors.add(factor12); + nfg_projFactors.add(factor13); + nfg_projFactors.add(factor14); + values.insert(l0, *point); + + double actualError = smartFactor1->error(values); + double expectedError = nfg_projFactors.error(values); + 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)); +//} + +//#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 + /* ************************************************************************* */ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");