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

now SmartFactorBase doesn't know about stereoPoint2 (removed also header). The functionality to check if the right pixel is missing has been moved to SmartStereoProjectionFactor, removing the casting.

release/4.3a0
lcarlone 2016-08-02 22:56:48 -04:00
parent a93c1e86f2
commit e0869719fa
2 changed files with 34 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/slam/SmartFactorBase.h
View File

@ -27,7 +27,6 @@
#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>
@ -221,29 +220,17 @@ 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 < cameras.size(); i++){
const StereoPoint2 * z3 = reinterpret_cast<const StereoPoint2*>(&measured_.at(i));
if(ZDim==3 && z3 && std::isnan(z3->uR())) // if it's a stereo point and the right pixel is invalid
{
if(Fs){ // 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;
}
if(E) // 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;
}
}
correctForMissingMeasurements(cameras, ue, Fs, E);
return ue;
}
/**
* This corrects the Jacobians for the case in which some pixel measurement is missing (nan)
* In practice, this does not do anything in the monocular case, but it is implemented in the stereo version
*/
virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
boost::optional<Matrix&> E = boost::none) const {}
/**
* Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z]
* Noise model applied

26
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -516,6 +516,32 @@ public:
}
}
/**
* This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan)
*/
virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue,
boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
boost::optional<Matrix&> E = boost::none) const
{
// when using stereo cameras, some of the measurements might be missing:
for(size_t i=0; i < cameras.size(); i++){
const StereoPoint2& z = measured_.at(i);
if(std::isnan(z.uR())) // if the right pixel is invalid
{
if(Fs){ // 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;
}
if(E) // delete influence of right point on jacobian E
E->row(ZDim * i + 1) = Matrix::Zero(1, E->cols());
// set the corresponding entry of vector ue to zero
ue(ZDim * i + 1) = 0.0;
}
}
}
/** return the landmark */
TriangulationResult point() const {
return result_;