diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 6824d77e8..789e792c2 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -255,7 +255,7 @@ namespace gtsam { * camera and returns a 2-dimensional point, no calibration applied * With optional 2by3 derivative */ - inline static Point2 project_point_at_infinity_to_camera(const Point3& P, + inline static Point2 projectPointAtInfinityToCamera(const Point3& P, boost::optional H1 = boost::none){ if (H1) { double d = 1.0 / P.z(), d2 = d * d; @@ -316,7 +316,7 @@ namespace gtsam { * @param H2 is the jacobian w.r.t. point3 * @param H3 is the jacobian w.r.t. calibration */ - inline Point2 project_point_at_infinity(const Point3& pw, + inline Point2 projectPointAtInfinity(const Point3& pw, boost::optional H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { @@ -324,7 +324,7 @@ namespace gtsam { if (!H1 && !H2 && !H3) { const Point3 pc = pose_.rotation().unrotate(pw) ; if ( pc.z() <= 0 ) throw CheiralityException(); - const Point2 pn = project_point_at_infinity_to_camera(pc) ; + const Point2 pn = projectPointAtInfinityToCamera(pc) ; return K_.uncalibrate(pn); } @@ -335,7 +335,7 @@ namespace gtsam { // camera to normalized image coordinate Matrix Hn; // 2*3 - const Point2 pn = project_point_at_infinity_to_camera(pc, Hn) ; + const Point2 pn = projectPointAtInfinityToCamera(pc, Hn) ; // uncalibration Matrix Hk, Hi; // 2*2 @@ -386,6 +386,13 @@ namespace gtsam { return pose_.transform_from(pc); } + /// backproject a 2-dimensional point to a 3-dimensional point at infinity + inline Point3 backprojectPointAtInfinity(const Point2& p) const { + const Point2 pn = K_.calibrate(p); + const Point3 pc(pn.x(), pn.y(), 1.0); + return pose_.rotation().rotate(pc); + } + /** * Calculate range to a landmark * @param point 3D location of landmark diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 66bf5e745..74293d1bd 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -47,7 +47,7 @@ namespace gtsam { boost::shared_ptr K_; ///< shared pointer to calibration object boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame boost::shared_ptr state_; - boost::optional point_; + mutable Point3 point_; // verbosity handling for Cheirality Exceptions bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) @@ -175,6 +175,8 @@ namespace gtsam { virtual boost::shared_ptr linearize(const Values& values) const { bool blockwise = false; + bool degenerate = false; + int dim_landmark = 3; unsigned int numKeys = keys_.size(); std::vector js; @@ -194,6 +196,11 @@ namespace gtsam { // We triangulate the 3D position of the landmark try { point_ = triangulatePoint3(cameraPoses, measured_, *K_); + } catch( TriangulationUnderconstrainedException& e) { + // point is triangulated at infinity + //std::cout << e.what() << std::end; + degenerate = true; + dim_landmark = 2; } catch( TriangulationCheiralityException& e) { // point is behind one of the cameras, turn factor off by setting everything to 0 //std::cout << e.what() << std::end; @@ -202,12 +209,6 @@ namespace gtsam { return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); } - bool degenerate = true; - - int dim_landmark = 2; - - - if (blockwise){ // ========================================================================================================== std::vector Hx(numKeys); @@ -273,8 +274,12 @@ namespace gtsam { for(size_t i = 0; i < measured_.size(); i++) { Pose3 pose = cameraPoses.at(i); PinholeCamera camera(pose, *K_); + if(i==0){ // first pose + point_ = camera.backprojectPointAtInfinity(measured_.at(i)); // 3D parametrization of point at infinity + std::cout << "point_ " << point_<< std::endl; + } Matrix Hxi, Hli; - Vector bi = -( camera.project(point,Hxi,Hli) - measured_.at(i) ).vector(); + Vector bi = -( camera.projectPointAtInfinity(point_,Hxi,Hli) - measured_.at(i) ).vector(); noise_-> WhitenSystem(Hxi, Hli, bi); f += bi.squaredNorm(); @@ -286,11 +291,12 @@ namespace gtsam { } } else{ + std::cout << "non degenerate " << point_<< std::endl; for(size_t i = 0; i < measured_.size(); i++) { Pose3 pose = cameraPoses.at(i); PinholeCamera camera(pose, *K_); Matrix Hxi, Hli; - Vector bi = -( camera.project(point,Hxi,Hli) - measured_.at(i) ).vector(); + Vector bi = -( camera.project(point_,Hxi,Hli) - measured_.at(i) ).vector(); noise_-> WhitenSystem(Hxi, Hli, bi); f += bi.squaredNorm(); @@ -338,6 +344,9 @@ namespace gtsam { virtual double error(const Values& values) const { if (this->active(values)) { double overallError=0; + bool degenerate = false; + + std::cout << "evaluating error in smart factor " << std::endl; // Collect all poses (Cameras) std::vector cameraPoses; @@ -351,22 +360,44 @@ namespace gtsam { // We triangulate the 3D position of the landmark try { - point_ = triangulatePoint3(cameraPoses, measured_, *K_); + point_ = triangulatePoint3(cameraPoses, measured_, *K_); } catch( TriangulationCheiralityException& e) { + std::cout << "TriangulationCheiralityException " << std::endl; // point is behind one of the cameras, turn factor off by setting everything to 0 //std::cout << e.what() << std::end; return 0.0; + } catch( TriangulationUnderconstrainedException& e) { + // point is triangulated at infinity + //std::cout << e.what() << std::endl; + degenerate = true; } - for(size_t i = 0; i < measured_.size(); i++) { - Pose3 pose = cameraPoses.at(i); - PinholeCamera camera(pose, *K_); + std::cout << "degenerate " << degenerate << std::endl; - Point2 reprojectionError(camera.project(point_) - measured_.at(i)); - overallError += noise_->distance( reprojectionError.vector() ); + if(degenerate){ + for(size_t i = 0; i < measured_.size(); i++) { + Pose3 pose = cameraPoses.at(i); + PinholeCamera camera(pose, *K_); + if(i==0){ // first pose + point_ = camera.backprojectPointAtInfinity(measured_.at(i)); // 3D parametrization of point at infinity + std::cout << "point_ " << point_<< std::endl; + } + Point2 reprojectionError(camera.projectPointAtInfinity(point_) - measured_.at(i)); + overallError += noise_->distance( reprojectionError.vector() ); + } + return overallError; } - return overallError; - } else { + else{ + for(size_t i = 0; i < measured_.size(); i++) { + Pose3 pose = cameraPoses.at(i); + PinholeCamera camera(pose, *K_); + + Point2 reprojectionError(camera.project(point_) - measured_.at(i)); + overallError += noise_->distance( reprojectionError.vector() ); + } + return overallError; + } + } else { // else of active flag return 0.0; } } diff --git a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp index 3095e7ff7..eace3962e 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -41,6 +42,7 @@ #include #include + #include using namespace std; @@ -85,7 +87,7 @@ TEST( SmartProjectionFactor, ConstructorWithTransform) { measurements.push_back(Point2(323.0, 240.0)); Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - TestSmartProjectionFactor factor(measurements, model, views, K, boost::none, body_P_sensor); + TestSmartProjectionFactor factor(measurements, model, views, K, body_P_sensor); } /* ************************************************************************* */ @@ -111,8 +113,8 @@ TEST( SmartProjectionFactor, EqualsWithTransform ) { std::vector views; views += X(1); - TestSmartProjectionFactor factor1(measurements, model, views, K, boost::none, body_P_sensor); - TestSmartProjectionFactor factor2(measurements, model, views, K, boost::none, body_P_sensor); + TestSmartProjectionFactor factor1(measurements, model, views, K, body_P_sensor); + TestSmartProjectionFactor factor2(measurements, model, views, K, body_P_sensor); CHECK(assert_equal(factor1, factor2)); } @@ -581,6 +583,102 @@ TEST( SmartProjectionFactor, 3poses_projection_factor ){ } +/* ************************************************************************* +TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){ + cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; + + Symbol x1('X', 1); + Symbol x2('X', 2); + Symbol x3('X', 3); + + const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); + + std::vector views; + 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 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 pose2 = Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)) * pose1; + SimpleCamera cam2(pose2, *K); + + // create third camera 1 meter above the first camera + Pose3 pose3 = Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)) * pose2; + SimpleCamera 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 + 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; + + // + 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; + + + 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)); + + 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)); + graph.print(); +// 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); + values.at(x3).print("Smart: Pose3 before optimization: "); + + LevenbergMarquardtParams params; + params.verbosityLM = LevenbergMarquardtParams::TRYDELTA; + params.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + gttic_(SmartProjectionFactor); + LevenbergMarquardtOptimizer optimizer(graph, values, params); + result = optimizer.optimize(); + gttoc_(SmartProjectionFactor); + tictoc_finishedIteration_(); + + // result.print("results of 3 camera, 3 landmark optimization \n"); + result.at(x3).print("Smart: Pose3 after optimization: "); + EXPECT(assert_equal(pose3,result.at(x3))); + tictoc_print_(); + +} /* ************************************************************************* */