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

Merge branch 'feature/cameraTemplateForAllSmartFactors' into feature/sphericalCamera 

# Conflicts:
#	gtsam/slam/SmartProjectionFactorP.h
#	gtsam/slam/tests/smartFactorScenarios.h
#	gtsam/slam/tests/testSmartProjectionRigFactor.cpp
#	gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
#	gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
release/4.3a0
lcarlone 2021-11-07 14:23:14 -05:00
parent 8546a71d33 2e8d373ff5
commit 2f57a1a307
15 changed files with 1872 additions and 1233 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
gtsam/slam/JacobianFactorSVD.h
View File

@ -9,20 +9,21 @@
namespace gtsam { 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. * 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, * In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are
* is multiplied by Enull, a matrix that spans the left nullspace of E, i.e., * poses, is multiplied by Enull, a matrix that spans the left nullspace of E,
* The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm * mx3 * 3x3) * i.e., The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm *
* where Enull is an m x (m-3) matrix * mx3 * 3x3) where Enull is an m x (m-3) matrix Then Enull'*E*dp = 0, and
* Then Enull'*E*dp = 0, and
* |Enull'*E*dp + Enull'*F*dX - Enull'*b| == |Enull'*F*dX - Enull'*b| * |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. * 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. * The code below assumes that F is block diagonal and is given as a vector of
* Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for D=6) * ZDim*D blocks. Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for
* Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as a 1x2 * 2x6 mult * 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> template<size_t D, size_t ZDim>
class JacobianFactorSVD: public RegularJacobianFactor<D> { class JacobianFactorSVD: public RegularJacobianFactor<D> {
@ -37,10 +38,10 @@ public:
JacobianFactorSVD() { JacobianFactorSVD() {
} }
/// Empty constructor with keys /// Empty constructor with keys.
JacobianFactorSVD(const KeyVector& keys, // JacobianFactorSVD(const KeyVector& keys,
const SharedDiagonal& model = SharedDiagonal()) : const SharedDiagonal& model = SharedDiagonal())
Base() { : Base() {
Matrix zeroMatrix = Matrix::Zero(0, D); Matrix zeroMatrix = Matrix::Zero(0, D);
Vector zeroVector = Vector::Zero(0); Vector zeroVector = Vector::Zero(0);
std::vector<KeyMatrix> QF; std::vector<KeyMatrix> QF;
@ -51,18 +52,21 @@ public:
} }
/** /**
* @brief Constructor * @brief Construct a new JacobianFactorSVD object, createing a reduced-rank
* Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F) * Jacobian factor on the CameraSet.
* and a reduced point derivative, Enull
* and creates 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, JacobianFactorSVD(
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks, const Matrix& Enull, const KeyVector& keys,
const Vector& b, // const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks,
const SharedDiagonal& model = SharedDiagonal()) : const Matrix& Enull, const Vector& b,
Base() { const SharedDiagonal& model = SharedDiagonal())
: Base() {
size_t numKeys = Enull.rows() / ZDim; size_t numKeys = Enull.rows() / ZDim;
size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()? size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()?
// PLAIN nullptr SPACE TRICK // PLAIN nullptr SPACE TRICK
@ -74,9 +78,8 @@ public:
QF.reserve(numKeys); QF.reserve(numKeys);
for (size_t k = 0; k < Fblocks.size(); ++k) { for (size_t k = 0; k < Fblocks.size(); ++k) {
Key key = keys[k]; Key key = keys[k];
QF.push_back( QF.emplace_back(
KeyMatrix(key, key, (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]);
(Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]));
} }
JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
} }

18
gtsam/slam/ProjectionFactor.h
View File

@ -11,7 +11,7 @@
/** /**
* @file ProjectionFactor.h * @file ProjectionFactor.h
* @brief Basic bearing factor from 2D measurement * @brief Reprojection of a LANDMARK to a 2D point.
* @author Chris Beall * @author Chris Beall
* @author Richard Roberts * @author Richard Roberts
* @author Frank Dellaert * @author Frank Dellaert
@ -22,17 +22,21 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <boost/optional.hpp> #include <boost/optional.hpp>
namespace gtsam { namespace gtsam {
/** /**
* Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. * Non-linear factor for a constraint derived from a 2D measurement.
* i.e. the main building block for visual SLAM. * The calibration is known here.
* The main building block for visual SLAM.
* @addtogroup 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> { class GenericProjectionFactor: public NoiseModelFactor2<POSE, LANDMARK> {
protected: protected:
@ -57,9 +61,9 @@ namespace gtsam {
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor /// Default constructor
GenericProjectionFactor() : GenericProjectionFactor() :
measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) { measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
} }
/** /**
* Constructor * Constructor

68
gtsam/slam/ReadMe.md Normal file
View File

@ -0,0 +1,68 @@
# 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!
### SmartProjectionRigFactor
Same as `SmartProjectionPoseFactor`, except:
- it is templated on `CAMERA`, i.e., it allows cameras beyond pinhole;
- it allows measurements from multiple cameras, each camera with fixed but potentially different intrinsics and extrinsics;
- it allows multiple observations from the same pose/key, again, to model a multi-camera system.
## 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.

41
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -1,6 +1,6 @@
/** /**
* @file RegularImplicitSchurFactor.h * @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 Frank Dellaert
* @author Luca Carlone * @author Luca Carlone
*/ */
@ -20,6 +20,20 @@ namespace gtsam {
/** /**
* RegularImplicitSchurFactor * 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> template<class CAMERA>
class RegularImplicitSchurFactor: public GaussianFactor { class RegularImplicitSchurFactor: public GaussianFactor {
@ -38,9 +52,10 @@ protected:
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension 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, 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 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 Matrix E_; ///< The 2m*3 E Jacobian with respect to the point
const Vector b_; ///< 2m-dimensional RHS vector 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 /// 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) : * @brief Construct a new RegularImplicitSchurFactor object.
GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) { *
} * @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 /// Destructor
~RegularImplicitSchurFactor() override { ~RegularImplicitSchurFactor() override {
} }
std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks() const { const FBlocks& Fs() const {
return FBlocks_; return FBlocks_;
} }

155
gtsam/slam/SmartFactorBase.h
View File

@ -37,12 +37,14 @@
namespace gtsam { 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 * This base class has no internal point, but it has a measurement, noise model
* and an optional sensor pose. * and an optional sensor pose.
* This class mainly computes the derivatives and returns them as a variety of factors. * This class mainly computes the derivatives and returns them as a variety of
* The methods take a Cameras argument, which should behave like PinholeCamera, and * factors. The methods take a CameraSet<CAMERA> argument and the value of a
* the value of a point, which is kept in the base class. * point, which is kept in the derived class.
*
* @tparam CAMERA should behave like a PinholeCamera.
*/ */
template<class CAMERA> template<class CAMERA>
class SmartFactorBase: public NonlinearFactor { class SmartFactorBase: public NonlinearFactor {
@ -64,19 +66,20 @@ protected:
/** /**
* As of Feb 22, 2015, the noise model is the same for all measurements and * 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 * 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. * with what is normally done.
*/ */
SharedIsotropic noiseModel_; SharedIsotropic noiseModel_;
/** /**
* 2D measurement and noise model for each of the m views * Measurements for each of the m views.
* We keep a copy of measurements for I/O and computing the error. * 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. * The order is kept the same as the keys that we use to create the factor.
*/ */
ZVector measured_; 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 // Cache for Fblocks, to avoid a malloc ever time we re-linearize
mutable FBlocks Fs; mutable FBlocks Fs;
@ -84,16 +87,16 @@ protected:
public: public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW 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; 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; typedef CameraSet<CAMERA> Cameras;
/// Default Constructor, for serialization /// Default Constructor, for serialization.
SmartFactorBase() {} SmartFactorBase() {}
/// Constructor /// Construct with given noise model and optional arguments.
SmartFactorBase(const SharedNoiseModel& sharedNoiseModel, SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none, boost::optional<Pose3> body_P_sensor = boost::none,
size_t expectedNumberCameras = 10) size_t expectedNumberCameras = 10)
@ -111,12 +114,12 @@ protected:
noiseModel_ = sharedIsotropic; noiseModel_ = sharedIsotropic;
} }
/// Virtual destructor, subclasses from NonlinearFactor /// Virtual destructor, subclasses from NonlinearFactor.
~SmartFactorBase() override { ~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 measured is the 2m dimensional projection of a single landmark
* @param key is the index corresponding to the camera observing the landmark * @param key is the index corresponding to the camera observing the landmark
*/ */
@ -129,9 +132,7 @@ protected:
this->keys_.push_back(key); 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) { void add(const ZVector& measurements, const KeyVector& cameraKeys) {
assert(measurements.size() == cameraKeys.size()); assert(measurements.size() == cameraKeys.size());
for (size_t i = 0; i < measurements.size(); i++) { 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). * Add an entire SfM_track (collection of cameras observing a single point).
* The noise is assumed to be the same for all measurements * The noise is assumed to be the same for all measurements.
*/ */
template<class SFM_TRACK> template<class SFM_TRACK>
void add(const SFM_TRACK& trackToAdd) { void add(const SFM_TRACK& trackToAdd) {
@ -151,17 +152,13 @@ protected:
} }
} }
/// get the dimension (number of rows!) of the factor /// Return the dimension (number of rows!) of the factor.
size_t dim() const override { size_t dim() const override { return ZDim * this->measured_.size(); }
return ZDim * this->measured_.size();
}
/** return the measurements */ /// Return the 2D measurements (ZDim, in general).
const ZVector& measured() const { const ZVector& measured() const { return measured_; }
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 { virtual Cameras cameras(const Values& values) const {
Cameras cameras; Cameras cameras;
for(const Key& k: this->keys_) for(const Key& k: this->keys_)
@ -188,25 +185,30 @@ protected:
/// equals /// equals
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
const This *e = dynamic_cast<const This*>(&p); if (const This* e = dynamic_cast<const This*>(&p)) {
// Check that all measurements are the same.
bool areMeasurementsEqual = true; for (size_t i = 0; i < measured_.size(); i++) {
for (size_t i = 0; i < measured_.size(); i++) { if (!traits<Z>::Equals(this->measured_.at(i), e->measured_.at(i), tol))
if (traits<Z>::Equals(this->measured_.at(i), e->measured_.at(i), tol) == false) return false;
areMeasurementsEqual = false; }
break; // 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 /// 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> template <class POINT>
Vector unwhitenedError( Vector unwhitenedError(
const Cameras& cameras, const POINT& point, const Cameras& cameras, const POINT& point,
boost::optional<typename Cameras::FBlocks&> Fs = boost::none, // boost::optional<typename Cameras::FBlocks&> Fs = boost::none, //
boost::optional<Matrix&> E = boost::none) const { 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) { if (body_P_sensor_ && Fs) {
const Pose3 sensor_P_body = body_P_sensor_->inverse(); const Pose3 sensor_P_body = body_P_sensor_->inverse();
constexpr int camera_dim = traits<CAMERA>::dimension; constexpr int camera_dim = traits<CAMERA>::dimension;
@ -224,52 +226,60 @@ protected:
Fs->at(i) = Fs->at(i) * J; 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) * This corrects the Jacobians for the case in which some 2D measurement is
* In practice, this does not do anything in the monocular case, but it is implemented in the stereo version * 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, virtual void correctForMissingMeasurements(
boost::optional<Matrix&> E = boost::none) const {} 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] * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) -
* Noise model applied * z], with the noise model applied.
*/ */
template<class POINT> template<class POINT>
Vector whitenedError(const Cameras& cameras, const POINT& point) const { Vector whitenedError(const Cameras& cameras, const POINT& point) const {
Vector e = cameras.reprojectionError(point, measured_); Vector error = cameras.reprojectionError(point, measured_);
if (noiseModel_) if (noiseModel_)
noiseModel_->whitenInPlace(e); noiseModel_->whitenInPlace(error);
return e; 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. * Calculate the error of the factor.
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. * Gaussian. In this class, we take the raw prediction error \f$ h(x)-z \f$,
* Will be used in "error(Values)" function required by NonlinearFactor base class * 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> template<class POINT>
double totalReprojectionError(const Cameras& cameras, double totalReprojectionError(const Cameras& cameras,
const POINT& point) const { const POINT& point) const {
Vector e = whitenedError(cameras, point); Vector error = whitenedError(cameras, point);
return 0.5 * e.dot(e); return 0.5 * error.dot(error);
} }
/// Computes Point Covariance P from E /// Computes Point Covariance P from the "point Jacobian" E.
static Matrix PointCov(Matrix& E) { static Matrix PointCov(const Matrix& E) {
return (E.transpose() * E).inverse(); return (E.transpose() * E).inverse();
} }
/** /**
* Compute F, E, and b (called below in both vanilla and SVD versions), where * 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 * F is a vector of derivatives wrpt the cameras, and E the stacked
* with respect to the point. The value of cameras/point are passed as parameters. * derivatives with respect to the point. The value of cameras/point are
* TODO: Kill this obsolete method * passed as parameters.
*/ */
template<class POINT> template<class POINT>
void computeJacobians(FBlocks& Fs, Matrix& E, Vector& b, void computeJacobians(FBlocks& Fs, Matrix& E, Vector& b,
@ -281,7 +291,11 @@ protected:
b = -unwhitenedError(cameras, point, Fs, E); 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> template<class POINT>
void computeJacobiansSVD(FBlocks& Fs, Matrix& Enull, void computeJacobiansSVD(FBlocks& Fs, Matrix& Enull,
Vector& b, const Cameras& cameras, const POINT& point) const { 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) 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); Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
Vector s = svd.singularValues();
size_t m = this->keys_.size(); size_t m = this->keys_.size();
Enull = svd.matrixU().block(0, N, ZDim * m, ZDim * m - N); // last ZDim*m-N columns 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( boost::shared_ptr<RegularHessianFactor<Dim> > createHessianFactor(
const Cameras& cameras, const Point3& point, const double lambda = 0.0, const Cameras& cameras, const Point3& point, const double lambda = 0.0,
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
@ -351,9 +365,7 @@ protected:
P, b); P, b);
} }
/** /// Return Jacobians as JacobianFactorQ.
* Return Jacobians as JacobianFactorQ
*/
boost::shared_ptr<JacobianFactorQ<Dim, ZDim> > createJacobianQFactor( boost::shared_ptr<JacobianFactorQ<Dim, ZDim> > createJacobianQFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0, const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
@ -368,8 +380,8 @@ protected:
} }
/** /**
* Return Jacobians as JacobianFactorSVD * Return Jacobians as JacobianFactorSVD.
* TODO lambda is currently ignored * TODO(dellaert): lambda is currently ignored
*/ */
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor( boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0) const { 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); F.block<ZDim, Dim>(ZDim * i, Dim * i) = Fs.at(i);
} }
// Return sensor pose.
Pose3 body_P_sensor() const{ Pose3 body_P_sensor() const{
if(body_P_sensor_) if(body_P_sensor_)
return *body_P_sensor_; return *body_P_sensor_;

106
gtsam/slam/SmartProjectionFactor.h
View File

@ -61,10 +61,11 @@ protected:
/// @name Caching triangulation /// @name Caching triangulation
/// @{ /// @{
mutable TriangulationResult result_; ///< result from triangulateSafe 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 /// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
@ -117,21 +118,31 @@ public:
&& Base::equals(p, tol); && 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 { 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 // Several calls to linearize will be done from the same linearization
// Note that this is not yet "selecting linearization", that will come later, and we only check if the // point, hence it is not needed to re-triangulate. Note that this is not
// current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point // 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(); size_t m = cameras.size();
bool retriangulate = false; 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() if (cameraPosesTriangulation_.empty()
|| cameras.size() != cameraPosesTriangulation_.size()) || cameras.size() != cameraPosesTriangulation_.size())
retriangulate = true; retriangulate = true;
// Otherwise, check poses against cache.
if (!retriangulate) { if (!retriangulate) {
for (size_t i = 0; i < cameras.size(); i++) { for (size_t i = 0; i < cameras.size(); i++) {
if (!cameras[i].pose().equals(cameraPosesTriangulation_[i], if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
@ -142,7 +153,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_.clear();
cameraPosesTriangulation_.reserve(m); cameraPosesTriangulation_.reserve(m);
for (size_t i = 0; i < m; i++) for (size_t i = 0; i < m; i++)
@ -150,10 +162,15 @@ public:
cameraPosesTriangulation_.push_back(cameras[i].pose()); 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 { TriangulationResult triangulateSafe(const Cameras& cameras) const {
size_t m = cameras.size(); size_t m = cameras.size();
@ -167,17 +184,21 @@ public:
return result_; 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 { bool triangulateForLinearize(const Cameras& cameras) const {
triangulateSafe(cameras); // imperative, might reset result_ triangulateSafe(cameras); // imperative, might reset result_
return bool(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( boost::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor(
const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping = const Cameras& cameras, const double lambda = 0.0,
false) const { bool diagonalDamping = false) const {
size_t numKeys = this->keys_.size(); size_t numKeys = this->keys_.size();
// Create structures for Hessian Factors // Create structures for Hessian Factors
KeyVector js; KeyVector js;
@ -185,39 +206,38 @@ public:
std::vector<Vector> gs(numKeys); std::vector<Vector> gs(numKeys);
if (this->measured_.size() != cameras.size()) if (this->measured_.size() != cameras.size())
throw std::runtime_error("SmartProjectionHessianFactor: this->measured_" throw std::runtime_error(
".size() inconsistent with input"); "SmartProjectionHessianFactor: this->measured_"
".size() inconsistent with input");
triangulateSafe(cameras); triangulateSafe(cameras);
if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
// failed: return"empty" Hessian // failed: return"empty" Hessian
for(Matrix& m: Gs) for (Matrix& m : Gs) m = Matrix::Zero(Base::Dim, Base::Dim);
m = Matrix::Zero(Base::Dim, Base::Dim); for (Vector& v : gs) v = Vector::Zero(Base::Dim);
for(Vector& v: gs)
v = Vector::Zero(Base::Dim);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, 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(). // 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; Matrix E;
Vector b; Vector b;
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
// Whiten using noise model // Whiten using noise model
Base::whitenJacobians(Fblocks, E, b); Base::whitenJacobians(Fs, E, b);
// build augmented hessian // build augmented hessian
SymmetricBlockMatrix augmentedHessian = // SymmetricBlockMatrix augmentedHessian = //
Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping); Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, return boost::make_shared<RegularHessianFactor<Base::Dim> >(
augmentedHessian); this->keys_, augmentedHessian);
} }
// create factor // Create RegularImplicitSchurFactor factor.
boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > createRegularImplicitSchurFactor( boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > createRegularImplicitSchurFactor(
const Cameras& cameras, double lambda) const { const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras)) if (triangulateForLinearize(cameras))
@ -227,7 +247,7 @@ public:
return boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> >(); return boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> >();
} }
/// create factor /// Create JacobianFactorQ factor.
boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor( boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
const Cameras& cameras, double lambda) const { const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras)) if (triangulateForLinearize(cameras))
@ -237,13 +257,13 @@ public:
return boost::make_shared<JacobianFactorQ<Base::Dim, 2> >(this->keys_); 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( boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
const Values& values, double lambda) const { const Values& values, double lambda) const {
return createJacobianQFactor(this->cameras(values), lambda); 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( boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, double lambda) const { const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras)) if (triangulateForLinearize(cameras))
@ -253,19 +273,19 @@ public:
return boost::make_shared<JacobianFactorSVD<Base::Dim, 2> >(this->keys_); 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( virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
const Values& values, double lambda = 0.0) const { const Values& values, double lambda = 0.0) const {
return createHessianFactor(this->cameras(values), lambda); 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( virtual boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > linearizeToImplicit(
const Values& values, double lambda = 0.0) const { const Values& values, double lambda = 0.0) const {
return createRegularImplicitSchurFactor(this->cameras(values), lambda); return createRegularImplicitSchurFactor(this->cameras(values), lambda);
} }
/// linearize to a JacobianfactorQ /// Linearize to a JacobianfactorQ.
virtual boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > linearizeToJacobian( virtual boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > linearizeToJacobian(
const Values& values, double lambda = 0.0) const { const Values& values, double lambda = 0.0) const {
return createJacobianQFactor(this->cameras(values), lambda); return createJacobianQFactor(this->cameras(values), lambda);
@ -335,7 +355,7 @@ public:
/// Assumes the point has been computed /// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate /// Note E can be 2m*3 or 2m*2, in case point is degenerate
void computeJacobiansWithTriangulatedPoint( 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 { const Cameras& cameras) const {
if (!result_) { if (!result_) {
@ -343,32 +363,32 @@ public:
// TODO check flag whether we should do this // TODO check flag whether we should do this
Unit3 backProjected = cameras[0].backprojectPointAtInfinity( Unit3 backProjected = cameras[0].backprojectPointAtInfinity(
this->measured_.at(0)); this->measured_.at(0));
Base::computeJacobians(Fblocks, E, b, cameras, backProjected); Base::computeJacobians(Fs, E, b, cameras, backProjected);
} else { } else {
// valid result: just return Base version // 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 /// Version that takes values, and creates the point
bool triangulateAndComputeJacobians( 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 { const Values& values) const {
Cameras cameras = this->cameras(values); Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras); bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate) if (nonDegenerate)
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
return nonDegenerate; return nonDegenerate;
} }
/// takes values /// takes values
bool triangulateAndComputeJacobiansSVD( 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 { const Values& values) const {
Cameras cameras = this->cameras(values); Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras); bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate) if (nonDegenerate)
Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_); Base::computeJacobiansSVD(Fs, Enull, b, cameras, *result_);
return nonDegenerate; return nonDegenerate;
} }

360
gtsam/slam/SmartProjectionFactorP.h
View File

@ -1,360 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SmartProjectionFactorP.h
* @brief Smart factor on poses, assuming camera calibration is fixed.
* 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)
* @author Luca Carlone
* @author Chris Beall
* @author Zsolt Kira
*/
#pragma once
#include <gtsam/slam/SmartProjectionFactor.h>
namespace gtsam {
/**
*
* @addtogroup SLAM
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
* independent sets in factor graphs: a unifying perspective based on smart factors,
* Int. Conf. on Robotics and Automation (ICRA), 2014.
*
*/
/**
* This factor assumes that camera calibration is fixed (but each camera
* measurement can have a different extrinsic and intrinsic calibration).
* The factor only constrains poses (variable dimension is 6).
* This factor requires that values contains the involved poses (Pose3).
* If all measurements share the same calibration (i.e., are from the same camera), use SmartProjectionPoseFactor instead!
* If the calibration should be optimized, as well, use SmartProjectionFactor instead!
* @addtogroup SLAM
*/
template<class CAMERA>
class SmartProjectionFactorP : public SmartProjectionFactor<CAMERA> {
private:
typedef SmartProjectionFactor<CAMERA> Base;
typedef SmartProjectionFactorP<CAMERA> This;
typedef typename CAMERA::CalibrationType CALIBRATION;
typedef typename CAMERA::Measurement MEASUREMENT;
typedef typename CAMERA::MeasurementVector MEASUREMENTS;
static const int DimPose = 6; ///< Pose3 dimension
static const int ZDim = 2; ///< Measurement dimension (Point2)
protected:
/// vector of keys (one for each observation) with potentially repeated keys
KeyVector nonUniqueKeys_;
/// shared pointer to calibration object (one for each observation)
std::vector<boost::shared_ptr<CALIBRATION> > K_all_;
/// Pose of the camera in the body frame (one for each observation)
std::vector<Pose3> body_P_sensors_;
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;
/// Default constructor, only for serialization
SmartProjectionFactorP() {
}
/**
* Constructor
* @param sharedNoiseModel isotropic noise model for the 2D feature measurements
* @param params parameters for the smart projection factors
*/
SmartProjectionFactorP(const SharedNoiseModel& sharedNoiseModel,
const SmartProjectionParams& params =
SmartProjectionParams())
: Base(sharedNoiseModel, params) {
// use only configuration that works with this factor
Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY;
Base::params_.linearizationMode = gtsam::HESSIAN;
}
/** Virtual destructor */
~SmartProjectionFactorP() override {
}
/**
* add a new measurement, corresponding to an observation from pose "poseKey" whose camera
* has intrinsic calibration K and extrinsic calibration body_P_sensor.
* @param measured 2-dimensional location of the projection of a
* single landmark in a single view (the measurement)
* @param poseKey key corresponding to the body pose of the camera taking the measurement
* @param K (fixed) camera intrinsic calibration
* @param body_P_sensor (fixed) camera extrinsic calibration
*/
void add(const MEASUREMENT& measured, const Key& poseKey,
const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor =
Pose3::identity()) {
// store measurement and key
this->measured_.push_back(measured);
this->nonUniqueKeys_.push_back(poseKey);
// also store keys in the keys_ vector: these keys are assumed to be unique, so we avoid duplicates here
if (std::find(this->keys_.begin(), this->keys_.end(), poseKey) == this->keys_.end())
this->keys_.push_back(poseKey); // add only unique keys
// store fixed intrinsic calibration
K_all_.push_back(K);
// store fixed extrinsics of the camera
body_P_sensors_.push_back(body_P_sensor);
}
/**
* Variant of the previous "add" function in which we include multiple measurements
* @param measurements vector of the 2m dimensional location of the projection
* of a single landmark in the m views (the measurements)
* @param poseKeys keys corresponding to the body poses of the cameras taking the measurements
* @param Ks vector of (fixed) intrinsic calibration objects
* @param body_P_sensors vector of (fixed) extrinsic calibration objects
*/
void add(const MEASUREMENTS& measurements, const KeyVector& poseKeys,
const std::vector<boost::shared_ptr<CALIBRATION>>& Ks,
const std::vector<Pose3> body_P_sensors = std::vector<Pose3>()) {
assert(poseKeys.size() == measurements.size());
assert(poseKeys.size() == Ks.size());
for (size_t i = 0; i < measurements.size(); i++) {
if (poseKeys.size() == body_P_sensors.size()) {
add(measurements[i], poseKeys[i], Ks[i], body_P_sensors[i]);
} else {
add(measurements[i], poseKeys[i], Ks[i]); // use default extrinsics
}
}
}
/// return the calibration object
inline std::vector<boost::shared_ptr<CALIBRATION>> calibration() const {
return K_all_;
}
/// return the extrinsic camera calibration body_P_sensors
const std::vector<Pose3> body_P_sensors() const {
return body_P_sensors_;
}
/// return (for each observation) the (possibly non unique) keys involved in the measurements
const KeyVector nonUniqueKeys() const {
return nonUniqueKeys_;
}
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override {
std::cout << s << "SmartProjectionFactorP: \n ";
for (size_t i = 0; i < K_all_.size(); i++) {
std::cout << "-- Measurement nr " << i << std::endl;
std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl;
body_P_sensors_[i].print("extrinsic calibration:\n");
K_all_[i]->print("intrinsic calibration = ");
}
Base::print("", keyFormatter);
}
/// equals
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
const This *e = dynamic_cast<const This*>(&p);
double extrinsicCalibrationEqual = true;
if (this->body_P_sensors_.size() == e->body_P_sensors().size()) {
for (size_t i = 0; i < this->body_P_sensors_.size(); i++) {
if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])) {
extrinsicCalibrationEqual = false;
break;
}
}
} else {
extrinsicCalibrationEqual = false;
}
return e && Base::equals(p, tol) && K_all_ == e->calibration()
&& nonUniqueKeys_ == e->nonUniqueKeys() && extrinsicCalibrationEqual;
}
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses corresponding
* to keys involved in this factor
* @return vector of cameras
*/
typename Base::Cameras cameras(const Values& values) const override {
typename Base::Cameras cameras;
for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
const Pose3& body_P_cam_i = body_P_sensors_[i];
const Pose3 world_P_sensor_i = values.at<Pose3>(nonUniqueKeys_[i])
* body_P_cam_i;
cameras.emplace_back(world_P_sensor_i, K_all_[i]);
}
return cameras;
}
/**
* error calculates the error of the factor.
*/
double error(const Values& values) const override {
if (this->active(values)) {
return this->totalReprojectionError(this->cameras(values));
} else { // else of active flag
return 0.0;
}
}
/**
* Compute jacobian F, E and error vector at a given linearization point
* @param values Values structure which must contain camera poses
* corresponding to keys involved in this factor
* @return Return arguments are the camera jacobians Fs (including the jacobian with
* 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,
const Cameras& cameras) const {
if (!this->result_) {
throw("computeJacobiansWithTriangulatedPoint");
} else { // valid result: compute jacobians
b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E);
for (size_t i = 0; i < Fs.size(); i++) {
const Pose3 sensor_P_body = body_P_sensors_[i].inverse();
const Pose3 world_P_body = cameras[i].pose() * sensor_P_body;
Eigen::Matrix<double, DimPose, DimPose> H;
world_P_body.compose(body_P_sensors_[i], H);
Fs.at(i) = Fs.at(i) * H;
}
}
}
/// linearize and return a Hessianfactor that is an approximation of error(p)
boost::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor(
const Values& values, const double lambda = 0.0, bool diagonalDamping =
false) const {
// we may have multiple observation sharing the same keys (e.g., 2 cameras measuring from the same body pose),
// hence the number of unique keys may be smaller than nrMeasurements
size_t nrUniqueKeys = this->keys_.size(); // note: by construction, keys_ only contains unique keys
Cameras cameras = this->cameras(values);
// Create structures for Hessian Factors
KeyVector js;
std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
std::vector < Vector > gs(nrUniqueKeys);
if (this->measured_.size() != this->cameras(values).size()) // 1 observation per camera
throw std::runtime_error("SmartProjectionFactorP: "
"measured_.size() inconsistent with input");
// triangulate 3D point at given linearization point
this->triangulateSafe(cameras);
if (!this->result_) { // failed: return "empty/zero" Hessian
if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) {
for (Matrix& m : Gs)
m = Matrix::Zero(DimPose, DimPose);
for (Vector& v : gs)
v = Vector::Zero(DimPose);
return boost::make_shared < RegularHessianFactor<DimPose>
> (this->keys_, Gs, gs, 0.0);
} else {
throw std::runtime_error(
"SmartProjectionFactorP: "
"only supported degeneracy mode is ZERO_ON_DEGENERACY");
}
}
// compute Jacobian given triangulated 3D Point
FBlocks Fs;
Matrix E;
Vector b;
this->computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
// Whiten using noise model
this->noiseModel_->WhitenSystem(E, b);
for (size_t i = 0; i < Fs.size(); i++){
Fs[i] = this->noiseModel_->Whiten(Fs[i]);
}
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_
SymmetricBlockMatrix augmentedHessianUniqueKeys =
Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>(
Fs, E, P, b, nonUniqueKeys_, this->keys_);
return boost::make_shared < RegularHessianFactor<DimPose>
> (this->keys_, augmentedHessianUniqueKeys);
}
/**
* Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM)
* @param values Values structure which must contain camera poses and extrinsic pose for this factor
* @return a Gaussian factor
*/
boost::shared_ptr<GaussianFactor> linearizeDamped(
const Values& values, const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different linear factors
switch (this->params_.linearizationMode) {
case HESSIAN:
return this->createHessianFactor(values, lambda);
default:
throw std::runtime_error(
"SmartProjectioFactorP: unknown linearization mode");
}
}
/// linearize
boost::shared_ptr<GaussianFactor> linearize(const Values& values) const
override {
return this->linearizeDamped(values);
}
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(K_all_);
ar & BOOST_SERIALIZATION_NVP(nonUniqueKeys_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensors_);
}
};
// end of class declaration
/// traits
template<class CAMERA>
struct traits<SmartProjectionFactorP<CAMERA> > : public Testable<
SmartProjectionFactorP<CAMERA> > {
};
} // \ namespace gtsam

393
gtsam/slam/SmartProjectionRigFactor.h Normal file
View File

@ -0,0 +1,393 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SmartProjectionRigFactor.h
* @brief Smart factor on poses, assuming camera calibration is fixed.
* 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 rig system)
* - it allows multiple observations from the same pose/key (again, to
* model a multi-camera system)
* @author Luca Carlone
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/slam/SmartProjectionFactor.h>
namespace gtsam {
/**
*
* @addtogroup SLAM
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
* conditionally independent sets in factor graphs: a unifying perspective based
* on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014.
*/
/**
* This factor assumes that camera calibration is fixed (but each measurement
* can be taken by a different camera in the rig, hence can have a different
* extrinsic and intrinsic calibration). The factor only constrains poses
* (variable dimension is 6 for each pose). This factor requires that values
* contains the involved poses (Pose3). If all measurements share the same
* calibration (i.e., are from the same camera), use SmartProjectionPoseFactor
* instead! If the calibration should be optimized, as well, use
* SmartProjectionFactor instead!
* @addtogroup SLAM
*/
template <class CAMERA>
class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
private:
typedef SmartProjectionFactor<CAMERA> Base;
typedef SmartProjectionRigFactor<CAMERA> This;
typedef typename CAMERA::CalibrationType CALIBRATION;
typedef typename CAMERA::Measurement MEASUREMENT;
typedef typename CAMERA::MeasurementVector MEASUREMENTS;
static const int DimPose = 6; ///< Pose3 dimension
static const int ZDim = 2; ///< Measurement dimension
protected:
/// vector of keys (one for each observation) with potentially repeated keys
KeyVector nonUniqueKeys_;
/// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each
/// camera)
typename Base::Cameras cameraRig_;
/// vector of camera Ids (one for each observation, in the same order),
/// identifying which camera took the measurement
FastVector<size_t> cameraIds_;
public:
typedef CAMERA Camera;
typedef CameraSet<CAMERA> Cameras;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor, only for serialization
SmartProjectionRigFactor() {}
/**
* Constructor
* @param sharedNoiseModel isotropic noise model for the 2D feature
* measurements
* @param cameraRig set of cameras (fixed poses wrt body and intrinsics) in
* the camera rig
* @param params parameters for the smart projection factors
*/
SmartProjectionRigFactor(
const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig,
const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
// throw exception if configuration is not supported by this factor
if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
throw std::runtime_error(
"SmartProjectionRigFactor: "
"degeneracyMode must be set to ZERO_ON_DEGENERACY");
if (Base::params_.linearizationMode != gtsam::HESSIAN)
throw std::runtime_error(
"SmartProjectionRigFactor: "
"linearizationMode must be set to HESSIAN");
}
/**
* Constructor
* @param sharedNoiseModel isotropic noise model for the 2D feature
* measurements
* @param camera single camera (fixed poses wrt body and intrinsics)
* @param params parameters for the smart projection factors
*/
SmartProjectionRigFactor(
const SharedNoiseModel& sharedNoiseModel, const Camera& camera,
const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params) {
// throw exception if configuration is not supported by this factor
if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
throw std::runtime_error(
"SmartProjectionRigFactor: "
"degeneracyMode must be set to ZERO_ON_DEGENERACY");
if (Base::params_.linearizationMode != gtsam::HESSIAN)
throw std::runtime_error(
"SmartProjectionRigFactor: "
"linearizationMode must be set to HESSIAN");
cameraRig_.push_back(camera);
}
/** Virtual destructor */
~SmartProjectionRigFactor() override {}
/**
* add a new measurement, corresponding to an observation from pose "poseKey"
* and taken from the camera in the rig identified by "cameraId"
* @param measured 2-dimensional location of the projection of a
* single landmark in a single view (the measurement)
* @param poseKey key corresponding to the body pose of the camera taking the
* measurement
* @param cameraId ID of the camera in the rig taking the measurement (default
* 0)
*/
void add(const MEASUREMENT& measured, const Key& poseKey,
const size_t& cameraId = 0) {
// store measurement and key
this->measured_.push_back(measured);
this->nonUniqueKeys_.push_back(poseKey);
// also store keys in the keys_ vector: these keys are assumed to be
// unique, so we avoid duplicates here
if (std::find(this->keys_.begin(), this->keys_.end(), poseKey) ==
this->keys_.end())
this->keys_.push_back(poseKey); // add only unique keys
// store id of the camera taking the measurement
cameraIds_.push_back(cameraId);
}
/**
* Variant of the previous "add" function in which we include multiple
* measurements
* @param measurements vector of the 2m dimensional location of the projection
* of a single landmark in the m views (the measurements)
* @param poseKeys keys corresponding to the body poses of the cameras taking
* the measurements
* @param cameraIds IDs of the cameras in the rig taking each measurement
* (same order as the measurements)
*/
void add(const MEASUREMENTS& measurements, const KeyVector& poseKeys,
const FastVector<size_t>& cameraIds = FastVector<size_t>()) {
if (poseKeys.size() != measurements.size() ||
(poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) {
throw std::runtime_error(
"SmartProjectionRigFactor: "
"trying to add inconsistent inputs");
}
if (cameraIds.size() == 0 && cameraRig_.size() > 1) {
throw std::runtime_error(
"SmartProjectionRigFactor: "
"camera rig includes multiple camera but add did not input "
"cameraIds");
}
for (size_t i = 0; i < measurements.size(); i++) {
add(measurements[i], poseKeys[i],
cameraIds.size() == 0 ? 0 : cameraIds[i]);
}
}
/// return (for each observation) the (possibly non unique) keys involved in
/// the measurements
const KeyVector& nonUniqueKeys() const { return nonUniqueKeys_; }
/// return the calibration object
const Cameras& cameraRig() const { return cameraRig_; }
/// return the calibration object
const FastVector<size_t>& cameraIds() const { return cameraIds_; }
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "SmartProjectionRigFactor: \n ";
for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
std::cout << "-- Measurement nr " << i << std::endl;
std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl;
std::cout << "cameraId: " << cameraIds_[i] << std::endl;
cameraRig_[cameraIds_[i]].print("camera in rig:\n");
}
Base::print("", keyFormatter);
}
/// equals
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() &&
cameraRig_.equals(e->cameraRig()) &&
std::equal(cameraIds_.begin(), cameraIds_.end(),
e->cameraIds().begin());
}
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain body poses corresponding
* to keys involved in this factor
* @return vector of cameras
*/
typename Base::Cameras cameras(const Values& values) const override {
typename Base::Cameras cameras;
cameras.reserve(nonUniqueKeys_.size()); // preallocate
for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]];
const Pose3 world_P_sensor_i =
values.at<Pose3>(nonUniqueKeys_[i]) // = world_P_body
* camera_i.pose(); // = body_P_cam_i
cameras.emplace_back(world_P_sensor_i,
make_shared<typename CAMERA::CalibrationType>(
camera_i.calibration()));
}
return cameras;
}
/**
* error calculates the error of the factor.
*/
double error(const Values& values) const override {
if (this->active(values)) {
return this->totalReprojectionError(this->cameras(values));
} else { // else of active flag
return 0.0;
}
}
/**
* Compute jacobian F, E and error vector at a given linearization point
* @param values Values structure which must contain camera poses
* corresponding to keys involved in this factor
* @return Return arguments are the camera jacobians Fs (including the
* jacobian with 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(typename Base::FBlocks& Fs,
Matrix& E, Vector& b,
const Cameras& cameras) const {
if (!this->result_) {
throw("computeJacobiansWithTriangulatedPoint");
} else { // valid result: compute jacobians
b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E);
for (size_t i = 0; i < Fs.size(); i++) {
const Pose3& body_P_sensor = cameraRig_[cameraIds_[i]].pose();
const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse();
Eigen::Matrix<double, DimPose, DimPose> H;
world_P_body.compose(body_P_sensor, H);
Fs.at(i) = Fs.at(i) * H;
}
}
}
/// linearize and return a Hessianfactor that is an approximation of error(p)
boost::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor(
const Values& values, const double& lambda = 0.0,
bool diagonalDamping = false) const {
// we may have multiple observation sharing the same keys (e.g., 2 cameras
// measuring from the same body pose), hence the number of unique keys may
// be smaller than nrMeasurements
size_t nrUniqueKeys =
this->keys_
.size(); // note: by construction, keys_ only contains unique keys
Cameras cameras = this->cameras(values);
// Create structures for Hessian Factors
std::vector<size_t> js;
std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
std::vector<Vector> gs(nrUniqueKeys);
if (this->measured_.size() != cameras.size()) // 1 observation per camera
throw std::runtime_error(
"SmartProjectionRigFactor: "
"measured_.size() inconsistent with input");
// triangulate 3D point at given linearization point
this->triangulateSafe(cameras);
if (!this->result_) { // failed: return "empty/zero" Hessian
if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) {
for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
for (Vector& v : gs) v = Vector::Zero(DimPose);
return boost::make_shared<RegularHessianFactor<DimPose> >(this->keys_,
Gs, gs, 0.0);
} else {
throw std::runtime_error(
"SmartProjectionRigFactor: "
"only supported degeneracy mode is ZERO_ON_DEGENERACY");
}
}
// compute Jacobian given triangulated 3D Point
typename Base::FBlocks Fs;
Matrix E;
Vector b;
this->computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
// Whiten using noise model
this->noiseModel_->WhitenSystem(E, b);
for (size_t i = 0; i < Fs.size(); i++) {
Fs[i] = this->noiseModel_->Whiten(Fs[i]);
}
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_
SymmetricBlockMatrix augmentedHessianUniqueKeys =
Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>(
Fs, E, P, b, nonUniqueKeys_, this->keys_);
return boost::make_shared<RegularHessianFactor<DimPose> >(
this->keys_, augmentedHessianUniqueKeys);
}
/**
* Linearize to Gaussian Factor (possibly adding a damping factor Lambda for
* LM)
* @param values Values structure which must contain camera poses and
* extrinsic pose for this factor
* @return a Gaussian factor
*/
boost::shared_ptr<GaussianFactor> linearizeDamped(
const Values& values, const double& lambda = 0.0) const {
// depending on flag set on construction we may linearize to different
// linear factors
switch (this->params_.linearizationMode) {
case HESSIAN:
return this->createHessianFactor(values, lambda);
default:
throw std::runtime_error(
"SmartProjectionRigFactor: unknown linearization mode");
}
}
/// linearize
boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const override {
return this->linearizeDamped(values);
}
private:
/// Serialization function
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_);
ar& BOOST_SERIALIZATION_NVP(cameraRig_);
ar& BOOST_SERIALIZATION_NVP(cameraIds_);
}
};
// end of class declaration
/// traits
template <class CAMERA>
struct traits<SmartProjectionRigFactor<CAMERA> >
: public Testable<SmartProjectionRigFactor<CAMERA> > {};
} // namespace gtsam

