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

actually set everything to zero to turn factor off during linearization

release/4.3a0
Chris Beall 2013-08-09 15:28:57 +00:00
parent 7b0c559091
commit 369e69b0d2
1 changed files with 8 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -161,8 +161,7 @@ namespace gtsam {
// We triangulate the 3D position of the landmark // We triangulate the 3D position of the landmark
boost::optional<Point3> point = triangulatePoint3(cameraPoses, measured_, *K_); boost::optional<Point3> point = triangulatePoint3(cameraPoses, measured_, *K_);
if (!point)
return HessianFactor::shared_ptr(new HessianFactor());
if (debug) { if (debug) {
std::cout << "point " << *point << std::endl; std::cout << "point " << *point << std::endl;
@ -178,6 +177,13 @@ namespace gtsam {
js += ordering[k]; js += ordering[k];
} }
// point is behind one of the cameras, turn factor off by setting everything to 0
if (!point) {
BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6,6);
BOOST_FOREACH(Vector& v, gs) v = zero(6);
return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f));
}
// For debug only // For debug only
std::vector<Matrix> Gs1; std::vector<Matrix> Gs1;
std::vector<Vector> gs1; std::vector<Vector> gs1;