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

Cheirality check for Projection Factor with Calibrated Camera.

release/4.3a0
Chris Beall 2011-12-15 04:15:05 +00:00
parent 5a75c9e963
commit 33c15dd423
3 changed files with 19 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/CalibratedCamera.cpp
View File

@ -61,6 +61,8 @@ Point2 CalibratedCamera::project(const Point3& point,
const Rot3& R = pose.rotation();
const Point3& r1 = R.r1(), r2 = R.r2(), r3 = R.r3();
Point3 q = pose.transform_to(point);
if(q.z() <= 0)
throw CheiralityException();
if (D_intrinsic_pose || D_intrinsic_point) {
double X = q.x(), Y = q.y(), Z = q.z();

5
gtsam/geometry/CalibratedCamera.h
View File

@ -23,6 +23,11 @@
namespace gtsam {
class CheiralityException: public std::runtime_error {
public:
CheiralityException() : std::runtime_error("Cheirality Exception") {}
};
/**
* A Calibrated camera class [R|-R't], calibration K=I.
* If calibration is known, it is more computationally efficient

15
gtsam/slam/ProjectionFactor.h
View File

@ -82,9 +82,18 @@ namespace gtsam {
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
SimpleCamera camera(*K_, pose);
Point2 reprojectionError(camera.project(point, H1, H2) - z_);
return reprojectionError.vector();
try {
SimpleCamera camera(*K_, pose);
Point2 reprojectionError(camera.project(point, H1, H2) - z_);
return reprojectionError.vector();
}
catch( CheiralityException& e) {
if (H1) *H1 = zeros(2,6);
if (H2) *H2 = zeros(2,3);
cout << e.what() << ": Landmark "<< this->key2_.index() <<
" moved behind camera " << this->key1_.index() << endl;
return zero(2);
}
}
/** return the measurement */