15
gtsam/slam/tests/smartFactorScenarios.h
View File

@ -18,12 +18,12 @@
#pragma once #pragma once
#include <gtsam/slam/SmartProjectionPoseFactor.h> #include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <gtsam/slam/SmartProjectionFactorP.h>
#include <gtsam/slam/SmartProjectionFactor.h> #include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h> #include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h> #include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/SphericalCamera.h> #include <gtsam/geometry/SphericalCamera.h>
#include "../SmartProjectionRigFactor.h"
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -70,8 +70,9 @@ SmartProjectionParams params;
// default Cal3_S2 poses // default Cal3_S2 poses
namespace vanillaPose { namespace vanillaPose {
typedef PinholePose<Cal3_S2> Camera; typedef PinholePose<Cal3_S2> Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor; typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
typedef SmartProjectionFactorP<Camera> SmartFactorP; typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
Camera level_camera(level_pose, sharedK); Camera level_camera(level_pose, sharedK);
Camera level_camera_right(pose_right, sharedK); Camera level_camera_right(pose_right, sharedK);
@ -84,8 +85,9 @@ Camera cam3(pose_above, sharedK);
// default Cal3_S2 poses // default Cal3_S2 poses
namespace vanillaPose2 { namespace vanillaPose2 {
typedef PinholePose<Cal3_S2> Camera; typedef PinholePose<Cal3_S2> Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor; typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
typedef SmartProjectionFactorP<Camera> SmartFactorP; typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480));
Camera level_camera(level_pose, sharedK2); Camera level_camera(level_pose, sharedK2);
Camera level_camera_right(pose_right, sharedK2); Camera level_camera_right(pose_right, sharedK2);
@ -98,6 +100,7 @@ Camera cam3(pose_above, sharedK2);
// Cal3Bundler cameras // Cal3Bundler cameras
namespace bundler { namespace bundler {
typedef PinholeCamera<Cal3Bundler> Camera; typedef PinholeCamera<Cal3Bundler> Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionFactor<Camera> SmartFactor; typedef SmartProjectionFactor<Camera> SmartFactor;
static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0); static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0);
Camera level_camera(level_pose, K); Camera level_camera(level_pose, K);
@ -115,8 +118,9 @@ typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
// Cal3Bundler poses // Cal3Bundler poses
namespace bundlerPose { namespace bundlerPose {
typedef PinholePose<Cal3Bundler> Camera; typedef PinholePose<Cal3Bundler> Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionPoseFactor<Cal3Bundler> SmartFactor; typedef SmartProjectionPoseFactor<Cal3Bundler> SmartFactor;
typedef SmartProjectionFactorP<Camera> SmartFactorP; typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
static boost::shared_ptr<Cal3Bundler> sharedBundlerK( static boost::shared_ptr<Cal3Bundler> sharedBundlerK(
new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
Camera level_camera(level_pose, sharedBundlerK); Camera level_camera(level_pose, sharedBundlerK);
@ -130,7 +134,8 @@ Camera cam3(pose_above, sharedBundlerK);
// sphericalCamera // sphericalCamera
namespace sphericalCamera { namespace sphericalCamera {
typedef SphericalCamera Camera; typedef SphericalCamera Camera;
typedef SmartProjectionFactorP<Camera> SmartFactorP; typedef CameraSet<Camera> Cameras;
typedef SmartProjectionRigFactor<Camera> SmartFactorP;
static EmptyCal::shared_ptr emptyK; static EmptyCal::shared_ptr emptyK;
Camera level_camera(level_pose); Camera level_camera(level_pose);
Camera level_camera_right(pose_right); Camera level_camera_right(pose_right);

1023
gtsam/slam/tests/testSmartProjectionFactorP.cpp → gtsam/slam/tests/testSmartProjectionRigFactor.cpp
View File

File diff suppressed because it is too large Load Diff

40
gtsam_unstable/slam/ReadMe.md Normal file
View File

@ -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.

268
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -40,9 +40,9 @@ namespace gtsam {
* which the pixel observation pose can be interpolated. * which the pixel observation pose can be interpolated.
* @addtogroup SLAM * @addtogroup SLAM
*/ */
template<class CAMERA> template <class CAMERA>
class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAMERA> { class SmartProjectionPoseFactorRollingShutter
: public SmartProjectionFactor<CAMERA> {
private: private:
typedef SmartProjectionFactor<CAMERA> Base; typedef SmartProjectionFactor<CAMERA> Base;
typedef SmartProjectionPoseFactorRollingShutter<CAMERA> This; typedef SmartProjectionPoseFactorRollingShutter<CAMERA> This;
@ -55,9 +55,6 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
static const int ZDim = 2; ///< Measurement dimension (Point2) static const int ZDim = 2; ///< Measurement dimension (Point2)
protected: protected:
/// shared pointer to calibration object (one for each observation)
std::vector<boost::shared_ptr<CALIBRATION>> K_all_;
/// The keys of the pose of the body (with respect to an external world /// The keys of the pose of the body (with respect to an external world
/// frame): two consecutive poses for each observation /// frame): two consecutive poses for each observation
std::vector<std::pair<Key, Key>> world_P_body_key_pairs_; std::vector<std::pair<Key, Key>> world_P_body_key_pairs_;
@ -66,10 +63,17 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
/// pair of consecutive poses /// pair of consecutive poses
std::vector<double> alphas_; std::vector<double> alphas_;
/// Pose of the camera in the body frame /// one or more cameras taking observations (fixed poses wrt body + fixed
std::vector<Pose3> body_P_sensors_; /// intrinsics)
typename Base::Cameras cameraRig_;
/// vector of camera Ids (one for each observation, in the same order),
/// identifying which camera took the measurement
FastVector<size_t> cameraIds_;
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef CAMERA Camera; typedef CAMERA Camera;
typedef CameraSet<CAMERA> Cameras; typedef CameraSet<CAMERA> Cameras;
typedef Eigen::Matrix<double, ZDim, DimBlock> MatrixZD; // F blocks (derivatives wrt block of 2 poses) typedef Eigen::Matrix<double, ZDim, DimBlock> MatrixZD; // F blocks (derivatives wrt block of 2 poses)
@ -79,29 +83,57 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor, only for serialization /// Default constructor, only for serialization
SmartProjectionPoseFactorRollingShutter() { SmartProjectionPoseFactorRollingShutter() {}
/**
* Constructor
* @param Isotropic measurement noise
* @param cameraRig set of cameras (fixed poses wrt body and intrinsics)
* taking the measurements
* @param params internal parameters of the smart factors
*/
SmartProjectionPoseFactorRollingShutter(
const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig,
const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
// throw exception if configuration is not supported by this factor
if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
throw std::runtime_error(
"SmartProjectionRigFactor: "
"degeneracyMode must be set to ZERO_ON_DEGENERACY");
if (Base::params_.linearizationMode != gtsam::HESSIAN)
throw std::runtime_error(
"SmartProjectionRigFactor: "
"linearizationMode must be set to HESSIAN");
} }
/** /**
* Constructor * Constructor
* @param Isotropic measurement noise * @param Isotropic measurement noise
* @param camera single camera (fixed poses wrt body and intrinsics)
* @param params internal parameters of the smart factors * @param params internal parameters of the smart factors
*/ */
SmartProjectionPoseFactorRollingShutter( SmartProjectionPoseFactorRollingShutter(
const SharedNoiseModel& sharedNoiseModel, const SharedNoiseModel& sharedNoiseModel, const Camera& camera,
const SmartProjectionParams& params = SmartProjectionParams()) const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params) { : Base(sharedNoiseModel, params) {
// use only configuration that works with this factor // throw exception if configuration is not supported by this factor
Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
Base::params_.linearizationMode = gtsam::HESSIAN; throw std::runtime_error(
"SmartProjectionRigFactor: "
"degeneracyMode must be set to ZERO_ON_DEGENERACY");
if (Base::params_.linearizationMode != gtsam::HESSIAN)
throw std::runtime_error(
"SmartProjectionRigFactor: "
"linearizationMode must be set to HESSIAN");
cameraRig_.push_back(camera);
} }
/** Virtual destructor */ /** Virtual destructor */
~SmartProjectionPoseFactorRollingShutter() override = default; ~SmartProjectionPoseFactorRollingShutter() override = default;
/** /**
* add a new measurement, with 2 pose keys, interpolation factor, camera * add a new measurement, with 2 pose keys, interpolation factor, and cameraId
* (intrinsic and extrinsic) calibration, and observed pixel.
* @param measured 2-dimensional location of the projection of a single * @param measured 2-dimensional location of the projection of a single
* landmark in a single view (the measurement), interpolated from the 2 poses * landmark in a single view (the measurement), interpolated from the 2 poses
* @param world_P_body_key1 key corresponding to the first body poses (time <= * @param world_P_body_key1 key corresponding to the first body poses (time <=
@ -110,13 +142,11 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
* >= time pixel is acquired) * >= time pixel is acquired)
* @param alpha interpolation factor in [0,1], such that if alpha = 0 the * @param alpha interpolation factor in [0,1], such that if alpha = 0 the
* interpolated pose is the same as world_P_body_key1 * interpolated pose is the same as world_P_body_key1
* @param K (fixed) camera intrinsic calibration * @param cameraId ID of the camera taking the measurement (default 0)
* @param body_P_sensor (fixed) camera extrinsic calibration
*/ */
void add(const MEASUREMENT& measured, const Key& world_P_body_key1, void add(const MEASUREMENT& measured, const Key& world_P_body_key1,
const Key& world_P_body_key2, const double& alpha, const Key& world_P_body_key2, const double& alpha,
const boost::shared_ptr<CALIBRATION>& K, const size_t& cameraId = 0) {
const Pose3& body_P_sensor = Pose3::identity()) {
// store measurements in base class // store measurements in base class
this->measured_.push_back(measured); this->measured_.push_back(measured);
@ -136,11 +166,8 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
// store interpolation factor // store interpolation factor
alphas_.push_back(alpha); alphas_.push_back(alpha);
// store fixed intrinsic calibration // store id of the camera taking the measurement
K_all_.push_back(K); cameraIds_.push_back(cameraId);
// store fixed extrinsics of the camera
body_P_sensors_.push_back(body_P_sensor);
} }
/** /**
@ -153,56 +180,36 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
* for the i0-th measurement can be interpolated * for the i0-th measurement can be interpolated
* @param alphas vector of interpolation params (in [0,1]), one for each * @param alphas vector of interpolation params (in [0,1]), one for each
* measurement (in the same order) * measurement (in the same order)
* @param Ks vector of (fixed) intrinsic calibration objects * @param cameraIds IDs of the cameras taking each measurement (same order as
* @param body_P_sensors vector of (fixed) extrinsic calibration objects * the measurements)
*/ */
void add(const MEASUREMENTS& measurements, void add(const MEASUREMENTS& measurements,
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs, const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
const std::vector<double>& alphas, const std::vector<double>& alphas,
const std::vector<boost::shared_ptr<CALIBRATION>>& Ks, const FastVector<size_t>& cameraIds = FastVector<size_t>()) {
const std::vector<Pose3>& body_P_sensors) { if (world_P_body_key_pairs.size() != measurements.size() ||
assert(world_P_body_key_pairs.size() == measurements.size()); world_P_body_key_pairs.size() != alphas.size() ||
assert(world_P_body_key_pairs.size() == alphas.size()); (world_P_body_key_pairs.size() != cameraIds.size() &&
assert(world_P_body_key_pairs.size() == Ks.size()); cameraIds.size() != 0)) { // cameraIds.size()=0 is default
throw std::runtime_error(
"SmartProjectionPoseFactorRollingShutter: "
"trying to add inconsistent inputs");
}
if (cameraIds.size() == 0 && cameraRig_.size() > 1) {
throw std::runtime_error(
"SmartProjectionPoseFactorRollingShutter: "
"camera rig includes multiple camera but add did not input "
"cameraIds");
}
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
add(measurements[i], world_P_body_key_pairs[i].first, add(measurements[i], world_P_body_key_pairs[i].first,
world_P_body_key_pairs[i].second, alphas[i], Ks[i], world_P_body_key_pairs[i].second, alphas[i],
body_P_sensors[i]); cameraIds.size() == 0 ? 0
: cameraIds[i]); // use 0 as default if
// cameraIds was not specified
} }
} }
/**
* Variant of the previous "add" function in which we include multiple
* measurements with the same (intrinsic and extrinsic) calibration
* @param measurements vector of the 2m dimensional location of the projection
* of a single landmark in the m views (the measurements)
* @param world_P_body_key_pairs vector where the i-th element contains a pair
* of keys corresponding to the pair of poses from which the observation pose
* for the i0-th measurement can be interpolated
* @param alphas vector of interpolation params (in [0,1]), one for each
* measurement (in the same order)
* @param K (fixed) camera intrinsic calibration (same for all measurements)
* @param body_P_sensor (fixed) camera extrinsic calibration (same for all
* measurements)
*/
void add(const MEASUREMENTS& measurements,
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
const std::vector<double>& alphas,
const boost::shared_ptr<CALIBRATION>& K,
const Pose3& body_P_sensor = Pose3::identity()) {
assert(world_P_body_key_pairs.size() == measurements.size());
assert(world_P_body_key_pairs.size() == alphas.size());
for (size_t i = 0; i < measurements.size(); i++) {
add(measurements[i], world_P_body_key_pairs[i].first,
world_P_body_key_pairs[i].second, alphas[i], K, body_P_sensor);
}
}
/// return the calibration object
const std::vector<boost::shared_ptr<CALIBRATION>>& calibration() const {
return K_all_;
}
/// return (for each observation) the keys of the pair of poses from which we /// return (for each observation) the keys of the pair of poses from which we
/// interpolate /// interpolate
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs() const { const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs() const {
@ -212,8 +219,11 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
/// return the interpolation factors alphas /// return the interpolation factors alphas
const std::vector<double>& alphas() const { return alphas_; } const std::vector<double>& alphas() const { return alphas_; }
/// return the extrinsic camera calibration body_P_sensors /// return the calibration object
const std::vector<Pose3>& body_P_sensors() const { return body_P_sensors_; } const Cameras& cameraRig() const { return cameraRig_; }
/// return the calibration object
const FastVector<size_t>& cameraIds() const { return cameraIds_; }
/** /**
* print * print
@ -224,15 +234,15 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
const std::string& s = "", const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n "; std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n ";
for (size_t i = 0; i < K_all_.size(); i++) { for (size_t i = 0; i < cameraIds_.size(); i++) {
std::cout << "-- Measurement nr " << i << std::endl; std::cout << "-- Measurement nr " << i << std::endl;
std::cout << " pose1 key: " std::cout << " pose1 key: "
<< keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl;
std::cout << " pose2 key: " std::cout << " pose2 key: "
<< keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl;
std::cout << " alpha: " << alphas_[i] << std::endl; std::cout << " alpha: " << alphas_[i] << std::endl;
body_P_sensors_[i].print("extrinsic calibration:\n"); std::cout << "cameraId: " << cameraIds_[i] << std::endl;
K_all_[i]->print("intrinsic calibration = "); cameraRig_[cameraIds_[i]].print("camera in rig:\n");
} }
Base::print("", keyFormatter); Base::print("", keyFormatter);
} }
@ -260,57 +270,10 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
keyPairsEqual = false; keyPairsEqual = false;
} }
double extrinsicCalibrationEqual = true; return e && Base::equals(p, tol) && alphas_ == e->alphas() &&
if (this->body_P_sensors_.size() == e->body_P_sensors().size()) { keyPairsEqual && cameraRig_.equals(e->cameraRig()) &&
for (size_t i = 0; i < this->body_P_sensors_.size(); i++) { std::equal(cameraIds_.begin(), cameraIds_.end(),
if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])) { e->cameraIds().begin());
extrinsicCalibrationEqual = false;
break;
}
}
} else {
extrinsicCalibrationEqual = false;
}
return e && Base::equals(p, tol) && K_all_ == e->calibration() &&
alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual;
}
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses
* corresponding to keys involved in this factor
* @return Cameras
*/
typename Base::Cameras cameras(const Values& values) const override {
size_t numViews = this->measured_.size();
assert(numViews == K_all_.size());
assert(numViews == alphas_.size());
assert(numViews == body_P_sensors_.size());
assert(numViews == world_P_body_key_pairs_.size());
typename Base::Cameras cameras;
for (size_t i = 0; i < numViews; i++) { // for each measurement
const Pose3& w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first);
const Pose3& w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second);
double interpolationFactor = alphas_[i];
const Pose3& w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
const Pose3& body_P_cam = body_P_sensors_[i];
const Pose3& w_P_cam = w_P_body.compose(body_P_cam);
cameras.emplace_back(w_P_cam, K_all_[i]);
}
return cameras;
}
/**
* error calculates the error of the factor.
*/
double error(const Values& values) const override {
if (this->active(values)) {
return this->totalReprojectionError(this->cameras(values));
} else { // else of active flag
return 0.0;
}
} }
/** /**
@ -345,9 +308,12 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
auto w_P_body = auto w_P_body =
interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor, interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor,
dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); dInterpPose_dPoseBody1, dInterpPose_dPoseBody2);
auto body_P_cam = body_P_sensors_[i]; const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]];
auto body_P_cam = camera_i.pose();
auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose);
typename Base::Camera camera(w_P_cam, K_all_[i]); typename Base::Camera camera(
w_P_cam, make_shared<typename CAMERA::CalibrationType>(
camera_i.calibration()));
// get jacobians and error vector for current measurement // get jacobians and error vector for current measurement
Point2 reprojectionError_i = camera.reprojectionError( Point2 reprojectionError_i = camera.reprojectionError(
@ -371,7 +337,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
/// linearize and return a Hessianfactor that is an approximation of error(p) /// linearize and return a Hessianfactor that is an approximation of error(p)
boost::shared_ptr<RegularHessianFactor<DimPose>> createHessianFactor( boost::shared_ptr<RegularHessianFactor<DimPose>> createHessianFactor(
const Values& values, const double lambda = 0.0, const Values& values, const double& lambda = 0.0,
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
// we may have multiple observation sharing the same keys (due to the // we may have multiple observation sharing the same keys (due to the
// rolling shutter interpolation), hence the number of unique keys may be // rolling shutter interpolation), hence the number of unique keys may be
@ -380,19 +346,21 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
this->keys_ this->keys_
.size(); // note: by construction, keys_ only contains unique keys .size(); // note: by construction, keys_ only contains unique keys
typename Base::Cameras cameras = this->cameras(values);
// Create structures for Hessian Factors // Create structures for Hessian Factors
KeyVector js; KeyVector js;
std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
std::vector<Vector> gs(nrUniqueKeys); std::vector<Vector> gs(nrUniqueKeys);
if (this->measured_.size() != if (this->measured_.size() !=
this->cameras(values).size()) // 1 observation per interpolated camera cameras.size()) // 1 observation per interpolated camera
throw std::runtime_error( throw std::runtime_error(
"SmartProjectionPoseFactorRollingShutter: " "SmartProjectionPoseFactorRollingShutter: "
"measured_.size() inconsistent with input"); "measured_.size() inconsistent with input");
// triangulate 3D point at given linearization point // triangulate 3D point at given linearization point
this->triangulateSafe(this->cameras(values)); this->triangulateSafe(cameras);
if (!this->result_) { // failed: return "empty/zero" Hessian if (!this->result_) { // failed: return "empty/zero" Hessian
if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) { if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) {
@ -439,12 +407,51 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
} }
/** /**
* Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM) * error calculates the error of the factor.
* @param values Values structure which must contain camera poses and extrinsic pose for this factor */
double error(const Values& values) const override {
if (this->active(values)) {
return this->totalReprojectionError(this->cameras(values));
} else { // else of active flag
return 0.0;
}
}
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses
* corresponding to keys involved in this factor
* @return Cameras
*/
typename Base::Cameras cameras(const Values& values) const override {
typename Base::Cameras cameras;
for (size_t i = 0; i < this->measured_.size();
i++) { // for each measurement
const Pose3& w_P_body1 =
values.at<Pose3>(world_P_body_key_pairs_[i].first);
const Pose3& w_P_body2 =
values.at<Pose3>(world_P_body_key_pairs_[i].second);
double interpolationFactor = alphas_[i];
const Pose3& w_P_body =
interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
const Pose3& body_P_cam = cameraRig_[cameraIds_[i]].pose();
const Pose3& w_P_cam = w_P_body.compose(body_P_cam);
cameras.emplace_back(w_P_cam,
make_shared<typename CAMERA::CalibrationType>(
cameraRig_[cameraIds_[i]].calibration()));
}
return cameras;
}
/**
* Linearize to Gaussian Factor (possibly adding a damping factor Lambda for
* LM)
* @param values Values structure which must contain camera poses and
* extrinsic pose for this factor
* @return a Gaussian factor * @return a Gaussian factor
*/ */
boost::shared_ptr<GaussianFactor> linearizeDamped( boost::shared_ptr<GaussianFactor> linearizeDamped(
const Values& values, const double lambda = 0.0) const { const Values& values, const double& lambda = 0.0) const {
// depending on flag set on construction we may linearize to different // depending on flag set on construction we may linearize to different
// linear factors // linear factors
switch (this->params_.linearizationMode) { switch (this->params_.linearizationMode) {
@ -469,7 +476,6 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
template <class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) { void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar& BOOST_SERIALIZATION_NVP(K_all_);
} }
}; };
// end of class declaration // end of class declaration

22
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -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, void correctForMissingMeasurements(
boost::optional<typename Cameras::FBlocks&> Fs = boost::none, const Cameras& cameras, Vector& ue,
boost::optional<Matrix&> E = boost::none) const override 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: // 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); 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); MatrixZD& Fi = Fs->at(i);
for(size_t ii=0; ii<Dim; ii++) for (size_t ii = 0; ii < Dim; ii++) Fi(1, ii) = 0.0;
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()); E->row(ZDim * i + 1) = Matrix::Zero(1, E->cols());
// set the corresponding entry of vector ue to zero // set the corresponding entry of vector ue to zero

8
gtsam_unstable/slam/SmartStereoProjectionFactorPP.h
View File

@ -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). * This factor optimizes the pose of the body as well as the extrinsic camera
* Each camera may have its own extrinsic calibration or the same calibration can be shared by multiple cameras. * calibration (pose of camera wrt body). Each camera may have its own extrinsic
* This factor requires that values contain the involved poses and extrinsics (both are Pose3 variables). * 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 * @addtogroup SLAM
*/ */
class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {

533
gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
View File

@ -16,18 +16,19 @@
* @date July 2021 * @date July 2021
*/ */
#include "gtsam/slam/tests/smartFactorScenarios.h" #include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h>
#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serializationTestHelpers.h> #include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h>
#include <gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h>
#include <boost/assign/std/map.hpp> #include <boost/assign/std/map.hpp>
#include <iostream> #include <iostream>
#include "gtsam/slam/tests/smartFactorScenarios.h"
#define DISABLE_TIMING #define DISABLE_TIMING
using namespace gtsam; using namespace gtsam;
@ -60,10 +61,13 @@ static double interp_factor1 = 0.3;
static double interp_factor2 = 0.4; static double interp_factor2 = 0.4;
static double interp_factor3 = 0.5; static double interp_factor3 = 0.5;
static size_t cameraId1 = 0;
/* ************************************************************************* */ /* ************************************************************************* */
// default Cal3_S2 poses with rolling shutter effect // default Cal3_S2 poses with rolling shutter effect
namespace vanillaPoseRS { namespace vanillaPoseRS {
typedef PinholePose<Cal3_S2> Camera; typedef PinholePose<Cal3_S2> Camera;
typedef CameraSet<Camera> Cameras;
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h));
Pose3 interp_pose1 = interpolate<Pose3>(level_pose, pose_right, interp_factor1); Pose3 interp_pose1 = interpolate<Pose3>(level_pose, pose_right, interp_factor1);
Pose3 interp_pose2 = interpolate<Pose3>(pose_right, pose_above, interp_factor2); Pose3 interp_pose2 = interpolate<Pose3>(pose_right, pose_above, interp_factor2);
@ -71,6 +75,9 @@ Pose3 interp_pose3 = interpolate<Pose3>(pose_above, level_pose, interp_factor3);
Camera cam1(interp_pose1, sharedK); Camera cam1(interp_pose1, sharedK);
Camera cam2(interp_pose2, sharedK); Camera cam2(interp_pose2, sharedK);
Camera cam3(interp_pose3, sharedK); Camera cam3(interp_pose3, sharedK);
SmartProjectionParams params(
gtsam::HESSIAN,
gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors
} // namespace vanillaPoseRS } // namespace vanillaPoseRS
LevenbergMarquardtParams lmParams; LevenbergMarquardtParams lmParams;
@ -79,26 +86,29 @@ typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); using namespace vanillaPoseRS;
SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
SmartProjectionParams params; using namespace vanillaPoseRS;
params.setRankTolerance(rankTol); params.setRankTolerance(rankTol);
SmartFactorRS factor1(model, params); SmartFactorRS factor1(model, Camera(Pose3::identity(), sharedK), params);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, add) { TEST(SmartProjectionPoseFactorRollingShutter, add) {
using namespace vanillaPose; using namespace vanillaPoseRS;
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr factor1(
factor1->add(measurement1, x1, x2, interp_factor, sharedK, body_P_sensor); new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
factor1->add(measurement1, x1, x2, interp_factor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, Equals) { TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
using namespace vanillaPose; using namespace vanillaPoseRS;
// create fake measurements // create fake measurements
Point2Vector measurements; Point2Vector measurements;
@ -111,68 +121,88 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
key_pairs.push_back(std::make_pair(x2, x3)); key_pairs.push_back(std::make_pair(x2, x3));
key_pairs.push_back(std::make_pair(x3, x4)); key_pairs.push_back(std::make_pair(x3, x4));
std::vector<boost::shared_ptr<Cal3_S2>> intrinsicCalibrations;
intrinsicCalibrations.push_back(sharedK);
intrinsicCalibrations.push_back(sharedK);
intrinsicCalibrations.push_back(sharedK);
std::vector<Pose3> extrinsicCalibrations;
extrinsicCalibrations.push_back(body_P_sensor);
extrinsicCalibrations.push_back(body_P_sensor);
extrinsicCalibrations.push_back(body_P_sensor);
std::vector<double> interp_factors; std::vector<double> interp_factors;
interp_factors.push_back(interp_factor1); interp_factors.push_back(interp_factor1);
interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
FastVector<size_t> cameraIds{0, 0, 0};
Cameras cameraRig;
cameraRig.push_back(Camera(body_P_sensor, sharedK));
// create by adding a batch of measurements with a bunch of calibrations // create by adding a batch of measurements with a bunch of calibrations
SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model)); SmartFactorRS::shared_ptr factor2(
factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, new SmartFactorRS(model, cameraRig, params));
extrinsicCalibrations); factor2->add(measurements, key_pairs, interp_factors, cameraIds);
// create by adding a batch of measurements with a single calibrations // create by adding a batch of measurements with a single calibrations
SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model)); SmartFactorRS::shared_ptr factor3(
factor3->add(measurements, key_pairs, interp_factors, sharedK, body_P_sensor); new SmartFactorRS(model, cameraRig, params));
factor3->add(measurements, key_pairs, interp_factors, cameraIds);
{ // create equal factors and show equal returns true { // create equal factors and show equal returns true
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr factor1(
factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); new SmartFactorRS(model, cameraRig, params));
factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); factor1->add(measurement2, x2, x3, interp_factor2, cameraId1);
factor1->add(measurement3, x3, x4, interp_factor3, cameraId1);
EXPECT(factor1->equals(*factor2));
EXPECT(factor1->equals(*factor3));
}
{ // create equal factors and show equal returns true (use default cameraId)
SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, cameraRig, params));
factor1->add(measurement1, x1, x2, interp_factor1);
factor1->add(measurement2, x2, x3, interp_factor2);
factor1->add(measurement3, x3, x4, interp_factor3);
EXPECT(factor1->equals(*factor2));
EXPECT(factor1->equals(*factor3));
}
{ // create equal factors and show equal returns true (use default cameraId)
SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, cameraRig, params));
factor1->add(measurements, key_pairs, interp_factors);
EXPECT(factor1->equals(*factor2)); EXPECT(factor1->equals(*factor2));
EXPECT(factor1->equals(*factor3)); EXPECT(factor1->equals(*factor3));
} }
{ // create slightly different factors (different keys) and show equal { // create slightly different factors (different keys) and show equal
// returns false // returns false (use default cameraIds)
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr factor1(
factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); new SmartFactorRS(model, cameraRig, params));
factor1->add(measurement2, x2, x2, interp_factor2, sharedK, factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
body_P_sensor); // different! factor1->add(measurement2, x2, x2, interp_factor2,
factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); cameraId1); // different!
factor1->add(measurement3, x3, x4, interp_factor3, cameraId1);
EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor2));
EXPECT(!factor1->equals(*factor3)); EXPECT(!factor1->equals(*factor3));
} }
{ // create slightly different factors (different extrinsics) and show equal { // create slightly different factors (different extrinsics) and show equal
// returns false // returns false
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); Cameras cameraRig2;
factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); cameraRig2.push_back(Camera(body_P_sensor * body_P_sensor, sharedK));
factor1->add(measurement2, x2, x3, interp_factor2, sharedK, SmartFactorRS::shared_ptr factor1(
body_P_sensor * body_P_sensor); // different! new SmartFactorRS(model, cameraRig2, params));
factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
factor1->add(measurement2, x2, x3, interp_factor2,
cameraId1); // different!
factor1->add(measurement3, x3, x4, interp_factor3, cameraId1);
EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor2));
EXPECT(!factor1->equals(*factor3)); EXPECT(!factor1->equals(*factor3));
} }
{ // create slightly different factors (different interp factors) and show { // create slightly different factors (different interp factors) and show
// equal returns false // equal returns false
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr factor1(
factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); new SmartFactorRS(model, cameraRig, params));
factor1->add(measurement2, x2, x3, interp_factor1, sharedK, factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
body_P_sensor); // different! factor1->add(measurement2, x2, x3, interp_factor1,
factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); cameraId1); // different!
factor1->add(measurement3, x3, x4, interp_factor3, cameraId1);
EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor2));
EXPECT(!factor1->equals(*factor3)); EXPECT(!factor1->equals(*factor3));
@ -196,9 +226,9 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) {
Point2 level_uv_right = cam2.project(landmark1); Point2 level_uv_right = cam2.project(landmark1);
Pose3 body_P_sensorId = Pose3::identity(); Pose3 body_P_sensorId = Pose3::identity();
SmartFactorRS factor(model); SmartFactorRS factor(model, Camera(body_P_sensorId, sharedK), params);
factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); factor.add(level_uv, x1, x2, interp_factor1);
factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); factor.add(level_uv_right, x2, x3, interp_factor2);
Values values; // it's a pose factor, hence these are poses Values values; // it's a pose factor, hence these are poses
values.insert(x1, level_pose); values.insert(x1, level_pose);
@ -271,10 +301,9 @@ TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) {
Point2 level_uv_right = cam2.project(landmark1); Point2 level_uv_right = cam2.project(landmark1);
Pose3 body_P_sensorNonId = body_P_sensor; Pose3 body_P_sensorNonId = body_P_sensor;
SmartFactorRS factor(model); SmartFactorRS factor(model, Camera(body_P_sensorNonId, sharedK), params);
factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId); factor.add(level_uv, x1, x2, interp_factor1);
factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, factor.add(level_uv_right, x2, x3, interp_factor2);
body_P_sensorNonId);
Values values; // it's a pose factor, hence these are poses Values values; // it's a pose factor, hence these are poses
values.insert(x1, level_pose); values.insert(x1, level_pose);
@ -363,14 +392,17 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr smartFactor1(
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); SmartFactorRS::shared_ptr smartFactor2(
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); SmartFactorRS::shared_ptr smartFactor3(
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -410,6 +442,170 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6)); EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
} }
/* *************************************************************************/
TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) {
using namespace vanillaPoseRS;
Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
// Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3);
// create inputs
std::vector<std::pair<Key, Key>> key_pairs;
key_pairs.push_back(std::make_pair(x1, x2));
key_pairs.push_back(std::make_pair(x2, x3));
key_pairs.push_back(std::make_pair(x3, x1));
std::vector<double> interp_factors;
interp_factors.push_back(interp_factor1);
interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3);
Cameras cameraRig;
cameraRig.push_back(Camera(body_P_sensor, sharedK));
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, cameraRig, params));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {1, 1, 1});
SmartFactorRS::shared_ptr smartFactor2(
new SmartFactorRS(model, cameraRig, params));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {1, 1, 1});
SmartFactorRS::shared_ptr smartFactor3(
new SmartFactorRS(model, cameraRig, params));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {1, 1, 1});
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.addPrior(x1, level_pose, noisePrior);
graph.addPrior(x2, pose_right, noisePrior);
Values groundTruth;
groundTruth.insert(x1, level_pose);
groundTruth.insert(x2, pose_right);
groundTruth.insert(x3, pose_above); // pose above is the pose of the camera
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
// Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, level_pose);
values.insert(x2, pose_right);
// initialize third pose with some noise, we expect it to move back to
// original pose_above
values.insert(x3, pose_above * noise_pose);
EXPECT( // check that the pose is actually noisy
assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
-0.0313952598, -0.000986635786, 0.0314107591,
-0.999013364, -0.0313952598),
Point3(0.1, -0.1, 1.9)),
values.at<Pose3>(x3)));
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize();
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
}
/* *************************************************************************/
TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) {
using namespace vanillaPoseRS;
Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3;
// create arbitrary body_T_sensor (transforms from sensor to body)
Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-0.03, 0., 0.01), Point3(1, 1, 1));
Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-0.1, 0., 0.05), Point3(0, 0, 1));
Pose3 body_T_sensor3 = Pose3(Rot3::Ypr(-0.3, 0., -0.05), Point3(0, 1, 1));
Camera camera1(interp_pose1 * body_T_sensor1, sharedK);
Camera camera2(interp_pose2 * body_T_sensor2, sharedK);
Camera camera3(interp_pose3 * body_T_sensor3, sharedK);
// Project three landmarks into three cameras
projectToMultipleCameras(camera1, camera2, camera3, landmark1,
measurements_lmk1);
projectToMultipleCameras(camera1, camera2, camera3, landmark2,
measurements_lmk2);
projectToMultipleCameras(camera1, camera2, camera3, landmark3,
measurements_lmk3);
// create inputs
std::vector<std::pair<Key, Key>> key_pairs;
key_pairs.push_back(std::make_pair(x1, x2));
key_pairs.push_back(std::make_pair(x2, x3));
key_pairs.push_back(std::make_pair(x3, x1));
std::vector<double> interp_factors;
interp_factors.push_back(interp_factor1);
interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3);
Cameras cameraRig;
cameraRig.push_back(Camera(body_T_sensor1, sharedK));
cameraRig.push_back(Camera(body_T_sensor2, sharedK));
cameraRig.push_back(Camera(body_T_sensor3, sharedK));
SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, cameraRig, params));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {0, 1, 2});
SmartFactorRS::shared_ptr smartFactor2(
new SmartFactorRS(model, cameraRig, params));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {0, 1, 2});
SmartFactorRS::shared_ptr smartFactor3(
new SmartFactorRS(model, cameraRig, params));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0, 1, 2});
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.addPrior(x1, level_pose, noisePrior);
graph.addPrior(x2, pose_right, noisePrior);
Values groundTruth;
groundTruth.insert(x1, level_pose);
groundTruth.insert(x2, pose_right);
groundTruth.insert(x3, pose_above); // pose above is the pose of the camera
DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10),
// Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
Values values;
values.insert(x1, level_pose);
values.insert(x2, pose_right);
// initialize third pose with some noise, we expect it to move back to
// original pose_above
values.insert(x3, pose_above * noise_pose);
EXPECT( // check that the pose is actually noisy
assert_equal(Pose3(Rot3(0, -0.0314107591, 0.99950656, -0.99950656,
-0.0313952598, -0.000986635786, 0.0314107591,
-0.999013364, -0.0313952598),
Point3(0.1, -0.1, 1.9)),
values.at<Pose3>(x3)));
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize();
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-4));
}
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
// here we replicate a test in SmartProjectionPoseFactor by setting // here we replicate a test in SmartProjectionPoseFactor by setting
@ -417,7 +613,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
// falls back to standard pixel measurements) Note: this is a quite extreme // falls back to standard pixel measurements) Note: this is a quite extreme
// test since in typical camera you would not have more than 1 measurement per // test since in typical camera you would not have more than 1 measurement per
// landmark at each interpolated pose // landmark at each interpolated pose
using namespace vanillaPose; using namespace vanillaPoseRS;
// Default cameras for simple derivatives // Default cameras for simple derivatives
static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
@ -437,15 +633,14 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
measurements_lmk1.push_back(cam1.project(landmark1)); measurements_lmk1.push_back(cam1.project(landmark1));
measurements_lmk1.push_back(cam2.project(landmark1)); measurements_lmk1.push_back(cam2.project(landmark1));
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple), params));
double interp_factor = 0; // equivalent to measurement taken at pose 1 double interp_factor = 0; // equivalent to measurement taken at pose 1
smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor);
body_P_sensorId);
interp_factor = 1; // equivalent to measurement taken at pose 2 interp_factor = 1; // equivalent to measurement taken at pose 2
smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor);
body_P_sensorId);
SmartFactor::Cameras cameras; SmartFactorRS::Cameras cameras;
cameras.push_back(cam1); cameras.push_back(cam1);
cameras.push_back(cam2); cameras.push_back(cam2);
@ -533,14 +728,14 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) {
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
params.setEnableEPI(true); params.setEnableEPI(true);
SmartFactorRS smartFactor1(model, params); SmartFactorRS smartFactor1(model, Camera(Pose3::identity(), sharedK), params);
smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS smartFactor2(model, params); SmartFactorRS smartFactor2(model, Camera(Pose3::identity(), sharedK), params);
smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS smartFactor3(model, params); SmartFactorRS smartFactor3(model, Camera(Pose3::identity(), sharedK), params);
smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -593,18 +788,20 @@ TEST(SmartProjectionPoseFactorRollingShutter,
SmartProjectionParams params; SmartProjectionParams params;
params.setRankTolerance(1.0); params.setRankTolerance(1.0);
params.setLinearizationMode(gtsam::HESSIAN); params.setLinearizationMode(gtsam::HESSIAN);
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); // params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); // this would give an
// exception as expected
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
params.setEnableEPI(false); params.setEnableEPI(false);
SmartFactorRS smartFactor1(model, params); SmartFactorRS smartFactor1(model, Camera(Pose3::identity(), sharedK), params);
smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS smartFactor2(model, params); SmartFactorRS smartFactor2(model, Camera(Pose3::identity(), sharedK), params);
smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS smartFactor3(model, params); SmartFactorRS smartFactor3(model, Camera(Pose3::identity(), sharedK), params);
smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -672,17 +869,21 @@ TEST(SmartProjectionPoseFactorRollingShutter,
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
params.setEnableEPI(false); params.setEnableEPI(false);
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, params)); SmartFactorRS::shared_ptr smartFactor1(
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, params)); SmartFactorRS::shared_ptr smartFactor2(
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, params)); SmartFactorRS::shared_ptr smartFactor3(
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model, params)); SmartFactorRS::shared_ptr smartFactor4(
smartFactor4->add(measurements_lmk4, key_pairs, interp_factors, sharedK); new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor4->add(measurements_lmk4, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -732,8 +933,9 @@ TEST(SmartProjectionPoseFactorRollingShutter,
interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr smartFactor1(
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
@ -869,9 +1071,9 @@ TEST(SmartProjectionPoseFactorRollingShutter,
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
interp_factors.push_back(interp_factor1); interp_factors.push_back(interp_factor1);
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr smartFactor1(
smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
sharedK); smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
@ -1025,15 +1227,18 @@ TEST(SmartProjectionPoseFactorRollingShutter,
interp_factors_redundant.push_back( interp_factors_redundant.push_back(
interp_factors.at(0)); // we readd the first interp factor interp_factors.at(0)); // we readd the first interp factor
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant,
interp_factors_redundant, sharedK); interp_factors_redundant);
SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); SmartFactorRS::shared_ptr smartFactor2(
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); SmartFactorRS::shared_ptr smartFactor3(
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -1075,16 +1280,20 @@ TEST(SmartProjectionPoseFactorRollingShutter,
#ifndef DISABLE_TIMING #ifndef DISABLE_TIMING
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
// -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) //-Total: 0 CPU (0 times, 0 wall, 0.21 children, min: 0 max: 0)
//| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: //| -SF RS LINEARIZE: 0.14 CPU
// 0 max: 0) | -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 //(10000 times, 0.131202 wall, 0.14 children, min: 0 max: 0)
// children, min: 0 max: 0) //| -RS LINEARIZE: 0.06 CPU
//(10000 times, 0.066951 wall, 0.06 children, min: 0 max: 0)
/* *************************************************************************/ /* *************************************************************************/
TEST(SmartProjectionPoseFactorRollingShutter, timing) { TEST(SmartProjectionPoseFactorRollingShutter, timing) {
using namespace vanillaPose; using namespace vanillaPose;
// Default cameras for simple derivatives // Default cameras for simple derivatives
static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0));
SmartProjectionParams params(
gtsam::HESSIAN,
gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors
Rot3 R = Rot3::identity(); Rot3 R = Rot3::identity();
Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); Pose3 pose1 = Pose3(R, Point3(0, 0, 0));
@ -1101,16 +1310,15 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) {
measurements_lmk1.push_back(cam1.project(landmark1)); measurements_lmk1.push_back(cam1.project(landmark1));
measurements_lmk1.push_back(cam2.project(landmark1)); measurements_lmk1.push_back(cam2.project(landmark1));
size_t nrTests = 1000; size_t nrTests = 10000;
for (size_t i = 0; i < nrTests; i++) { for (size_t i = 0; i < nrTests; i++) {
SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model)); SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(
model, Camera(body_P_sensorId, sharedKSimple), params));
double interp_factor = 0; // equivalent to measurement taken at pose 1 double interp_factor = 0; // equivalent to measurement taken at pose 1
smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor, smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor);
sharedKSimple, body_P_sensorId);
interp_factor = 1; // equivalent to measurement taken at pose 2 interp_factor = 1; // equivalent to measurement taken at pose 2
smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor);
sharedKSimple, body_P_sensorId);
Values values; Values values;
values.insert(x1, pose1); values.insert(x1, pose1);
@ -1121,7 +1329,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) {
} }
for (size_t i = 0; i < nrTests; i++) { for (size_t i = 0; i < nrTests; i++) {
SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); SmartFactor::shared_ptr smartFactor(
new SmartFactor(model, sharedKSimple, params));
smartFactor->add(measurements_lmk1[0], x1); smartFactor->add(measurements_lmk1[0], x1);
smartFactor->add(measurements_lmk1[1], x2); smartFactor->add(measurements_lmk1[1], x2);
@ -1141,6 +1350,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) {
// spherical Camera with rolling shutter effect // spherical Camera with rolling shutter effect
namespace sphericalCameraRS { namespace sphericalCameraRS {
typedef SphericalCamera Camera; typedef SphericalCamera Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionPoseFactorRollingShutter<Camera> SmartFactorRS_spherical; typedef SmartProjectionPoseFactorRollingShutter<Camera> SmartFactorRS_spherical;
Pose3 interp_pose1 = interpolate<Pose3>(level_pose,pose_right,interp_factor1); Pose3 interp_pose1 = interpolate<Pose3>(level_pose,pose_right,interp_factor1);
Pose3 interp_pose2 = interpolate<Pose3>(pose_right,pose_above,interp_factor2); Pose3 interp_pose2 = interpolate<Pose3>(pose_right,pose_above,interp_factor2);
@ -1173,55 +1383,60 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_sphericalCame
interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
SmartProjectionParams params; SmartProjectionParams params(
gtsam::HESSIAN,
gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors
params.setRankTolerance(0.1); params.setRankTolerance(0.1);
SmartFactorRS_spherical::shared_ptr smartFactor1( Cameras cameraRig;
new SmartFactorRS_spherical(model,params)); cameraRig.push_back(Camera(Pose3::identity(), emptyK));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, emptyK);
// SmartFactorRS_spherical::shared_ptr smartFactor2( SmartFactorRS_spherical::shared_ptr smartFactor1(
// new SmartFactorRS_spherical(model,params)); new SmartFactorRS_spherical(model,cameraRig,params));
// smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, emptyK); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
//
// SmartFactorRS_spherical::shared_ptr smartFactor3( SmartFactorRS_spherical::shared_ptr smartFactor2(
// new SmartFactorRS_spherical(model,params)); new SmartFactorRS_spherical(model,cameraRig,params));
// smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, emptyK); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
//
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); SmartFactorRS_spherical::shared_ptr smartFactor3(
// new SmartFactorRS_spherical(model,cameraRig,params));
// NonlinearFactorGraph graph; smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
// graph.push_back(smartFactor1);
// graph.push_back(smartFactor2); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
// graph.push_back(smartFactor3);
// graph.addPrior(x1, level_pose, noisePrior); NonlinearFactorGraph graph;
// graph.addPrior(x2, pose_right, noisePrior); graph.push_back(smartFactor1);
// graph.push_back(smartFactor2);
// Values groundTruth; graph.push_back(smartFactor3);
// groundTruth.insert(x1, level_pose); graph.addPrior(x1, level_pose, noisePrior);
// groundTruth.insert(x2, pose_right); graph.addPrior(x2, pose_right, noisePrior);
// groundTruth.insert(x3, pose_above);
// DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); Values groundTruth;
// groundTruth.insert(x1, level_pose);
// // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below groundTruth.insert(x2, pose_right);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), groundTruth.insert(x3, pose_above);
// Point3(0.1, 0.1, 0.1)); // smaller noise DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9);
// Values values;
// values.insert(x1, level_pose); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
// values.insert(x2, pose_right); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
// // initialize third pose with some noise, we expect it to move back to original pose_above Point3(0.1, 0.1, 0.1)); // smaller noise
// values.insert(x3, pose_above * noise_pose); Values values;
// EXPECT( // check that the pose is actually noisy values.insert(x1, level_pose);
// assert_equal( values.insert(x2, pose_right);
// Pose3( // initialize third pose with some noise, we expect it to move back to original pose_above
// Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, values.insert(x3, pose_above * noise_pose);
// -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), EXPECT( // check that the pose is actually noisy
// Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3))); assert_equal(
// Pose3(
// Values result; Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598,
// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
// result = optimizer.optimize(); Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
// EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
result = optimizer.optimize();
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */