12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Cleaned up projectSafe and cheirality exception

release/4.3a0
dellaert 2015-02-21 10:41:53 +01:00
parent f08e228173
commit c7a41d30cc
3 changed files with 32 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/geometry/CalibratedCamera.cpp
View File

@ -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)

18
gtsam/geometry/CalibratedCamera.h
View File

@ -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

6
gtsam/geometry/PinholePose.h
View File

@ -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