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

first implementation of smartStereo with possibly left-only pixel measurements

release/4.3a0
Luca 2016-07-24 18:27:20 -04:00
parent 3c15ef5d1e
commit f2bec78a58
2 changed files with 26 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/slam/SmartFactorBase.h
View File

@ -27,6 +27,7 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/RegularHessianFactor.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/StereoPoint2.h>
#include <boost/optional.hpp>
#include <boost/serialization/optional.hpp>
@ -210,6 +211,27 @@ public:
Fs->at(i) = Fs->at(i) * J;
}
}
static const int Np = FixedDimension<POINT>::value; // 2 (Unit3) or 3 (Point3)
// 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(isnan(z3.vector()[1])){ // .. and the right pixel is invalid
// delete influence of right point on jacobian Fs
std::cout << "unwhitenedError:: isnan(z3->uR()" << z3.vector() << std::endl;
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
}
}
}
return ue;
}

6
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -257,8 +257,10 @@ public:
const StereoPoint2 zi = measured_[i];
monoCameras.push_back(leftCamera_i);
monoMeasured.push_back(Point2(zi.uL(),zi.v()));
monoCameras.push_back(rightCamera_i);
monoMeasured.push_back(Point2(zi.uR(),zi.v()));
if(!isnan(zi.uR())){ // if right point is valid
monoCameras.push_back(rightCamera_i);
monoMeasured.push_back(Point2(zi.uR(),zi.v()));
}
}
if (retriangulate)
result_ = gtsam::triangulateSafe(monoCameras, monoMeasured,