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

fixed bug with smart measurement calling .vector(): Point2 does not admit .vector() for some cmake configuration. Using using casting: more elegant now!

release/4.3a0
Luca 2016-07-29 12:14:09 -04:00
parent bb32cadfb0
commit 76353cb10d
1 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/slam/SmartFactorBase.h
View File

@ -226,19 +226,19 @@ public:
// when using stereo cameras, some of the measurements might be missing:
for(size_t i=0; i < Fs->size(); i++){
if(ZDim == 3){ // it's a stereo point ..
Z z3 = measured_.at(i);
if(std::isnan(z3.vector()[1])){ // .. and the right pixel is invalid
// delete influence of right point on jacobian Fs
MatrixZD& Fi = Fs->at(i);
for(size_t ii=0; ii<Dim; ii++)
Fi(1,ii) = 0.0;
const StereoPoint2 * z3 = reinterpret_cast<const StereoPoint2*>(&measured_.at(i));
if(z3 && std::isnan(z3->uR())) // if it's a stereo point and the right pixel is invalid
{
// delete influence of right point on jacobian Fs
MatrixZD& Fi = Fs->at(i);
for(size_t ii=0; ii<Dim; ii++)
Fi(1,ii) = 0.0;
// delete influence of right point on jacobian E
E->block<1, Np>(ZDim * i + 1, 0) = Matrix::Zero(1, Np);
// set to zero entry from vector ue
ue(ZDim * i + 1) = 0.0; // this should not matter anyway
}
// delete influence of right point on jacobian E
E->block<1, Np>(ZDim * i + 1, 0) = Matrix::Zero(1, Np);
// set to zero entry from vector ue
ue(ZDim * i + 1) = 0.0;
}
}
return ue;