Merge pull request #865 from borglab/feature/readmes
Frank Dellaert
GitHub
commit
f3c65d9de8
|
|
@ -9,20 +9,21 @@
|
|||
|
||||
namespace gtsam {
|
||||
/**
|
||||
* JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis
|
||||
* JacobianFactor for Schur complement that uses the "Nullspace Trick" by
|
||||
* Mourikis et al.
|
||||
*
|
||||
* This trick is equivalent to the Schur complement, but can be faster.
|
||||
* In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are poses,
|
||||
* is multiplied by Enull, a matrix that spans the left nullspace of E, i.e.,
|
||||
* The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm * mx3 * 3x3)
|
||||
* where Enull is an m x (m-3) matrix
|
||||
* Then Enull'*E*dp = 0, and
|
||||
* In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are
|
||||
* poses, is multiplied by Enull, a matrix that spans the left nullspace of E,
|
||||
* i.e., The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm *
|
||||
* mx3 * 3x3) where Enull is an m x (m-3) matrix Then Enull'*E*dp = 0, and
|
||||
* |Enull'*E*dp + Enull'*F*dX - Enull'*b| == |Enull'*F*dX - Enull'*b|
|
||||
* Normally F is m x 6*numKeys, and Enull'*F yields an (m-3) x 6*numKeys matrix.
|
||||
*
|
||||
* The code below assumes that F is block diagonal and is given as a vector of ZDim*D blocks.
|
||||
* Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for D=6)
|
||||
* Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as a 1x2 * 2x6 mult
|
||||
* The code below assumes that F is block diagonal and is given as a vector of
|
||||
* ZDim*D blocks. Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for
|
||||
* D=6) Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as
|
||||
* a 1x2 * 2x6 multiplication.
|
||||
*/
|
||||
template<size_t D, size_t ZDim>
|
||||
class JacobianFactorSVD: public RegularJacobianFactor<D> {
|
||||
|
|
@ -37,10 +38,10 @@ public:
|
|||
JacobianFactorSVD() {
|
||||
}
|
||||
|
||||
/// Empty constructor with keys
|
||||
JacobianFactorSVD(const KeyVector& keys, //
|
||||
const SharedDiagonal& model = SharedDiagonal()) :
|
||||
Base() {
|
||||
/// Empty constructor with keys.
|
||||
JacobianFactorSVD(const KeyVector& keys,
|
||||
const SharedDiagonal& model = SharedDiagonal())
|
||||
: Base() {
|
||||
Matrix zeroMatrix = Matrix::Zero(0, D);
|
||||
Vector zeroVector = Vector::Zero(0);
|
||||
std::vector<KeyMatrix> QF;
|
||||
|
|
@ -51,18 +52,21 @@ public:
|
|||
}
|
||||
|
||||
/**
|
||||
* @brief Constructor
|
||||
* Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F)
|
||||
* and a reduced point derivative, Enull
|
||||
* and creates a reduced-rank Jacobian factor on the CameraSet
|
||||
* @brief Construct a new JacobianFactorSVD object, createing a reduced-rank
|
||||
* Jacobian factor on the CameraSet.
|
||||
*
|
||||
* @Fblocks:
|
||||
* @param keys keys associated with F blocks.
|
||||
* @param Fblocks CameraSet derivatives, ZDim*D blocks of block-diagonal F
|
||||
* @param Enull a reduced point derivative
|
||||
* @param b right-hand side
|
||||
* @param model noise model
|
||||
*/
|
||||
JacobianFactorSVD(const KeyVector& keys,
|
||||
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks, const Matrix& Enull,
|
||||
const Vector& b, //
|
||||
const SharedDiagonal& model = SharedDiagonal()) :
|
||||
Base() {
|
||||
JacobianFactorSVD(
|
||||
const KeyVector& keys,
|
||||
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks,
|
||||
const Matrix& Enull, const Vector& b,
|
||||
const SharedDiagonal& model = SharedDiagonal())
|
||||
: Base() {
|
||||
size_t numKeys = Enull.rows() / ZDim;
|
||||
size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()?
|
||||
// PLAIN nullptr SPACE TRICK
|
||||
|
|
@ -74,9 +78,8 @@ public:
|
|||
QF.reserve(numKeys);
|
||||
for (size_t k = 0; k < Fblocks.size(); ++k) {
|
||||
Key key = keys[k];
|
||||
QF.push_back(
|
||||
KeyMatrix(key,
|
||||
(Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]));
|
||||
QF.emplace_back(
|
||||
key, (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]);
|
||||
}
|
||||
JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@
|
|||
|
||||
/**
|
||||
* @file ProjectionFactor.h
|
||||
* @brief Basic bearing factor from 2D measurement
|
||||
* @brief Reprojection of a LANDMARK to a 2D point.
|
||||
* @author Chris Beall
|
||||
* @author Richard Roberts
|
||||
* @author Frank Dellaert
|
||||
|
|
@ -22,17 +22,21 @@
|
|||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <boost/optional.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
|
||||
* i.e. the main building block for visual SLAM.
|
||||
* Non-linear factor for a constraint derived from a 2D measurement.
|
||||
* The calibration is known here.
|
||||
* The main building block for visual SLAM.
|
||||
* @addtogroup SLAM
|
||||
*/
|
||||
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
|
||||
template <class POSE = Pose3, class LANDMARK = Point3,
|
||||
class CALIBRATION = Cal3_S2>
|
||||
class GenericProjectionFactor: public NoiseModelFactor2<POSE, LANDMARK> {
|
||||
protected:
|
||||
|
||||
|
|
@ -57,9 +61,9 @@ namespace gtsam {
|
|||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/// Default constructor
|
||||
GenericProjectionFactor() :
|
||||
measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
|
||||
}
|
||||
GenericProjectionFactor() :
|
||||
measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
|
|
|
|||
|
|
@ -0,0 +1,70 @@
|
|||
# SLAM Factors
|
||||
|
||||
## GenericProjectionFactor (defined in ProjectionFactor.h)
|
||||
|
||||
Non-linear factor that minimizes the re-projection error with respect to a 2D measurement.
|
||||
The calibration is assumed known and passed in the constructor.
|
||||
The main building block for visual SLAM.
|
||||
|
||||
Templated on
|
||||
- `POSE`, default `Pose3`
|
||||
- `LANDMARK`, default `Point3`
|
||||
- `CALIBRATION`, default `Cal3_S2`
|
||||
|
||||
## SmartFactors
|
||||
|
||||
These are "structure-less" factors, i.e., rather than introducing a new variable for an observed 3D point or landmark, a single factor is created that provides a multi-view constraint on several poses and/or cameras.
|
||||
While one typically adds multiple GenericProjectionFactors (one for each observation of a landmark), a SmartFactor collects all measurements for a landmark, i.e., the factor graph contains 1 smart factor per landmark.
|
||||
|
||||
### SmartFactorBase
|
||||
|
||||
This is the base class for smart factors, templated on a `CAMERA` type.
|
||||
It has no internal point, but it saves the measurements, keeps a noise model, and an optional sensor pose.
|
||||
|
||||
### SmartProjectionFactor
|
||||
|
||||
Also templated on `CAMERA`. Triangulates a 3D point and keeps an estimate of it around.
|
||||
This factor operates with monocular cameras, and is used to optimize the camera pose
|
||||
*and* calibration for each camera, and requires variables of type `CAMERA` in values.
|
||||
|
||||
If the calibration is fixed use `SmartProjectionPoseFactor` instead!
|
||||
|
||||
|
||||
### SmartProjectionPoseFactor
|
||||
|
||||
Derives from `SmartProjectionFactor` but is templated on a `CALIBRATION` type, setting `CAMERA = PinholePose<CALIBRATION>`.
|
||||
This factor assumes that the camera calibration is fixed and the same for all cameras involved in this factor.
|
||||
The factor only constrains poses.
|
||||
|
||||
If the calibration should be optimized, as well, use `SmartProjectionFactor` instead!
|
||||
|
||||
### SmartProjectionFactorP
|
||||
|
||||
Same as `SmartProjectionPoseFactor`, except:
|
||||
- it is templated on `CAMERA`, i.e., it allows cameras beyond pinhole;
|
||||
- it admits a different calibration for each measurement, i.e., it can model a multi-camera system;
|
||||
- it allows multiple observations from the same pose/key, again, to model a multi-camera system.
|
||||
|
||||
TODO: DimPose and ZDim are hardcoded. Copy/paste from `SmartProjectionPoseFactor`. Unclear what the use case is.
|
||||
|
||||
## Linearized Smart Factors
|
||||
|
||||
The factors below are less likely to be relevant to the user, but result from using the non-linear smart factors above.
|
||||
|
||||
|
||||
### RegularImplicitSchurFactor
|
||||
|
||||
A specialization of a GaussianFactor to structure-less SFM, which is very fast in a conjugate gradient (CG) solver.
|
||||
It is produced by calling `createRegularImplicitSchurFactor` in `SmartFactorBase` or `SmartProjectionFactor`.
|
||||
|
||||
### JacobianFactorQ
|
||||
|
||||
A RegularJacobianFactor that uses some badly documented reduction on the Jacobians.
|
||||
|
||||
### JacobianFactorQR
|
||||
|
||||
A RegularJacobianFactor that eliminates a point using sequential elimination.
|
||||
|
||||
### JacobianFactorQR
|
||||
|
||||
A RegularJacobianFactor that uses the "Nullspace Trick" by Mourikis et al. See the documentation in the file, which *is* well documented.
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
/**
|
||||
* @file RegularImplicitSchurFactor.h
|
||||
* @brief A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor
|
||||
* @brief A subclass of GaussianFactor specialized to structureless SFM.
|
||||
* @author Frank Dellaert
|
||||
* @author Luca Carlone
|
||||
*/
|
||||
|
|
@ -20,6 +20,20 @@ namespace gtsam {
|
|||
|
||||
/**
|
||||
* RegularImplicitSchurFactor
|
||||
*
|
||||
* A specialization of a GaussianFactor to structure-less SFM, which is very
|
||||
* fast in a conjugate gradient (CG) solver. Specifically, as measured in
|
||||
* timeSchurFactors.cpp, it stays very fast for an increasing number of cameras.
|
||||
* The magic is in multiplyHessianAdd, which does the Hessian-vector multiply at
|
||||
* the core of CG, and implements
|
||||
* y += F'*alpha*(I - E*P*E')*F*x
|
||||
* where
|
||||
* - F is the 2mx6m Jacobian of the m 2D measurements wrpt m 6DOF poses
|
||||
* - E is the 2mx3 Jacabian of the m 2D measurements wrpt a 3D point
|
||||
* - P is the covariance on the point
|
||||
* The equation above implicitly executes the Schur complement by removing the
|
||||
* information E*P*E' from the Hessian. It is also very fast as we do not use
|
||||
* the full 6m*6m F matrix, but rather only it's m 6x6 diagonal blocks.
|
||||
*/
|
||||
template<class CAMERA>
|
||||
class RegularImplicitSchurFactor: public GaussianFactor {
|
||||
|
|
@ -38,9 +52,10 @@ protected:
|
|||
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
|
||||
|
||||
typedef Eigen::Matrix<double, ZDim, D> MatrixZD; ///< type of an F block
|
||||
typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
|
||||
typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera Hessian
|
||||
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;
|
||||
|
||||
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks_; ///< All ZDim*D F blocks (one for each camera)
|
||||
FBlocks FBlocks_; ///< All ZDim*D F blocks (one for each camera)
|
||||
const Matrix PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
|
||||
const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point
|
||||
const Vector b_; ///< 2m-dimensional RHS vector
|
||||
|
|
@ -52,17 +67,25 @@ public:
|
|||
}
|
||||
|
||||
/// Construct from blocks of F, E, inv(E'*E), and RHS vector b
|
||||
RegularImplicitSchurFactor(const KeyVector& keys,
|
||||
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix& P,
|
||||
const Vector& b) :
|
||||
GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) {
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Construct a new RegularImplicitSchurFactor object.
|
||||
*
|
||||
* @param keys keys corresponding to cameras
|
||||
* @param Fs All ZDim*D F blocks (one for each camera)
|
||||
* @param E Jacobian of measurements wrpt point.
|
||||
* @param P point covariance matrix
|
||||
* @param b RHS vector
|
||||
*/
|
||||
RegularImplicitSchurFactor(const KeyVector& keys, const FBlocks& Fs,
|
||||
const Matrix& E, const Matrix& P, const Vector& b)
|
||||
: GaussianFactor(keys), FBlocks_(Fs), PointCovariance_(P), E_(E), b_(b) {}
|
||||
|
||||
/// Destructor
|
||||
~RegularImplicitSchurFactor() override {
|
||||
}
|
||||
|
||||
std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks() const {
|
||||
const FBlocks& Fs() const {
|
||||
return FBlocks_;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -37,12 +37,14 @@
|
|||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* @brief Base class for smart factors
|
||||
* @brief Base class for smart factors.
|
||||
* This base class has no internal point, but it has a measurement, noise model
|
||||
* and an optional sensor pose.
|
||||
* This class mainly computes the derivatives and returns them as a variety of factors.
|
||||
* The methods take a Cameras argument, which should behave like PinholeCamera, and
|
||||
* the value of a point, which is kept in the base class.
|
||||
* This class mainly computes the derivatives and returns them as a variety of
|
||||
* factors. The methods take a CameraSet<CAMERA> argument and the value of a
|
||||
* point, which is kept in the derived class.
|
||||
*
|
||||
* @tparam CAMERA should behave like a PinholeCamera.
|
||||
*/
|
||||
template<class CAMERA>
|
||||
class SmartFactorBase: public NonlinearFactor {
|
||||
|
|
@ -64,19 +66,20 @@ protected:
|
|||
/**
|
||||
* As of Feb 22, 2015, the noise model is the same for all measurements and
|
||||
* is isotropic. This allows for moving most calculations of Schur complement
|
||||
* etc to be moved to CameraSet very easily, and also agrees pragmatically
|
||||
* etc. to be easily moved to CameraSet, and also agrees pragmatically
|
||||
* with what is normally done.
|
||||
*/
|
||||
SharedIsotropic noiseModel_;
|
||||
|
||||
/**
|
||||
* 2D measurement and noise model for each of the m views
|
||||
* We keep a copy of measurements for I/O and computing the error.
|
||||
* Measurements for each of the m views.
|
||||
* We keep a copy of the measurements for I/O and computing the error.
|
||||
* The order is kept the same as the keys that we use to create the factor.
|
||||
*/
|
||||
ZVector measured_;
|
||||
|
||||
boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame
|
||||
boost::optional<Pose3>
|
||||
body_P_sensor_; ///< Pose of the camera in the body frame
|
||||
|
||||
// Cache for Fblocks, to avoid a malloc ever time we re-linearize
|
||||
mutable FBlocks Fs;
|
||||
|
|
@ -84,16 +87,16 @@ protected:
|
|||
public:
|
||||
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
/// shorthand for a smart pointer to a factor.
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/// We use the new CameraSte data structure to refer to a set of cameras
|
||||
/// The CameraSet data structure is used to refer to a set of cameras.
|
||||
typedef CameraSet<CAMERA> Cameras;
|
||||
|
||||
/// Default Constructor, for serialization
|
||||
/// Default Constructor, for serialization.
|
||||
SmartFactorBase() {}
|
||||
|
||||
/// Constructor
|
||||
/// Construct with given noise model and optional arguments.
|
||||
SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
|
||||
boost::optional<Pose3> body_P_sensor = boost::none,
|
||||
size_t expectedNumberCameras = 10)
|
||||
|
|
@ -111,12 +114,12 @@ protected:
|
|||
noiseModel_ = sharedIsotropic;
|
||||
}
|
||||
|
||||
/// Virtual destructor, subclasses from NonlinearFactor
|
||||
/// Virtual destructor, subclasses from NonlinearFactor.
|
||||
~SmartFactorBase() override {
|
||||
}
|
||||
|
||||
/**
|
||||
* Add a new measurement and pose/camera key
|
||||
* Add a new measurement and pose/camera key.
|
||||
* @param measured is the 2m dimensional projection of a single landmark
|
||||
* @param key is the index corresponding to the camera observing the landmark
|
||||
*/
|
||||
|
|
@ -129,9 +132,7 @@ protected:
|
|||
this->keys_.push_back(key);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add a bunch of measurements, together with the camera keys
|
||||
*/
|
||||
/// Add a bunch of measurements, together with the camera keys.
|
||||
void add(const ZVector& measurements, const KeyVector& cameraKeys) {
|
||||
assert(measurements.size() == cameraKeys.size());
|
||||
for (size_t i = 0; i < measurements.size(); i++) {
|
||||
|
|
@ -140,8 +141,8 @@ protected:
|
|||
}
|
||||
|
||||
/**
|
||||
* Adds an entire SfM_track (collection of cameras observing a single point).
|
||||
* The noise is assumed to be the same for all measurements
|
||||
* Add an entire SfM_track (collection of cameras observing a single point).
|
||||
* The noise is assumed to be the same for all measurements.
|
||||
*/
|
||||
template<class SFM_TRACK>
|
||||
void add(const SFM_TRACK& trackToAdd) {
|
||||
|
|
@ -151,17 +152,13 @@ protected:
|
|||
}
|
||||
}
|
||||
|
||||
/// get the dimension (number of rows!) of the factor
|
||||
size_t dim() const override {
|
||||
return ZDim * this->measured_.size();
|
||||
}
|
||||
/// Return the dimension (number of rows!) of the factor.
|
||||
size_t dim() const override { return ZDim * this->measured_.size(); }
|
||||
|
||||
/** return the measurements */
|
||||
const ZVector& measured() const {
|
||||
return measured_;
|
||||
}
|
||||
/// Return the 2D measurements (ZDim, in general).
|
||||
const ZVector& measured() const { return measured_; }
|
||||
|
||||
/// Collect all cameras: important that in key order
|
||||
/// Collect all cameras: important that in key order.
|
||||
virtual Cameras cameras(const Values& values) const {
|
||||
Cameras cameras;
|
||||
for(const Key& k: this->keys_)
|
||||
|
|
@ -188,25 +185,30 @@ protected:
|
|||
|
||||
/// equals
|
||||
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
|
||||
const This *e = dynamic_cast<const This*>(&p);
|
||||
|
||||
bool areMeasurementsEqual = true;
|
||||
for (size_t i = 0; i < measured_.size(); i++) {
|
||||
if (traits<Z>::Equals(this->measured_.at(i), e->measured_.at(i), tol) == false)
|
||||
areMeasurementsEqual = false;
|
||||
break;
|
||||
if (const This* e = dynamic_cast<const This*>(&p)) {
|
||||
// Check that all measurements are the same.
|
||||
for (size_t i = 0; i < measured_.size(); i++) {
|
||||
if (!traits<Z>::Equals(this->measured_.at(i), e->measured_.at(i), tol))
|
||||
return false;
|
||||
}
|
||||
// If so, check base class.
|
||||
return Base::equals(p, tol);
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
return e && Base::equals(p, tol) && areMeasurementsEqual;
|
||||
}
|
||||
|
||||
/// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and
|
||||
/// derivatives
|
||||
/// derivatives. This is the error before the noise model is applied.
|
||||
template <class POINT>
|
||||
Vector unwhitenedError(
|
||||
const Cameras& cameras, const POINT& point,
|
||||
boost::optional<typename Cameras::FBlocks&> Fs = boost::none, //
|
||||
boost::optional<Matrix&> E = boost::none) const {
|
||||
Vector ue = cameras.reprojectionError(point, measured_, Fs, E);
|
||||
// Reproject, with optional derivatives.
|
||||
Vector error = cameras.reprojectionError(point, measured_, Fs, E);
|
||||
|
||||
// Apply chain rule if body_P_sensor_ is given.
|
||||
if (body_P_sensor_ && Fs) {
|
||||
const Pose3 sensor_P_body = body_P_sensor_->inverse();
|
||||
constexpr int camera_dim = traits<CAMERA>::dimension;
|
||||
|
|
@ -224,52 +226,60 @@ protected:
|
|||
Fs->at(i) = Fs->at(i) * J;
|
||||
}
|
||||
}
|
||||
correctForMissingMeasurements(cameras, ue, Fs, E);
|
||||
return ue;
|
||||
|
||||
// Correct the Jacobians in case some measurements are missing.
|
||||
correctForMissingMeasurements(cameras, error, Fs, E);
|
||||
|
||||
return error;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
* This corrects the Jacobians for the case in which some 2D 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 {}
|
||||
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
|
||||
* Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) -
|
||||
* z], with the noise model applied.
|
||||
*/
|
||||
template<class POINT>
|
||||
Vector whitenedError(const Cameras& cameras, const POINT& point) const {
|
||||
Vector e = cameras.reprojectionError(point, measured_);
|
||||
Vector error = cameras.reprojectionError(point, measured_);
|
||||
if (noiseModel_)
|
||||
noiseModel_->whitenInPlace(e);
|
||||
return e;
|
||||
noiseModel_->whitenInPlace(error);
|
||||
return error;
|
||||
}
|
||||
|
||||
/** Calculate the error of the factor.
|
||||
* This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
|
||||
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
|
||||
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
|
||||
* Will be used in "error(Values)" function required by NonlinearFactor base class
|
||||
/**
|
||||
* Calculate the error of the factor.
|
||||
* This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of
|
||||
* Gaussian. In this class, we take the raw prediction error \f$ h(x)-z \f$,
|
||||
* ask the noise model to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and
|
||||
* then multiply by 0.5. Will be used in "error(Values)" function required by
|
||||
* NonlinearFactor base class
|
||||
*/
|
||||
template<class POINT>
|
||||
double totalReprojectionError(const Cameras& cameras,
|
||||
const POINT& point) const {
|
||||
Vector e = whitenedError(cameras, point);
|
||||
return 0.5 * e.dot(e);
|
||||
Vector error = whitenedError(cameras, point);
|
||||
return 0.5 * error.dot(error);
|
||||
}
|
||||
|
||||
/// Computes Point Covariance P from E
|
||||
static Matrix PointCov(Matrix& E) {
|
||||
/// Computes Point Covariance P from the "point Jacobian" E.
|
||||
static Matrix PointCov(const Matrix& E) {
|
||||
return (E.transpose() * E).inverse();
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute F, E, and b (called below in both vanilla and SVD versions), where
|
||||
* F is a vector of derivatives wrpt the cameras, and E the stacked derivatives
|
||||
* with respect to the point. The value of cameras/point are passed as parameters.
|
||||
* TODO: Kill this obsolete method
|
||||
* Compute F, E, and b (called below in both vanilla and SVD versions), where
|
||||
* F is a vector of derivatives wrpt the cameras, and E the stacked
|
||||
* derivatives with respect to the point. The value of cameras/point are
|
||||
* passed as parameters.
|
||||
*/
|
||||
template<class POINT>
|
||||
void computeJacobians(FBlocks& Fs, Matrix& E, Vector& b,
|
||||
|
|
@ -281,7 +291,11 @@ protected:
|
|||
b = -unwhitenedError(cameras, point, Fs, E);
|
||||
}
|
||||
|
||||
/// SVD version
|
||||
/**
|
||||
* SVD version that produces smaller Jacobian matrices by doing an SVD
|
||||
* decomposition on E, and returning the left nulkl-space of E.
|
||||
* See JacobianFactorSVD for more documentation.
|
||||
* */
|
||||
template<class POINT>
|
||||
void computeJacobiansSVD(FBlocks& Fs, Matrix& Enull,
|
||||
Vector& b, const Cameras& cameras, const POINT& point) const {
|
||||
|
|
@ -291,14 +305,14 @@ protected:
|
|||
|
||||
static const int N = FixedDimension<POINT>::value; // 2 (Unit3) or 3 (Point3)
|
||||
|
||||
// Do SVD on A
|
||||
// Do SVD on A.
|
||||
Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
|
||||
Vector s = svd.singularValues();
|
||||
size_t m = this->keys_.size();
|
||||
Enull = svd.matrixU().block(0, N, ZDim * m, ZDim * m - N); // last ZDim*m-N columns
|
||||
}
|
||||
|
||||
/// Linearize to a Hessianfactor
|
||||
/// Linearize to a Hessianfactor.
|
||||
// TODO(dellaert): Not used/tested anywhere and not properly whitened.
|
||||
boost::shared_ptr<RegularHessianFactor<Dim> > createHessianFactor(
|
||||
const Cameras& cameras, const Point3& point, const double lambda = 0.0,
|
||||
bool diagonalDamping = false) const {
|
||||
|
|
@ -351,9 +365,7 @@ protected:
|
|||
P, b);
|
||||
}
|
||||
|
||||
/**
|
||||
* Return Jacobians as JacobianFactorQ
|
||||
*/
|
||||
/// Return Jacobians as JacobianFactorQ.
|
||||
boost::shared_ptr<JacobianFactorQ<Dim, ZDim> > createJacobianQFactor(
|
||||
const Cameras& cameras, const Point3& point, double lambda = 0.0,
|
||||
bool diagonalDamping = false) const {
|
||||
|
|
@ -368,8 +380,8 @@ protected:
|
|||
}
|
||||
|
||||
/**
|
||||
* Return Jacobians as JacobianFactorSVD
|
||||
* TODO lambda is currently ignored
|
||||
* Return Jacobians as JacobianFactorSVD.
|
||||
* TODO(dellaert): lambda is currently ignored
|
||||
*/
|
||||
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
|
||||
const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
|
||||
|
|
@ -393,6 +405,7 @@ protected:
|
|||
F.block<ZDim, Dim>(ZDim * i, Dim * i) = Fs.at(i);
|
||||
}
|
||||
|
||||
// Return sensor pose.
|
||||
Pose3 body_P_sensor() const{
|
||||
if(body_P_sensor_)
|
||||
return *body_P_sensor_;
|
||||
|
|
|
|||
|
|
@ -61,10 +61,11 @@ protected:
|
|||
/// @name Caching triangulation
|
||||
/// @{
|
||||
mutable TriangulationResult result_; ///< result from triangulateSafe
|
||||
mutable std::vector<Pose3, Eigen::aligned_allocator<Pose3> > cameraPosesTriangulation_; ///< current triangulation poses
|
||||
mutable std::vector<Pose3, Eigen::aligned_allocator<Pose3> >
|
||||
cameraPosesTriangulation_; ///< current triangulation poses
|
||||
/// @}
|
||||
|
||||
public:
|
||||
public:
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
|
@ -116,21 +117,31 @@ public:
|
|||
&& Base::equals(p, tol);
|
||||
}
|
||||
|
||||
/// Check if the new linearization point is the same as the one used for previous triangulation
|
||||
/**
|
||||
* @brief Check if the new linearization point is the same as the one used for
|
||||
* previous triangulation.
|
||||
*
|
||||
* @param cameras
|
||||
* @return true if we need to re-triangulate.
|
||||
*/
|
||||
bool decideIfTriangulate(const Cameras& cameras) const {
|
||||
// several calls to linearize will be done from the same linearization point, hence it is not needed to re-triangulate
|
||||
// Note that this is not yet "selecting linearization", that will come later, and we only check if the
|
||||
// current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point
|
||||
// Several calls to linearize will be done from the same linearization
|
||||
// point, hence it is not needed to re-triangulate. Note that this is not
|
||||
// yet "selecting linearization", that will come later, and we only check if
|
||||
// the current linearization is the "same" (up to tolerance) w.r.t. the last
|
||||
// time we triangulated the point.
|
||||
|
||||
size_t m = cameras.size();
|
||||
|
||||
bool retriangulate = false;
|
||||
|
||||
// if we do not have a previous linearization point or the new linearization point includes more poses
|
||||
// Definitely true if we do not have a previous linearization point or the
|
||||
// new linearization point includes more poses.
|
||||
if (cameraPosesTriangulation_.empty()
|
||||
|| cameras.size() != cameraPosesTriangulation_.size())
|
||||
retriangulate = true;
|
||||
|
||||
// Otherwise, check poses against cache.
|
||||
if (!retriangulate) {
|
||||
for (size_t i = 0; i < cameras.size(); i++) {
|
||||
if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
|
||||
|
|
@ -141,7 +152,8 @@ public:
|
|||
}
|
||||
}
|
||||
|
||||
if (retriangulate) { // we store the current poses used for triangulation
|
||||
// Store the current poses used for triangulation if we will re-triangulate.
|
||||
if (retriangulate) {
|
||||
cameraPosesTriangulation_.clear();
|
||||
cameraPosesTriangulation_.reserve(m);
|
||||
for (size_t i = 0; i < m; i++)
|
||||
|
|
@ -149,10 +161,15 @@ public:
|
|||
cameraPosesTriangulation_.push_back(cameras[i].pose());
|
||||
}
|
||||
|
||||
return retriangulate; // if we arrive to this point all poses are the same and we don't need re-triangulation
|
||||
return retriangulate;
|
||||
}
|
||||
|
||||
/// triangulateSafe
|
||||
/**
|
||||
* @brief Call gtsam::triangulateSafe iff we need to re-triangulate.
|
||||
*
|
||||
* @param cameras
|
||||
* @return TriangulationResult
|
||||
*/
|
||||
TriangulationResult triangulateSafe(const Cameras& cameras) const {
|
||||
|
||||
size_t m = cameras.size();
|
||||
|
|
@ -166,17 +183,21 @@ public:
|
|||
return result_;
|
||||
}
|
||||
|
||||
/// triangulate
|
||||
/**
|
||||
* @brief Possibly re-triangulate before calculating Jacobians.
|
||||
*
|
||||
* @param cameras
|
||||
* @return true if we could safely triangulate
|
||||
*/
|
||||
bool triangulateForLinearize(const Cameras& cameras) const {
|
||||
triangulateSafe(cameras); // imperative, might reset result_
|
||||
return bool(result_);
|
||||
}
|
||||
|
||||
/// linearize returns a Hessianfactor that is an approximation of error(p)
|
||||
/// Create a Hessianfactor that is an approximation of error(p).
|
||||
boost::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor(
|
||||
const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping =
|
||||
false) const {
|
||||
|
||||
const Cameras& cameras, const double lambda = 0.0,
|
||||
bool diagonalDamping = false) const {
|
||||
size_t numKeys = this->keys_.size();
|
||||
// Create structures for Hessian Factors
|
||||
KeyVector js;
|
||||
|
|
@ -184,39 +205,38 @@ public:
|
|||
std::vector<Vector> gs(numKeys);
|
||||
|
||||
if (this->measured_.size() != cameras.size())
|
||||
throw std::runtime_error("SmartProjectionHessianFactor: this->measured_"
|
||||
".size() inconsistent with input");
|
||||
throw std::runtime_error(
|
||||
"SmartProjectionHessianFactor: this->measured_"
|
||||
".size() inconsistent with input");
|
||||
|
||||
triangulateSafe(cameras);
|
||||
|
||||
if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
|
||||
// failed: return"empty" Hessian
|
||||
for(Matrix& m: Gs)
|
||||
m = Matrix::Zero(Base::Dim, Base::Dim);
|
||||
for(Vector& v: gs)
|
||||
v = Vector::Zero(Base::Dim);
|
||||
for (Matrix& m : Gs) m = Matrix::Zero(Base::Dim, Base::Dim);
|
||||
for (Vector& v : gs) v = Vector::Zero(Base::Dim);
|
||||
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
|
||||
Gs, gs, 0.0);
|
||||
Gs, gs, 0.0);
|
||||
}
|
||||
|
||||
// Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
|
||||
std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> > Fblocks;
|
||||
typename Base::FBlocks Fs;
|
||||
Matrix E;
|
||||
Vector b;
|
||||
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
|
||||
computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
|
||||
|
||||
// Whiten using noise model
|
||||
Base::whitenJacobians(Fblocks, E, b);
|
||||
Base::whitenJacobians(Fs, E, b);
|
||||
|
||||
// build augmented hessian
|
||||
SymmetricBlockMatrix augmentedHessian = //
|
||||
Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping);
|
||||
SymmetricBlockMatrix augmentedHessian = //
|
||||
Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping);
|
||||
|
||||
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
|
||||
augmentedHessian);
|
||||
return boost::make_shared<RegularHessianFactor<Base::Dim> >(
|
||||
this->keys_, augmentedHessian);
|
||||
}
|
||||
|
||||
// create factor
|
||||
// Create RegularImplicitSchurFactor factor.
|
||||
boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > createRegularImplicitSchurFactor(
|
||||
const Cameras& cameras, double lambda) const {
|
||||
if (triangulateForLinearize(cameras))
|
||||
|
|
@ -226,7 +246,7 @@ public:
|
|||
return boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> >();
|
||||
}
|
||||
|
||||
/// create factor
|
||||
/// Create JacobianFactorQ factor.
|
||||
boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
|
||||
const Cameras& cameras, double lambda) const {
|
||||
if (triangulateForLinearize(cameras))
|
||||
|
|
@ -236,13 +256,13 @@ public:
|
|||
return boost::make_shared<JacobianFactorQ<Base::Dim, 2> >(this->keys_);
|
||||
}
|
||||
|
||||
/// Create a factor, takes values
|
||||
/// Create JacobianFactorQ factor, takes values.
|
||||
boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
|
||||
const Values& values, double lambda) const {
|
||||
return createJacobianQFactor(this->cameras(values), lambda);
|
||||
}
|
||||
|
||||
/// different (faster) way to compute Jacobian factor
|
||||
/// Different (faster) way to compute a JacobianFactorSVD factor.
|
||||
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
|
||||
const Cameras& cameras, double lambda) const {
|
||||
if (triangulateForLinearize(cameras))
|
||||
|
|
@ -252,19 +272,19 @@ public:
|
|||
return boost::make_shared<JacobianFactorSVD<Base::Dim, 2> >(this->keys_);
|
||||
}
|
||||
|
||||
/// linearize to a Hessianfactor
|
||||
/// Linearize to a Hessianfactor.
|
||||
virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
|
||||
const Values& values, double lambda = 0.0) const {
|
||||
return createHessianFactor(this->cameras(values), lambda);
|
||||
}
|
||||
|
||||
/// linearize to an Implicit Schur factor
|
||||
/// Linearize to an Implicit Schur factor.
|
||||
virtual boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > linearizeToImplicit(
|
||||
const Values& values, double lambda = 0.0) const {
|
||||
return createRegularImplicitSchurFactor(this->cameras(values), lambda);
|
||||
}
|
||||
|
||||
/// linearize to a JacobianfactorQ
|
||||
/// Linearize to a JacobianfactorQ.
|
||||
virtual boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > linearizeToJacobian(
|
||||
const Values& values, double lambda = 0.0) const {
|
||||
return createJacobianQFactor(this->cameras(values), lambda);
|
||||
|
|
@ -334,7 +354,7 @@ public:
|
|||
/// Assumes the point has been computed
|
||||
/// Note E can be 2m*3 or 2m*2, in case point is degenerate
|
||||
void computeJacobiansWithTriangulatedPoint(
|
||||
std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> >& Fblocks, Matrix& E, Vector& b,
|
||||
typename Base::FBlocks& Fs, Matrix& E, Vector& b,
|
||||
const Cameras& cameras) const {
|
||||
|
||||
if (!result_) {
|
||||
|
|
@ -342,32 +362,32 @@ public:
|
|||
// TODO check flag whether we should do this
|
||||
Unit3 backProjected = cameras[0].backprojectPointAtInfinity(
|
||||
this->measured_.at(0));
|
||||
Base::computeJacobians(Fblocks, E, b, cameras, backProjected);
|
||||
Base::computeJacobians(Fs, E, b, cameras, backProjected);
|
||||
} else {
|
||||
// valid result: just return Base version
|
||||
Base::computeJacobians(Fblocks, E, b, cameras, *result_);
|
||||
Base::computeJacobians(Fs, E, b, cameras, *result_);
|
||||
}
|
||||
}
|
||||
|
||||
/// Version that takes values, and creates the point
|
||||
bool triangulateAndComputeJacobians(
|
||||
std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> >& Fblocks, Matrix& E, Vector& b,
|
||||
typename Base::FBlocks& Fs, Matrix& E, Vector& b,
|
||||
const Values& values) const {
|
||||
Cameras cameras = this->cameras(values);
|
||||
bool nonDegenerate = triangulateForLinearize(cameras);
|
||||
if (nonDegenerate)
|
||||
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
|
||||
computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
|
||||
return nonDegenerate;
|
||||
}
|
||||
|
||||
/// takes values
|
||||
bool triangulateAndComputeJacobiansSVD(
|
||||
std::vector<typename Base::MatrixZD, Eigen::aligned_allocator<typename Base::MatrixZD> >& Fblocks, Matrix& Enull, Vector& b,
|
||||
typename Base::FBlocks& Fs, Matrix& Enull, Vector& b,
|
||||
const Values& values) const {
|
||||
Cameras cameras = this->cameras(values);
|
||||
bool nonDegenerate = triangulateForLinearize(cameras);
|
||||
if (nonDegenerate)
|
||||
Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_);
|
||||
Base::computeJacobiansSVD(Fs, Enull, b, cameras, *result_);
|
||||
return nonDegenerate;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -71,8 +71,6 @@ class SmartProjectionFactorP : public SmartProjectionFactor<CAMERA> {
|
|||
public:
|
||||
typedef CAMERA Camera;
|
||||
typedef CameraSet<CAMERA> Cameras;
|
||||
typedef Eigen::Matrix<double, ZDim, DimPose> MatrixZD; // F blocks
|
||||
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
|
@ -234,7 +232,8 @@ class SmartProjectionFactorP : public SmartProjectionFactor<CAMERA> {
|
|||
* respect to both body poses we interpolate from), the point Jacobian E,
|
||||
* and the error vector b. Note that the jacobians are computed for a given point.
|
||||
*/
|
||||
void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b,
|
||||
void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks& Fs,
|
||||
Matrix& E, Vector& b,
|
||||
const Cameras& cameras) const {
|
||||
if (!this->result_) {
|
||||
throw("computeJacobiansWithTriangulatedPoint");
|
||||
|
|
@ -289,7 +288,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor<CAMERA> {
|
|||
}
|
||||
|
||||
// compute Jacobian given triangulated 3D Point
|
||||
FBlocks Fs;
|
||||
typename Base::FBlocks Fs;
|
||||
Matrix E;
|
||||
Vector b;
|
||||
this->computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
|
||||
|
|
@ -300,7 +299,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor<CAMERA> {
|
|||
Fs[i] = this->noiseModel_->Whiten(Fs[i]);
|
||||
}
|
||||
|
||||
Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
|
||||
const Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
|
||||
|
||||
// Build augmented Hessian (with last row/column being the information vector)
|
||||
// Note: we need to get the augumented hessian wrt the unique keys in key_
|
||||
|
|
|
|||
|
|
@ -0,0 +1,40 @@
|
|||
# SLAM Factors
|
||||
|
||||
## SmartFactors
|
||||
|
||||
These are "structure-less" factors, i.e., rather than introducing a new variable for an observed 3D point or landmark, a single factor is created that provides a multi-view constraint on several poses and/or cameras.
|
||||
|
||||
### SmartRangeFactor
|
||||
|
||||
An experiment in creating a structure-less 2D range-SLAM factor with range-only measurements.
|
||||
It uses a sophisticated `triangulate` logic based on circle intersections.
|
||||
|
||||
### SmartStereoProjectionFactor
|
||||
|
||||
Version of `SmartProjectionFactor` for stereo observations, specializes SmartFactorBase for `CAMERA == StereoCamera`.
|
||||
|
||||
TODO: a lot of commented out code and could move a lot to .cpp file.
|
||||
|
||||
### SmartStereoProjectionPoseFactor
|
||||
|
||||
Derives from `SmartStereoProjectionFactor` but adds an array of `Cal3_S2Stereo` calibration objects .
|
||||
|
||||
TODO: Again, as no template arguments, we could move a lot to .cpp file.
|
||||
|
||||
### SmartStereoProjectionFactorPP
|
||||
|
||||
Similar `SmartStereoProjectionPoseFactor` but *additionally* adds an array of body_P_cam poses. The dimensions seem to be hardcoded and the types defined in the SmartFactorBase have been re-defined.
|
||||
The body_P_cam poses are optimized here!
|
||||
|
||||
TODO: See above, same issues as `SmartStereoProjectionPoseFactor`.
|
||||
|
||||
### SmartProjectionPoseFactorRollingShutter
|
||||
|
||||
Is templated on a `CAMERA` type and derives from `SmartProjectionFactor`.
|
||||
|
||||
This factor optimizes two consecutive poses of a body assuming a rolling
|
||||
shutter model of the camera with given readout time. The factor requires that
|
||||
values contain (for each 2D observation) two consecutive camera poses from
|
||||
which the 2D observation pose can be interpolated.
|
||||
|
||||
TODO: the dimensions seem to be hardcoded and the types defined in the SmartFactorBase have been re-defined. Also, possibly a lot of copy/paste computation of things that (should) happen in base class.
|
||||
|
|
@ -447,23 +447,23 @@ public:
|
|||
}
|
||||
|
||||
/**
|
||||
* This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan)
|
||||
* This corrects the Jacobians and error vector for the case in which the
|
||||
* right 2D measurement in the monocular camera is missing (nan).
|
||||
*/
|
||||
void correctForMissingMeasurements(const Cameras& cameras, Vector& ue,
|
||||
boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
|
||||
boost::optional<Matrix&> E = boost::none) const override
|
||||
{
|
||||
void correctForMissingMeasurements(
|
||||
const Cameras& cameras, Vector& ue,
|
||||
boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
|
||||
boost::optional<Matrix&> E = boost::none) const override {
|
||||
// when using stereo cameras, some of the measurements might be missing:
|
||||
for(size_t i=0; i < cameras.size(); i++){
|
||||
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 (std::isnan(z.uR())) // if the right 2D measurement is invalid
|
||||
{
|
||||
if(Fs){ // delete influence of right point on jacobian Fs
|
||||
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;
|
||||
for (size_t ii = 0; ii < Dim; ii++) Fi(1, ii) = 0.0;
|
||||
}
|
||||
if(E) // delete influence of right point on jacobian E
|
||||
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
|
||||
|
|
|
|||
|
|
@ -33,9 +33,11 @@ namespace gtsam {
|
|||
*/
|
||||
|
||||
/**
|
||||
* This factor optimizes the pose of the body as well as the extrinsic camera calibration (pose of camera wrt body).
|
||||
* Each camera may have its own extrinsic calibration or the same calibration can be shared by multiple cameras.
|
||||
* This factor requires that values contain the involved poses and extrinsics (both are Pose3 variables).
|
||||
* This factor optimizes the pose of the body as well as the extrinsic camera
|
||||
* calibration (pose of camera wrt body). Each camera may have its own extrinsic
|
||||
* calibration or the same calibration can be shared by multiple cameras. This
|
||||
* factor requires that values contain the involved poses and extrinsics (both
|
||||
* are Pose3 variables).
|
||||
* @addtogroup SLAM
|
||||
*/
|
||||
class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
||||
|
|
|
|||
Loading…
Reference in New Issue