diff --git a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp index f152b7def..5e65f3022 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp @@ -151,16 +151,6 @@ TEST( MultiProjectionFactor, noisy ){ double actualError = smartFactor->error(values); double expectedError = sqrt(0.08); - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor); - graph.add(PriorFactor(x1, level_pose, noisePrior)); - - LevenbergMarquardtOptimizer optimizer(graph, values); - Values result = optimizer.optimize(); - - result.print("results of the optimization \n"); // we do not expect to be able to predict the error, since the error on the pixel will change // the triangulation of the landmark which is internal to the factor. // DOUBLES_EQUAL(expectedError, actualError, 1e-7); @@ -169,7 +159,7 @@ TEST( MultiProjectionFactor, noisy ){ ///* ************************************************************************* */ TEST( MultiProjectionFactor, 3poses ){ - cout << " ************************ MultiProjectionFactor: noisy ****************************" << endl; + cout << " ************************ MultiProjectionFactor: 3 cams + 3 landmarks **********************" << endl; Symbol x1('X', 1); Symbol x2('X', 2); @@ -178,53 +168,78 @@ TEST( MultiProjectionFactor, 3poses ){ const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); std::vector views; - views += x1, x2; //, x3; + views += x1, x2, x3; Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) - Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); - SimpleCamera level_camera(level_pose, *K); + Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); + SimpleCamera cam1(pose1, *K); // create second camera 1 meter to the right of first camera - Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); - SimpleCamera level_camera_right(level_pose_right, *K); + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); + SimpleCamera cam2(pose2, *K); - // landmark ~5 meters infront of camera - Point3 landmark(5, 0.5, 1.2); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); + SimpleCamera cam3(pose2, *K); - // 1. Project two landmarks into two cameras and triangulate - Point2 pixelError(0.2,0.2); - Point2 level_uv = level_camera.project(landmark) + pixelError; - Point2 level_uv_right = level_camera_right.project(landmark); + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2); + Point3 landmark2(5, -0.5, 1.2); + Point3 landmark3(5, 0, 3.0); - Values values; - values.insert(x1, level_pose); - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); - values.insert(x2, level_pose_right.compose(noise_pose)); + vector measurements_cam1, measurements_cam2, measurements_cam3; -// poses += level_pose, level_pose_right; - vector measurements; - measurements += level_uv, level_uv_right; + // 1. Project three landmarks into three cameras and triangulate + Point2 cam1_uv1 = cam1.project(landmark1); + Point2 cam2_uv1 = cam2.project(landmark1); + Point2 cam3_uv1 = cam3.project(landmark1); + measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1; - SmartProjectionFactor::shared_ptr - smartFactor(new SmartProjectionFactor(measurements, noiseProjection, views, K)); + // + Point2 cam1_uv2 = cam1.project(landmark2); + Point2 cam2_uv2 = cam2.project(landmark2); + Point2 cam3_uv2 = cam3.project(landmark2); + measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2; - double actualError = smartFactor->error(values); - double expectedError = sqrt(0.08); + + Point2 cam1_uv3 = cam1.project(landmark3); + Point2 cam2_uv3 = cam2.project(landmark3); + Point2 cam3_uv3 = cam3.project(landmark3); + measurements_cam3 += cam1_uv3, cam2_uv3, cam3_uv3; + + typedef SmartProjectionFactor SmartFactor; + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(measurements_cam1, noiseProjection, views, K)); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(measurements_cam2, noiseProjection, views, K)); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(measurements_cam3, noiseProjection, views, K)); + +// double actualError = smartFactor->error(values); +// double expectedError = sqrt(0.08); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); NonlinearFactorGraph graph; - graph.push_back(smartFactor); - graph.add(PriorFactor(x1, level_pose, noisePrior)); + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.add(PriorFactor(x1, pose1, noisePrior)); - LevenbergMarquardtOptimizer optimizer(graph, values); + + 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, pose1); + values.insert(x3, pose3* noise_pose); + + LevenbergMarquardtParams params; + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + params.verbosity = NonlinearOptimizerParams::ERROR; + LevenbergMarquardtOptimizer optimizer(graph, values, params); Values result = optimizer.optimize(); - result.print("results of the optimization \n"); - // we do not expect to be able to predict the error, since the error on the pixel will change - // the triangulation of the landmark which is internal to the factor. - // DOUBLES_EQUAL(expectedError, actualError, 1e-7); + result.print("results of 3 camera, 3 landmark optimization \n"); + }