Cleaned up projectSafe and cheirality exception
							parent
							
								
									f08e228173
								
							
						
					
					
						commit
						c7a41d30cc
					
				|  | @ -19,6 +19,8 @@ | |||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/geometry/CalibratedCamera.h> | ||||
| 
 | ||||
| using namespace std; | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  | @ -69,7 +71,7 @@ bool PinholeBase::equals(const PinholeBase &camera, double tol) const { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void PinholeBase::print(const std::string& s) const { | ||||
| void PinholeBase::print(const string& s) const { | ||||
|   pose_.print(s + ".pose"); | ||||
| } | ||||
| 
 | ||||
|  | @ -83,29 +85,36 @@ const Pose3& PinholeBase::pose(OptionalJacobian<6, 6> H) const { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Point2 PinholeBase::project_to_camera(const Point3& P, | ||||
| Point2 PinholeBase::project_to_camera(const Point3& pc, | ||||
|     OptionalJacobian<2, 3> Dpoint) { | ||||
| #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION | ||||
|   if (P.z() <= 0) | ||||
|   throw CheiralityException(); | ||||
| #endif | ||||
|   double d = 1.0 / P.z(); | ||||
|   const double u = P.x() * d, v = P.y() * d; | ||||
|   double d = 1.0 / pc.z(); | ||||
|   const double u = pc.x() * d, v = pc.y() * d; | ||||
|   if (Dpoint) | ||||
|     *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; | ||||
|   return Point2(u, v); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| pair<Point2, bool> PinholeBase::projectSafe(const Point3& pw) const { | ||||
|   const Point3 pc = pose().transform_to(pw); | ||||
|   const Point2 pn = project_to_camera(pc); | ||||
|   return make_pair(pn, pc.z() > 0); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose, | ||||
|     OptionalJacobian<2, 3> Dpoint) const { | ||||
| 
 | ||||
|   Matrix3 Rt; // calculated by transform_to if needed
 | ||||
|   const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0); | ||||
|   Point2 pn = project_to_camera(q); | ||||
| #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION | ||||
|   if (q.z() <= 0) | ||||
|   throw CheiralityException(); | ||||
| #endif | ||||
|   const Point2 pn = project_to_camera(q); | ||||
| 
 | ||||
|   if (Dpose || Dpoint) { | ||||
|     const double z = q.z(), d = 1.0 / z; | ||||
|     const double d = 1.0 / q.z(); | ||||
|     if (Dpose) | ||||
|       *Dpose = PinholeBase::Dpose(pn, d); | ||||
|     if (Dpoint) | ||||
|  |  | |||
|  | @ -138,21 +138,23 @@ public: | |||
|   /// @{
 | ||||
| 
 | ||||
|   /**
 | ||||
|    * projects a 3-dimensional point in camera coordinates into the | ||||
|    * camera and returns a 2-dimensional point | ||||
|    * @param P A point in camera coordinates | ||||
|    * @param Dpoint is the 2*3 Jacobian w.r.t. P | ||||
|    * Project from 3D point in camera coordinates into image | ||||
|    * Does *not* throw a CheiralityException, even if pc behind image plane | ||||
|    * @param pc point in camera coordinates | ||||
|    * @param Dpoint is the 2*3 Jacobian w.r.t. pc | ||||
|    */ | ||||
|   static Point2 project_to_camera(const Point3& P, //
 | ||||
|   static Point2 project_to_camera(const Point3& pc, //
 | ||||
|       OptionalJacobian<2, 3> Dpoint = boost::none); | ||||
| 
 | ||||
|   /**
 | ||||
|    * backproject a 2-dimensional point to a 3-dimensional point at given depth | ||||
|    */ | ||||
|   /// Project a point into the image and check depth
 | ||||
|   std::pair<Point2, bool> projectSafe(const Point3& pw) const; | ||||
| 
 | ||||
|   /// backproject a 2-dimensional point to a 3-dimensional point at given depth
 | ||||
|   static Point3 backproject_from_camera(const Point2& p, const double depth); | ||||
| 
 | ||||
|   /**
 | ||||
|    * Project point into the image | ||||
|    * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION | ||||
|    * @param point 3D point in world coordinates | ||||
|    * @param Dpose the optionally computed Jacobian with respect to camera | ||||
|    * @param Dpoint the optionally computed Jacobian with respect to the 3D point | ||||
|  |  | |||
|  | @ -72,9 +72,9 @@ public: | |||
| 
 | ||||
|   /// Project a point into the image and check depth
 | ||||
|   std::pair<Point2, bool> projectSafe(const Point3& pw) const { | ||||
|     const Point3 pc = pose().transform_to(pw); | ||||
|     const Point2 pn = project_to_camera(pc); | ||||
|     return std::make_pair(calibration().uncalibrate(pn), pc.z() > 0); | ||||
|     std::pair<Point2, bool> pn = PinholeBase::projectSafe(pw); | ||||
|     pn.first = calibration().uncalibrate(pn.first); | ||||
|     return pn; | ||||
|   } | ||||
| 
 | ||||
|   /** project a point from world coordinate to the image, fixed Jacobians
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue