diff --git a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp index b5ee3d61a..2659cb274 100644 --- a/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -32,7 +32,7 @@ using namespace std; using namespace boost::assign; using namespace gtsam; -static bool isDebugTest = false; +static bool isDebugTest = true; // make a realistic calibration matrix static double fov = 60; // degrees @@ -65,9 +65,11 @@ static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0 typedef SmartStereoProjectionPoseFactor SmartFactor; typedef SmartStereoProjectionPoseFactor SmartFactorBundler; -void stereo_projectToMultipleCameras( - StereoCamera cam1, StereoCamera cam2, StereoCamera cam3, Point3 landmark, - vector& measurements_cam){ +vector stereo_projectToMultipleCameras( + const StereoCamera& cam1, const StereoCamera& cam2, + const StereoCamera& cam3, Point3 landmark){ + + vector measurements_cam; StereoPoint2 cam1_uv1 = cam1.project(landmark); StereoPoint2 cam2_uv1 = cam2.project(landmark); @@ -75,6 +77,8 @@ void stereo_projectToMultipleCameras( measurements_cam.push_back(cam1_uv1); measurements_cam.push_back(cam2_uv1); measurements_cam.push_back(cam3_uv1); + + return measurements_cam; } /* ************************************************************************* */ @@ -217,7 +221,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ){ /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ - // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; + cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl; // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); @@ -236,12 +240,10 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){ Point3 landmark2(5, -0.5, 1.2); Point3 landmark3(3, 0, 3.0); - vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); std::vector views; views.push_back(x1); @@ -321,12 +323,10 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_iterative_smart_projection_factor Point3 landmark2(5, -0.5, 1.2); Point3 landmark3(3, 0, 3.0); - vector measurements_cam1, measurements_cam2, measurements_cam3; - // 1. Project three landmarks into three cameras and triangulate - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); SmartFactor::shared_ptr smartFactor1(new SmartFactor()); smartFactor1->add(measurements_cam1, views, model, K); @@ -372,747 +372,745 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_iterative_smart_projection_factor if(isDebugTest) tictoc_print_(); } -/* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - StereoCamera cam1(pose1, K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - StereoCamera cam2(pose2, K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - StereoCamera cam3(pose3, K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); - smartFactor1->add(measurements_cam1, views, model, K); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); - smartFactor2->add(measurements_cam2, views, model, K); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); - smartFactor3->add(measurements_cam3, views, model, K); - - 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(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); - - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); - - LevenbergMarquardtParams params; - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); -} - -/* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ - - double excludeLandmarksFutherThanDist = 2; - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - StereoCamera cam1(pose1, K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - StereoCamera cam2(pose2, K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - StereoCamera cam3(pose3, K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); - smartFactor1->add(measurements_cam1, views, model, K); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); - smartFactor2->add(measurements_cam2, views, model, K); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); - smartFactor3->add(measurements_cam3, views, model, K); - - 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(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); - - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); - - // All factors are disabled and pose should remain where it is - LevenbergMarquardtParams params; - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - EXPECT(assert_equal(values.at(x3),result.at(x3))); -} - -/* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ - - double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - StereoCamera cam1(pose1, K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - StereoCamera cam2(pose2, K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - StereoCamera cam3(pose3, K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - Point3 landmark4(5, -0.5, 1); - - vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; - - // 1. Project three landmarks into three cameras and triangulate - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); - measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10,10,1); // add outlier - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, - JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor1->add(measurements_cam1, views, model, K); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor2->add(measurements_cam2, views, model, K); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor3->add(measurements_cam3, views, model, K); - - SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor4->add(measurements_cam4, views, model, K); - - 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.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); - - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3); - - // All factors are disabled and pose should remain where it is - LevenbergMarquardtParams params; - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); -} - -/* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, jacobianQ ){ - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - StereoCamera cam1(pose1, K); - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - StereoCamera cam2(pose2, K); - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - StereoCamera cam3(pose3, K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); - smartFactor1->add(measurements_cam1, views, model, K); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); - smartFactor2->add(measurements_cam2, views, model, K); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); - smartFactor3->add(measurements_cam3, views, model, K); - - 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(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); - - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3*noise_pose); - - LevenbergMarquardtParams params; - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); -} - -/* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){ - // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - StereoCamera cam1(pose1, K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - StereoCamera cam2(pose2, K2); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - StereoCamera cam3(pose3, K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - - typedef GenericStereoFactor ProjectionFactor; - NonlinearFactorGraph graph; - - // 1. Project three landmarks into three cameras and triangulate - graph.push_back(ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2)); - - graph.push_back(ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2)); - - graph.push_back(ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2)); - graph.push_back(ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2)); - graph.push_back(ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2)); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PriorFactor(x2, pose2, noisePrior)); - - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3* noise_pose); - values.insert(L(1), landmark1); - values.insert(L(2), landmark2); - values.insert(L(3), landmark3); - if(isDebugTest) values.at(x3).print("Pose3 before optimization: "); - - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; - LevenbergMarquardtOptimizer optimizer(graph, values, params); - Values result = optimizer.optimize(); - - if(isDebugTest) result.at(x3).print("Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3))); -} - -/* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, CheckHessian){ - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - StereoCamera cam1(pose1, K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - StereoCamera cam2(pose2, K); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - StereoCamera cam3(pose3, K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - double rankTol = 10; - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); - smartFactor1->add(measurements_cam1, views, model, K); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol)); - smartFactor2->add(measurements_cam2, views, model, K); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); - smartFactor3->add(measurements_cam3, views, model, K); - - 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), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); - - boost::shared_ptr hessianFactor1 = smartFactor1->linearize(values); - boost::shared_ptr hessianFactor2 = smartFactor2->linearize(values); - boost::shared_ptr hessianFactor3 = smartFactor3->linearize(values); - - Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information(); - - boost::shared_ptr GaussianGraph = graph.linearize(values); - Matrix GraphInformation = GaussianGraph->hessian().first; - - // Check Hessian - EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); - - Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + - hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation(); - - // Check Information vector - // cout << AugInformationMatrix.size() << endl; - Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector - - // Check Hessian - EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); -} - -/* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ - // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - StereoCamera cam1(pose1, K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - StereoCamera cam2(pose2, K2); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - StereoCamera cam3(pose3, K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - - double rankTol = 50; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor1->add(measurements_cam1, views, model, K2); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor2->add(measurements_cam2, views, model, K2); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.1,0.1,0.1)); // smaller noise - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2*noise_pose); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); - - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; - - Values result; - gttic_(SmartStereoProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - gttoc_(SmartStereoProjectionPoseFactor); - tictoc_finishedIteration_(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; - // EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); -} - -/* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ - // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - StereoCamera cam1(pose1, K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - StereoCamera cam2(pose2, K); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); - StereoCamera cam3(pose3, K); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - Point3 landmark2(5, -0.5, 1.2); - Point3 landmark3(3, 0, 3.0); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // 1. Project three landmarks into three cameras and triangulate - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - double rankTol = 10; - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor1->add(measurements_cam1, views, model, K); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor2->add(measurements_cam2, views, model, K); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor3->add(measurements_cam3, views, model, K); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = gtsam::Point3(0,0,1); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, pose1, noisePrior)); - graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); - graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); - - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - // initialize third pose with some noise, we expect it to move back to original pose3 - values.insert(x3, pose3*noise_pose); - if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); - - LevenbergMarquardtParams params; - if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; - if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; - - Values result; - gttic_(SmartStereoProjectionPoseFactor); - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - gttoc_(SmartStereoProjectionPoseFactor); - tictoc_finishedIteration_(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; - // EXPECT(assert_equal(pose3,result.at(x3))); - if(isDebugTest) tictoc_print_(); -} - -/* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, Hessian ){ - // cout << " ************************ SmartStereoProjectionPoseFactor: Hessian **********************" << endl; - - std::vector views; - views.push_back(x1); - views.push_back(x2); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - StereoCamera cam1(pose1, K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - StereoCamera cam2(pose2, K2); - - // three landmarks ~5 meters infront of camera - Point3 landmark1(5, 0.5, 1.2); - - // 1. Project three landmarks into three cameras and triangulate - StereoPoint2 cam1_uv1 = cam1.project(landmark1); - StereoPoint2 cam2_uv1 = cam2.project(landmark1); - vector measurements_cam1; - measurements_cam1.push_back(cam1_uv1); - measurements_cam1.push_back(cam2_uv1); - - SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1,views, model, K2); - - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - - boost::shared_ptr hessianFactor = smartFactor1->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); - - // compute triangulation from linearization point - // compute reprojection errors (sum squared) - // compare with hessianFactor.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( SmartStereoProjectionPoseFactor, HessianWithRotation ){ - // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl; - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - StereoCamera cam1(pose1, K); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); - StereoCamera cam2(pose2, K); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); - StereoCamera cam3(pose3, K); - - Point3 landmark1(5, 0.5, 1.2); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - - SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); - smartFactorInstance->add(measurements_cam1, views, model, K); - - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3); - - boost::shared_ptr hessianFactor = smartFactorInstance->linearize(values); - // hessianFactor->print("Hessian factor \n"); - - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); - - Values rotValues; - rotValues.insert(x1, poseDrift.compose(pose1)); - rotValues.insert(x2, poseDrift.compose(pose2)); - rotValues.insert(x3, poseDrift.compose(pose3)); - - boost::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); - // hessianFactorRot->print("Hessian factor \n"); - - // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); - - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); - - Values tranValues; - tranValues.insert(x1, poseDrift2.compose(pose1)); - tranValues.insert(x2, poseDrift2.compose(pose2)); - tranValues.insert(x3, poseDrift2.compose(pose3)); - - boost::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); - - // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); -} - -/* *************************************************************************/ -TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){ - // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; - - std::vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - StereoCamera cam1(pose1, K2); - - // create second camera 1 meter to the right of first camera - Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0)); - StereoCamera cam2(pose2, K2); - - // create third camera 1 meter above the first camera - Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0)); - StereoCamera cam3(pose3, K2); - - Point3 landmark1(5, 0.5, 1.2); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - - SmartFactor::shared_ptr smartFactor(new SmartFactor()); - smartFactor->add(measurements_cam1, views, model, K2); - - - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - values.insert(x3, pose3); - - boost::shared_ptr hessianFactor = smartFactor->linearize(values); - if(isDebugTest) hessianFactor->print("Hessian factor \n"); - - Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); - - Values rotValues; - rotValues.insert(x1, poseDrift.compose(pose1)); - rotValues.insert(x2, poseDrift.compose(pose2)); - rotValues.insert(x3, poseDrift.compose(pose3)); - - boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); - if(isDebugTest) hessianFactorRot->print("Hessian factor \n"); - - // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); - - Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); - - Values tranValues; - tranValues.insert(x1, poseDrift2.compose(pose1)); - tranValues.insert(x2, poseDrift2.compose(pose2)); - tranValues.insert(x3, poseDrift2.compose(pose3)); - - boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); - - // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); -} +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){ +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// StereoCamera cam1(pose1, K); +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// StereoCamera cam2(pose2, K); +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// StereoCamera cam3(pose3, K); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// // 1. Project three landmarks into three cameras and triangulate +// vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1); +// vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2); +// vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3); +// +// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); +// smartFactor1->add(measurements_cam1, views, model, K); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); +// smartFactor2->add(measurements_cam2, views, model, K); +// +// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); +// smartFactor3->add(measurements_cam3, views, model, K); +// +// 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(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PriorFactor(x2, pose2, noisePrior)); +// +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// values.insert(x3, pose3*noise_pose); +// +// LevenbergMarquardtParams params; +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// EXPECT(assert_equal(pose3,result.at(x3))); +//} + +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){ +// +// double excludeLandmarksFutherThanDist = 2; +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// StereoCamera cam1(pose1, K); +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// StereoCamera cam2(pose2, K); +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// StereoCamera cam3(pose3, K); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // 1. Project three landmarks into three cameras and triangulate +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// +// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); +// smartFactor1->add(measurements_cam1, views, model, K); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); +// smartFactor2->add(measurements_cam2, views, model, K); +// +// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); +// smartFactor3->add(measurements_cam3, views, model, K); +// +// 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(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PriorFactor(x2, pose2, noisePrior)); +// +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// values.insert(x3, pose3*noise_pose); +// +// // All factors are disabled and pose should remain where it is +// LevenbergMarquardtParams params; +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// EXPECT(assert_equal(values.at(x3),result.at(x3))); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){ +// +// double excludeLandmarksFutherThanDist = 1e10; +// double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// StereoCamera cam1(pose1, K); +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// StereoCamera cam2(pose2, K); +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// StereoCamera cam3(pose3, K); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// Point3 landmark4(5, -0.5, 1); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; +// +// // 1. Project three landmarks into three cameras and triangulate +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); +// measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10,10,1); // add outlier +// +// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, +// JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); +// smartFactor1->add(measurements_cam1, views, model, K); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, +// excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); +// smartFactor2->add(measurements_cam2, views, model, K); +// +// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, +// excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); +// smartFactor3->add(measurements_cam3, views, model, K); +// +// SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, +// excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); +// smartFactor4->add(measurements_cam4, views, model, K); +// +// 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.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PriorFactor(x2, pose2, noisePrior)); +// +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// values.insert(x3, pose3); +// +// // All factors are disabled and pose should remain where it is +// LevenbergMarquardtParams params; +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// EXPECT(assert_equal(pose3,result.at(x3))); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, jacobianQ ){ +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// StereoCamera cam1(pose1, K); +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// StereoCamera cam2(pose2, K); +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// StereoCamera cam3(pose3, K); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // 1. Project three landmarks into three cameras and triangulate +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// +// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// smartFactor1->add(measurements_cam1, views, model, K); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// smartFactor2->add(measurements_cam2, views, model, K); +// +// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// smartFactor3->add(measurements_cam3, views, model, K); +// +// 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(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PriorFactor(x2, pose2, noisePrior)); +// +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// values.insert(x3, pose3*noise_pose); +// +// LevenbergMarquardtParams params; +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// EXPECT(assert_equal(pose3,result.at(x3))); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){ +// // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// StereoCamera cam1(pose1, K2); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// StereoCamera cam2(pose2, K2); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// StereoCamera cam3(pose3, K2); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// typedef GenericStereoFactor ProjectionFactor; +// NonlinearFactorGraph graph; +// +// // 1. Project three landmarks into three cameras and triangulate +// graph.push_back(ProjectionFactor(cam1.project(landmark1), model, x1, L(1), K2)); +// graph.push_back(ProjectionFactor(cam2.project(landmark1), model, x2, L(1), K2)); +// graph.push_back(ProjectionFactor(cam3.project(landmark1), model, x3, L(1), K2)); +// +// graph.push_back(ProjectionFactor(cam1.project(landmark2), model, x1, L(2), K2)); +// graph.push_back(ProjectionFactor(cam2.project(landmark2), model, x2, L(2), K2)); +// graph.push_back(ProjectionFactor(cam3.project(landmark2), model, x3, L(2), K2)); +// +// graph.push_back(ProjectionFactor(cam1.project(landmark3), model, x1, L(3), K2)); +// graph.push_back(ProjectionFactor(cam2.project(landmark3), model, x2, L(3), K2)); +// graph.push_back(ProjectionFactor(cam3.project(landmark3), model, x3, L(3), K2)); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// graph.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PriorFactor(x2, pose2, noisePrior)); +// +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// values.insert(x3, pose3* noise_pose); +// values.insert(L(1), landmark1); +// values.insert(L(2), landmark2); +// values.insert(L(3), landmark3); +// if(isDebugTest) values.at(x3).print("Pose3 before optimization: "); +// +// LevenbergMarquardtParams params; +// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; +// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// Values result = optimizer.optimize(); +// +// if(isDebugTest) result.at(x3).print("Pose3 after optimization: "); +// EXPECT(assert_equal(pose3,result.at(x3))); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, CheckHessian){ +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// StereoCamera cam1(pose1, K); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); +// StereoCamera cam2(pose2, K); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); +// StereoCamera cam3(pose3, K); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // 1. Project three landmarks into three cameras and triangulate +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// +// double rankTol = 10; +// +// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); +// smartFactor1->add(measurements_cam1, views, model, K); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol)); +// smartFactor2->add(measurements_cam2, views, model, K); +// +// SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); +// smartFactor3->add(measurements_cam3, views, model, K); +// +// 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), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// // initialize third pose with some noise, we expect it to move back to original pose3 +// values.insert(x3, pose3*noise_pose); +// if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); +// +// boost::shared_ptr hessianFactor1 = smartFactor1->linearize(values); +// boost::shared_ptr hessianFactor2 = smartFactor2->linearize(values); +// boost::shared_ptr hessianFactor3 = smartFactor3->linearize(values); +// +// Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information(); +// +// boost::shared_ptr GaussianGraph = graph.linearize(values); +// Matrix GraphInformation = GaussianGraph->hessian().first; +// +// // Check Hessian +// EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8)); +// +// Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + +// hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation(); +// +// // Check Information vector +// // cout << AugInformationMatrix.size() << endl; +// Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector +// +// // Check Hessian +// EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ +// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// StereoCamera cam1(pose1, K2); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); +// StereoCamera cam2(pose2, K2); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); +// StereoCamera cam3(pose3, K2); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // 1. Project three landmarks into three cameras and triangulate +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// +// double rankTol = 50; +// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// smartFactor1->add(measurements_cam1, views, model, K2); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// smartFactor2->add(measurements_cam2, views, model, K2); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); +// Point3 positionPrior = gtsam::Point3(0,0,1); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); +// graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); +// +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2*noise_pose); +// // initialize third pose with some noise, we expect it to move back to original pose3 +// values.insert(x3, pose3*noise_pose*noise_pose); +// if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); +// +// LevenbergMarquardtParams params; +// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; +// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; +// +// Values result; +// gttic_(SmartStereoProjectionPoseFactor); +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// gttoc_(SmartStereoProjectionPoseFactor); +// tictoc_finishedIteration_(); +// +// // result.print("results of 3 camera, 3 landmark optimization \n"); +// if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); +// std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; +// // EXPECT(assert_equal(pose3,result.at(x3))); +// if(isDebugTest) tictoc_print_(); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){ +// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// StereoCamera cam1(pose1, K); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); +// StereoCamera cam2(pose2, K); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); +// StereoCamera cam3(pose3, K); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// Point3 landmark2(5, -0.5, 1.2); +// Point3 landmark3(3, 0, 3.0); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// // 1. Project three landmarks into three cameras and triangulate +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); +// +// double rankTol = 10; +// +// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// smartFactor1->add(measurements_cam1, views, model, K); +// +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// smartFactor2->add(measurements_cam2, views, model, K); +// +// SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol, linThreshold, manageDegeneracy)); +// smartFactor3->add(measurements_cam3, views, model, K); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); +// Point3 positionPrior = gtsam::Point3(0,0,1); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.push_back(PriorFactor(x1, pose1, noisePrior)); +// graph.push_back(PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); +// graph.push_back(PoseTranslationPrior(x3, positionPrior, noisePriorTranslation)); +// +// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::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), gtsam::Point3(0.1,0.1,0.1)); // smaller noise +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// // initialize third pose with some noise, we expect it to move back to original pose3 +// values.insert(x3, pose3*noise_pose); +// if(isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); +// +// LevenbergMarquardtParams params; +// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; +// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR; +// +// Values result; +// gttic_(SmartStereoProjectionPoseFactor); +// LevenbergMarquardtOptimizer optimizer(graph, values, params); +// result = optimizer.optimize(); +// gttoc_(SmartStereoProjectionPoseFactor); +// tictoc_finishedIteration_(); +// +// // result.print("results of 3 camera, 3 landmark optimization \n"); +// if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); +// std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl; +// // EXPECT(assert_equal(pose3,result.at(x3))); +// if(isDebugTest) tictoc_print_(); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, Hessian ){ +// // cout << " ************************ SmartStereoProjectionPoseFactor: Hessian **********************" << endl; +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// +// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// StereoCamera cam1(pose1, K2); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// StereoCamera cam2(pose2, K2); +// +// // three landmarks ~5 meters infront of camera +// Point3 landmark1(5, 0.5, 1.2); +// +// // 1. Project three landmarks into three cameras and triangulate +// StereoPoint2 cam1_uv1 = cam1.project(landmark1); +// StereoPoint2 cam2_uv1 = cam2.project(landmark1); +// vector measurements_cam1; +// measurements_cam1.push_back(cam1_uv1); +// measurements_cam1.push_back(cam2_uv1); +// +// SmartFactor::shared_ptr smartFactor1(new SmartFactor()); +// smartFactor1->add(measurements_cam1,views, model, K2); +// +// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// +// boost::shared_ptr hessianFactor = smartFactor1->linearize(values); +// if(isDebugTest) hessianFactor->print("Hessian factor \n"); +// +// // compute triangulation from linearization point +// // compute reprojection errors (sum squared) +// // compare with hessianFactor.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( SmartStereoProjectionPoseFactor, HessianWithRotation ){ +// // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl; +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// StereoCamera cam1(pose1, K); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); +// StereoCamera cam2(pose2, K); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); +// StereoCamera cam3(pose3, K); +// +// Point3 landmark1(5, 0.5, 1.2); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// +// SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); +// smartFactorInstance->add(measurements_cam1, views, model, K); +// +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// values.insert(x3, pose3); +// +// boost::shared_ptr hessianFactor = smartFactorInstance->linearize(values); +// // hessianFactor->print("Hessian factor \n"); +// +// Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); +// +// Values rotValues; +// rotValues.insert(x1, poseDrift.compose(pose1)); +// rotValues.insert(x2, poseDrift.compose(pose2)); +// rotValues.insert(x3, poseDrift.compose(pose3)); +// +// boost::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); +// // hessianFactorRot->print("Hessian factor \n"); +// +// // Hessian is invariant to rotations in the nondegenerate case +// EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); +// +// Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); +// +// Values tranValues; +// tranValues.insert(x1, poseDrift2.compose(pose1)); +// tranValues.insert(x2, poseDrift2.compose(pose2)); +// tranValues.insert(x3, poseDrift2.compose(pose3)); +// +// boost::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); +// +// // Hessian is invariant to rotations and translations in the nondegenerate case +// EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); +//} +// +///* *************************************************************************/ +//TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){ +// // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; +// +// std::vector views; +// views.push_back(x1); +// views.push_back(x2); +// views.push_back(x3); +// +// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) +// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); +// StereoCamera cam1(pose1, K2); +// +// // create second camera 1 meter to the right of first camera +// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0)); +// StereoCamera cam2(pose2, K2); +// +// // create third camera 1 meter above the first camera +// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0)); +// StereoCamera cam3(pose3, K2); +// +// Point3 landmark1(5, 0.5, 1.2); +// +// vector measurements_cam1, measurements_cam2, measurements_cam3; +// +// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); +// +// SmartFactor::shared_ptr smartFactor(new SmartFactor()); +// smartFactor->add(measurements_cam1, views, model, K2); +// +// +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// values.insert(x3, pose3); +// +// boost::shared_ptr hessianFactor = smartFactor->linearize(values); +// if(isDebugTest) hessianFactor->print("Hessian factor \n"); +// +// Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); +// +// Values rotValues; +// rotValues.insert(x1, poseDrift.compose(pose1)); +// rotValues.insert(x2, poseDrift.compose(pose2)); +// rotValues.insert(x3, poseDrift.compose(pose3)); +// +// boost::shared_ptr hessianFactorRot = smartFactor->linearize(rotValues); +// if(isDebugTest) hessianFactorRot->print("Hessian factor \n"); +// +// // Hessian is invariant to rotations in the nondegenerate case +// EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); +// +// Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); +// +// Values tranValues; +// tranValues.insert(x1, poseDrift2.compose(pose1)); +// tranValues.insert(x2, poseDrift2.compose(pose2)); +// tranValues.insert(x3, poseDrift2.compose(pose3)); +// +// boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); +// +// // Hessian is invariant to rotations and translations in the nondegenerate case +// EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) ); +//} /* ************************************************************************* */