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

noiseModel_ is now private -> just call Errors

release/4.3a0
dellaert 2015-02-22 22:05:58 +01:00
parent cdaaee6fce
commit 66d12a1c30
1 changed files with 4 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/slam/SmartProjectionFactor.h
View File

@ -630,19 +630,10 @@ public:
std::cout
<< "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!"
<< std::endl;
size_t i = 0;
double overallError = 0;
BOOST_FOREACH(const Camera& camera, cameras) {
const Point2& zi = this->measured_.at(i);
if (i == 0) // first pose
this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity
Point2 reprojectionError(
camera.projectPointAtInfinity(this->point_) - zi);
overallError += 0.5
* this->noiseModel_->distance(reprojectionError.vector());
i += 1;
}
return overallError;
// 3D parameterization of point at infinity
const Point2& zi = this->measured_.at(0);
this->point_ = cameras.front().backprojectPointAtInfinity(zi);
return Base::totalReprojectionErrorAtInfinity(cameras,this->point_);
} else {
// Just use version in base class
return Base::totalReprojectionError(cameras, point_);