degenerate case in smart vision factor
							parent
							
								
									633220a9dd
								
							
						
					
					
						commit
						0422b9cfef
					
				|  | @ -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<Matrix&> 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<Matrix&> H1 = boost::none, | ||||
|             boost::optional<Matrix&> H2 = boost::none, | ||||
|             boost::optional<Matrix&> 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 | ||||
|  |  | |||
|  | @ -47,7 +47,7 @@ namespace gtsam { | |||
|     boost::shared_ptr<CALIBRATION> K_;  ///< shared pointer to calibration object
 | ||||
|     boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
 | ||||
|     boost::shared_ptr<SmartProjectionFactorState> state_; | ||||
|     boost::optional<Point3> 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<GaussianFactor> linearize(const Values& values) const { | ||||
| 
 | ||||
|       bool blockwise = false; | ||||
|       bool degenerate = false; | ||||
|       int dim_landmark = 3; | ||||
|        | ||||
|       unsigned int numKeys = keys_.size(); | ||||
|       std::vector<Index> 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<Matrix> Hx(numKeys); | ||||
|  | @ -273,8 +274,12 @@ namespace gtsam { | |||
|           for(size_t i = 0; i < measured_.size(); i++) { | ||||
|             Pose3 pose = cameraPoses.at(i); | ||||
|             PinholeCamera<CALIBRATION> 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<CALIBRATION> 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<Pose3> 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<CALIBRATION> 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<CALIBRATION> 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<CALIBRATION> 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; | ||||
|       } | ||||
|     } | ||||
|  |  | |||
|  | @ -24,6 +24,7 @@ | |||
| #include <gtsam/slam/PriorFactor.h> | ||||
| #include <gtsam/slam/BetweenFactor.h> | ||||
| #include <gtsam/slam/ProjectionFactor.h> | ||||
| #include <gtsam/slam/PoseTranslationPrior.h> | ||||
| #include <gtsam_unstable/slam/SmartProjectionFactor.h> | ||||
| #include <gtsam/nonlinear/ISAM2.h> | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
|  | @ -41,6 +42,7 @@ | |||
| #include <gtsam/geometry/Cal3_S2.h> | ||||
| #include <gtsam/geometry/SimpleCamera.h> | ||||
| 
 | ||||
| 
 | ||||
| #include <boost/assign/std/vector.hpp> | ||||
| 
 | ||||
| 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<Key> 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<Key> 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<Point2> 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<Pose3, Point3, Cal3_S2> 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<Pose3>(x1, pose1, noisePrior)); | ||||
|   graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation)); | ||||
|   graph.push_back(PoseTranslationPrior<Pose3>(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<Pose3>(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<Pose3>(x3).print("Smart: Pose3 after optimization: "); | ||||
|   EXPECT(assert_equal(pose3,result.at<Pose3>(x3))); | ||||
|   tictoc_print_(); | ||||
| 
 | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue