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

handle chirelity exception

release/4.3a0
Yong-Dian Jian 2012-02-17 23:33:29 +00:00
parent e9ae2b5cc7
commit b735d30c7d
1 changed files with 13 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/slam/GeneralSFMFactor.h
View File

@ -77,15 +77,20 @@ namespace gtsam {
}
/** h(x)-z */
Vector evaluateError(
const Cam& camera,
const Point3& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
Vector evaluateError(const Cam& camera, const Point3& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
Vector error = z_.localCoordinates(camera.project2(point,H1,H2));
// gtsam::print(error, "error");
return error;
try {
Point2 reprojError(camera.project2(point,H1,H2) - z_);
return reprojError.vector();
}
catch( CheiralityException& e) {
if (H1) *H1 = zeros(2, camera.dim());
if (H2) *H2 = zeros(2, point.dim());
// cout << e.what() << ": Landmark "<< this->key2_.index()
// << " behind Camera " << this->key1_.index() << endl;
return zero(2);
}
}
/** return the measured */