diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 39b5b679e..6824d77e8 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -250,6 +250,20 @@ namespace gtsam { return Point2(P.x() / P.z(), P.y() / P.z()); } + /** + * projects a 3-dimensional point at infinity (direction-only) in camera coordinates into the + * 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, + boost::optional H1 = boost::none){ + if (H1) { + double d = 1.0 / P.z(), d2 = d * d; + *H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); + } + return Point2(P.x() / P.z(), P.y() / P.z()); + } + /// Project a point into the image and check depth inline std::pair projectSafe(const Point3& pw) const { const Point3 pc = pose_.transform_to(pw) ; @@ -296,6 +310,48 @@ namespace gtsam { return pi; } + /** project a point at infinity from world coordinate to the image + * @param pw is a point in the world coordinate (it is pw = lambda*[pw_x pw_y pw_z] with lambda->inf) + * @param H1 is the jacobian w.r.t. pose3 + * @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, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const { + + 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) ; + return K_.uncalibrate(pn); + } + + // world to camera coordinate + Matrix Hc1 /* 3*6 */, Hc2 /* 3*3 */ ; + const Point3 pc = pose_.rotation().unrotate(pw, Hc1, Hc2) ; + if( pc.z() <= 0 ) throw CheiralityException(); + + // camera to normalized image coordinate + Matrix Hn; // 2*3 + const Point2 pn = project_point_at_infinity_to_camera(pc, Hn) ; + + // uncalibration + Matrix Hk, Hi; // 2*2 + Matrix H23 = Matrix::Zero(3,2); + H23.block(0,0,2,2) = Matrix::Identity(2,2); + const Point2 pi = K_.uncalibrate(pn, Hk, Hi); + + // chain the jacobian matrices + const Matrix tmp = Hi*Hn; + if (H1) *H1 = tmp * Hc1; + if (H2) *H2 = tmp * Hc2 * H23; + if (H3) *H3 = Hk; + return pi; + } + + /** project a point from world coordinate to the image * @param pw is a point in the world coordinate * @param H1 is the jacobian w.r.t. [pose3 calibration] diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp index c35cd11b3..0353a807f 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp @@ -285,7 +285,7 @@ int main(int argc, char** argv) { // This is a new landmark, create a new factor and add to mapping boost::shared_ptr smartFactorState(new SmartProjectionFactorState()); - SmartFactor::shared_ptr smartFactor(new SmartFactor(measurements, pixel_sigma, views, K, boost::none, boost::none, smartFactorState)); + SmartFactor::shared_ptr smartFactor(new SmartFactor(measurements, pixel_sigma, views, K)); smartFactorStates.insert( make_pair(L(l), smartFactorState) ); smartFactors.insert( make_pair(L(l), smartFactor) ); graph.push_back(smartFactor); diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 795534282..66bf5e745 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -45,9 +45,9 @@ namespace gtsam { const SharedNoiseModel noise_; ///< noise model used ///< (important that the order is the same as the keys that we use to create the factor) boost::shared_ptr K_; ///< shared pointer to calibration object - boost::optional point_; boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame boost::shared_ptr state_; + boost::optional point_; // verbosity handling for Cheirality Exceptions bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) @@ -78,10 +78,9 @@ namespace gtsam { */ SmartProjectionFactor(const std::vector measured, const SharedNoiseModel& model, std::vector poseKeys, const boost::shared_ptr& K, - boost::optional point = boost::none, boost::optional body_P_sensor = boost::none, boost::shared_ptr state = boost::shared_ptr()) : - measured_(measured), noise_(model), K_(K), point_(point), body_P_sensor_(body_P_sensor), + measured_(measured), noise_(model), K_(K), body_P_sensor_(body_P_sensor), state_(state), throwCheirality_(false), verboseCheirality_(false) { keys_.assign(poseKeys.begin(), poseKeys.end()); } @@ -100,10 +99,9 @@ namespace gtsam { SmartProjectionFactor(const std::vector measured, const SharedNoiseModel& model, std::vector poseKeys, const boost::shared_ptr& K, bool throwCheirality, bool verboseCheirality, - boost::optional point = boost::none, boost::optional body_P_sensor = boost::none, boost::shared_ptr state = boost::shared_ptr()) : - measured_(measured), noise_(model), K_(K), point_(point), body_P_sensor_(body_P_sensor), + measured_(measured), noise_(model), K_(K), body_P_sensor_(body_P_sensor), state_(state), throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} /** @@ -112,10 +110,9 @@ namespace gtsam { * @param K shared pointer to the constant calibration */ SmartProjectionFactor(const SharedNoiseModel& model, const boost::shared_ptr& K, - boost::optional point = boost::none, boost::optional body_P_sensor = boost::none, boost::shared_ptr state = boost::shared_ptr()) : - noise_(model), K_(K), point_(point), body_P_sensor_(body_P_sensor), state_(state) { + noise_(model), K_(K), body_P_sensor_(body_P_sensor), state_(state) { } /** Virtual destructor */ @@ -195,13 +192,8 @@ namespace gtsam { } // We triangulate the 3D position of the landmark - Point3 point; try { - if (point_) { - point = *point_; - } else { - point = triangulatePoint3(cameraPoses, measured_, *K_); - } + point_ = triangulatePoint3(cameraPoses, measured_, *K_); } catch( TriangulationCheiralityException& e) { // point is behind one of the cameras, turn factor off by setting everything to 0 //std::cout << e.what() << std::end; @@ -210,6 +202,12 @@ 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); @@ -219,7 +217,7 @@ namespace gtsam { for(size_t i = 0; i < measured_.size(); i++) { Pose3 pose = cameraPoses.at(i); PinholeCamera camera(pose, *K_); - b.at(i) = - ( camera.project(point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector(); + b.at(i) = - ( camera.project(point_,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector(); noise_-> WhitenSystem(Hx.at(i), Hl.at(i), b.at(i)); f += b.at(i).squaredNorm(); } @@ -266,24 +264,42 @@ namespace gtsam { if (blockwise == false){ // version with full matrix multiplication // ========================================================================================================== + Matrix Hx2 = zeros(2 * numKeys, 6 * numKeys); - Matrix Hl2 = zeros(2 * numKeys, 3); + Matrix Hl2 = zeros(2 * numKeys, dim_landmark); Vector b2 = zero(2 * numKeys); - 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(); + if(degenerate){ + 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(); - noise_-> WhitenSystem(Hxi, Hli, bi); - f += bi.squaredNorm(); + noise_-> WhitenSystem(Hxi, Hli, bi); + f += bi.squaredNorm(); - Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi; - Hl2.block( 2*i, 0, 2, 3 ) = Hli; + Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi; + Hl2.block( 2*i, 0, 2, 2 ) = Hli; - subInsert(b2,bi,2*i); + subInsert(b2,bi,2*i); + } + } + else{ + 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(); + noise_-> WhitenSystem(Hxi, Hli, bi); + f += bi.squaredNorm(); + + Hx2.block( 2*i, 6*i, 2, 6 ) = Hxi; + Hl2.block( 2*i, 0, 2, 3 ) = Hli; + + subInsert(b2,bi,2*i); + } } // Shur complement trick @@ -334,13 +350,8 @@ namespace gtsam { } // We triangulate the 3D position of the landmark - Point3 point; try { - if (point_) { - point = *point_; - } else { - point = triangulatePoint3(cameraPoses, measured_, *K_); - } + point_ = triangulatePoint3(cameraPoses, measured_, *K_); } catch( TriangulationCheiralityException& e) { // point is behind one of the cameras, turn factor off by setting everything to 0 //std::cout << e.what() << std::end; @@ -351,7 +362,7 @@ namespace gtsam { Pose3 pose = cameraPoses.at(i); PinholeCamera camera(pose, *K_); - Point2 reprojectionError(camera.project(point) - measured_.at(i)); + Point2 reprojectionError(camera.project(point_) - measured_.at(i)); overallError += noise_->distance( reprojectionError.vector() ); } return overallError; @@ -370,7 +381,7 @@ namespace gtsam { return noise_; } - /** return the noise landmark */ + /** return the landmark */ boost::optional point() const { return point_; } diff --git a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp index 3f2912c2e..3095e7ff7 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp @@ -167,7 +167,7 @@ TEST( SmartProjectionFactor, noisy ){ // DOUBLES_EQUAL(expectedError, actualError, 1e-7); } -/* ************************************************************************* */ +/* ************************************************************************* TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){ cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks, 1 iteration, comparison b/w Generic and Smart Projection Factors **********************" << endl;