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

18
gtsam/slam/ProjectionFactor.h
View File

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

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
* @brief A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor
* @brief A subclass of GaussianFactor specialized to structureless SFM.
* @author Frank Dellaert
* @author Luca Carlone
*/
@ -20,6 +20,20 @@ namespace gtsam {
/**
* RegularImplicitSchurFactor
*
* A specialization of a GaussianFactor to structure-less SFM, which is very
* fast in a conjugate gradient (CG) solver. Specifically, as measured in
* timeSchurFactors.cpp, it stays very fast for an increasing number of cameras.
* The magic is in multiplyHessianAdd, which does the Hessian-vector multiply at
* the core of CG, and implements
* y += F'*alpha*(I - E*P*E')*F*x
* where
* - F is the 2mx6m Jacobian of the m 2D measurements wrpt m 6DOF poses
* - E is the 2mx3 Jacabian of the m 2D measurements wrpt a 3D point
* - P is the covariance on the point
* The equation above implicitly executes the Schur complement by removing the
* information E*P*E' from the Hessian. It is also very fast as we do not use
* the full 6m*6m F matrix, but rather only it's m 6x6 diagonal blocks.
*/
template<class CAMERA>
class RegularImplicitSchurFactor: public GaussianFactor {
@ -38,9 +52,10 @@ protected:
static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
typedef Eigen::Matrix<double, ZDim, D> MatrixZD; ///< type of an F block
typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera Hessian
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks_; ///< All ZDim*D F blocks (one for each camera)
FBlocks FBlocks_; ///< All ZDim*D F blocks (one for each camera)
const Matrix PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point
const Vector b_; ///< 2m-dimensional RHS vector
@ -52,17 +67,25 @@ public:
}
/// Construct from blocks of F, E, inv(E'*E), and RHS vector b
RegularImplicitSchurFactor(const KeyVector& keys,
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix& P,
const Vector& b) :
GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) {
}
/**
* @brief Construct a new RegularImplicitSchurFactor object.
*
* @param keys keys corresponding to cameras
* @param Fs All ZDim*D F blocks (one for each camera)
* @param E Jacobian of measurements wrpt point.
* @param P point covariance matrix
* @param b RHS vector
*/
RegularImplicitSchurFactor(const KeyVector& keys, const FBlocks& Fs,
const Matrix& E, const Matrix& P, const Vector& b)
: GaussianFactor(keys), FBlocks_(Fs), PointCovariance_(P), E_(E), b_(b) {}
/// Destructor
~RegularImplicitSchurFactor() override {
}
std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks() const {
const FBlocks& Fs() const {
return FBlocks_;
}

155
gtsam/slam/SmartFactorBase.h
View File

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

106
gtsam/slam/SmartProjectionFactor.h
View File

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

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
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <gtsam/slam/SmartProjectionFactorP.h>
#include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/SphericalCamera.h>
#include "../SmartProjectionRigFactor.h"
using namespace std;
using namespace gtsam;
@ -70,8 +70,9 @@ SmartProjectionParams params;
// default Cal3_S2 poses
namespace vanillaPose {
typedef PinholePose<Cal3_S2> Camera;
typedef CameraSet<Camera> Cameras;
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));
Camera level_camera(level_pose, sharedK);
Camera level_camera_right(pose_right, sharedK);
@ -84,8 +85,9 @@ Camera cam3(pose_above, sharedK);
// default Cal3_S2 poses
namespace vanillaPose2 {
typedef PinholePose<Cal3_S2> Camera;
typedef CameraSet<Camera> Cameras;
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));
Camera level_camera(level_pose, sharedK2);
Camera level_camera_right(pose_right, sharedK2);
@ -98,6 +100,7 @@ Camera cam3(pose_above, sharedK2);
// Cal3Bundler cameras
namespace bundler {
typedef PinholeCamera<Cal3Bundler> Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionFactor<Camera> SmartFactor;
static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0);
Camera level_camera(level_pose, K);
@ -115,8 +118,9 @@ typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
// Cal3Bundler poses
namespace bundlerPose {
typedef PinholePose<Cal3Bundler> Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionPoseFactor<Cal3Bundler> SmartFactor;
typedef SmartProjectionFactorP<Camera> SmartFactorP;
typedef SmartProjectionRigFactor<Camera> SmartRigFactor;
static boost::shared_ptr<Cal3Bundler> sharedBundlerK(
new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
Camera level_camera(level_pose, sharedBundlerK);
@ -130,7 +134,8 @@ Camera cam3(pose_above, sharedBundlerK);
// sphericalCamera
namespace sphericalCamera {
typedef SphericalCamera Camera;
typedef SmartProjectionFactorP<Camera> SmartFactorP;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionRigFactor<Camera> SmartFactorP;
static EmptyCal::shared_ptr emptyK;
Camera level_camera(level_pose);
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.
* @addtogroup SLAM
*/
template<class CAMERA>
class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAMERA> {
template <class CAMERA>
class SmartProjectionPoseFactorRollingShutter
: public SmartProjectionFactor<CAMERA> {
private:
typedef SmartProjectionFactor<CAMERA> Base;
typedef SmartProjectionPoseFactorRollingShutter<CAMERA> This;
@ -55,9 +55,6 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
static const int ZDim = 2; ///< Measurement dimension (Point2)
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
/// frame): two consecutive poses for each observation
std::vector<std::pair<Key, Key>> world_P_body_key_pairs_;
@ -66,10 +63,17 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
/// pair of consecutive poses
std::vector<double> alphas_;
/// Pose of the camera in the body frame
std::vector<Pose3> body_P_sensors_;
/// one or more cameras taking observations (fixed poses wrt body + fixed
/// 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:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef CAMERA Camera;
typedef CameraSet<CAMERA> Cameras;
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;
/// 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
* @param Isotropic measurement noise
* @param camera single camera (fixed poses wrt body and intrinsics)
* @param params internal parameters of the smart factors
*/
SmartProjectionPoseFactorRollingShutter(
const SharedNoiseModel& sharedNoiseModel,
const SharedNoiseModel& sharedNoiseModel, const Camera& camera,
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;
: 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 */
~SmartProjectionPoseFactorRollingShutter() override = default;
/**
* add a new measurement, with 2 pose keys, interpolation factor, camera
* (intrinsic and extrinsic) calibration, and observed pixel.
* add a new measurement, with 2 pose keys, interpolation factor, and cameraId
* @param measured 2-dimensional location of the projection of a single
* 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 <=
@ -110,13 +142,11 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
* >= time pixel is acquired)
* @param alpha interpolation factor in [0,1], such that if alpha = 0 the
* interpolated pose is the same as world_P_body_key1
* @param K (fixed) camera intrinsic calibration
* @param body_P_sensor (fixed) camera extrinsic calibration
* @param cameraId ID of the camera taking the measurement (default 0)
*/
void add(const MEASUREMENT& measured, const Key& world_P_body_key1,
const Key& world_P_body_key2, const double& alpha,
const boost::shared_ptr<CALIBRATION>& K,
const Pose3& body_P_sensor = Pose3::identity()) {
const size_t& cameraId = 0) {
// store measurements in base class
this->measured_.push_back(measured);
@ -136,11 +166,8 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
// store interpolation factor
alphas_.push_back(alpha);
// store fixed intrinsic calibration
K_all_.push_back(K);
// store fixed extrinsics of the camera
body_P_sensors_.push_back(body_P_sensor);
// store id of the camera taking the measurement
cameraIds_.push_back(cameraId);
}
/**
@ -153,56 +180,36 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
* 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 Ks vector of (fixed) intrinsic calibration objects
* @param body_P_sensors vector of (fixed) extrinsic calibration objects
* @param cameraIds IDs of the cameras taking each measurement (same order as
* the measurements)
*/
void add(const MEASUREMENTS& measurements,
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
const std::vector<double>& alphas,
const std::vector<boost::shared_ptr<CALIBRATION>>& Ks,
const std::vector<Pose3>& body_P_sensors) {
assert(world_P_body_key_pairs.size() == measurements.size());
assert(world_P_body_key_pairs.size() == alphas.size());
assert(world_P_body_key_pairs.size() == Ks.size());
const FastVector<size_t>& cameraIds = FastVector<size_t>()) {
if (world_P_body_key_pairs.size() != measurements.size() ||
world_P_body_key_pairs.size() != alphas.size() ||
(world_P_body_key_pairs.size() != cameraIds.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++) {
add(measurements[i], world_P_body_key_pairs[i].first,
world_P_body_key_pairs[i].second, alphas[i], Ks[i],
body_P_sensors[i]);
world_P_body_key_pairs[i].second, alphas[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
/// interpolate
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
const std::vector<double>& alphas() const { return alphas_; }
/// return the extrinsic camera calibration body_P_sensors
const std::vector<Pose3>& body_P_sensors() const { return body_P_sensors_; }
/// return the calibration object
const Cameras& cameraRig() const { return cameraRig_; }
/// return the calibration object
const FastVector<size_t>& cameraIds() const { return cameraIds_; }
/**
* print
@ -224,15 +234,15 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
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 << " pose1 key: "
<< keyFormatter(world_P_body_key_pairs_[i].first) << std::endl;
std::cout << " pose2 key: "
<< keyFormatter(world_P_body_key_pairs_[i].second) << std::endl;
std::cout << " alpha: " << alphas_[i] << std::endl;
body_P_sensors_[i].print("extrinsic calibration:\n");
K_all_[i]->print("intrinsic calibration = ");
std::cout << "cameraId: " << cameraIds_[i] << std::endl;
cameraRig_[cameraIds_[i]].print("camera in rig:\n");
}
Base::print("", keyFormatter);
}
@ -260,57 +270,10 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
keyPairsEqual = false;
}
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() &&
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;
}
return e && Base::equals(p, tol) && alphas_ == e->alphas() &&
keyPairsEqual && cameraRig_.equals(e->cameraRig()) &&
std::equal(cameraIds_.begin(), cameraIds_.end(),
e->cameraIds().begin());
}
/**
@ -345,9 +308,12 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
auto w_P_body =
interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor,
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);
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
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)
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 {
// we may have multiple observation sharing the same keys (due to the
// rolling shutter interpolation), hence the number of unique keys may be
@ -380,19 +346,21 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
this->keys_
.size(); // note: by construction, keys_ only contains unique keys
typename Base::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 interpolated camera
cameras.size()) // 1 observation per interpolated camera
throw std::runtime_error(
"SmartProjectionPoseFactorRollingShutter: "
"measured_.size() inconsistent with input");
// 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->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)
* @param values Values structure which must contain camera poses and extrinsic pose for this factor
* 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;
}
}
/**
* 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
*/
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
// linear factors
switch (this->params_.linearizationMode) {
@ -469,7 +476,6 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar& BOOST_SERIALIZATION_NVP(K_all_);
}
};
// 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,
boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
boost::optional<Matrix&> E = boost::none) const override
{
void correctForMissingMeasurements(
const Cameras& cameras, Vector& ue,
boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
boost::optional<Matrix&> E = boost::none) const override {
// when using stereo cameras, some of the measurements might be missing:
for(size_t i=0; i < cameras.size(); i++){
for (size_t i = 0; i < cameras.size(); i++) {
const StereoPoint2& z = measured_.at(i);
if(std::isnan(z.uR())) // if the right pixel is invalid
if (std::isnan(z.uR())) // if the right 2D measurement is invalid
{
if(Fs){ // delete influence of right point on jacobian Fs
if (Fs) { // delete influence of right point on jacobian Fs
MatrixZD& Fi = Fs->at(i);
for(size_t ii=0; ii<Dim; ii++)
Fi(1,ii) = 0.0;
for (size_t ii = 0; ii < Dim; ii++) Fi(1, ii) = 0.0;
}
if(E) // delete influence of right point on jacobian E
if (E) // delete influence of right point on jacobian E
E->row(ZDim * i + 1) = Matrix::Zero(1, E->cols());
// set the corresponding entry of vector ue to zero

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

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

@ -16,18 +16,19 @@
* @date July 2021
*/
#include "gtsam/slam/tests/smartFactorScenarios.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 <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.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 <iostream>
#include "gtsam/slam/tests/smartFactorScenarios.h"
#define DISABLE_TIMING
using namespace gtsam;
@ -60,10 +61,13 @@ static double interp_factor1 = 0.3;
static double interp_factor2 = 0.4;
static double interp_factor3 = 0.5;
static size_t cameraId1 = 0;
/* ************************************************************************* */
// default Cal3_S2 poses with rolling shutter effect
namespace vanillaPoseRS {
typedef PinholePose<Cal3_S2> Camera;
typedef CameraSet<Camera> Cameras;
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_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 cam2(interp_pose2, sharedK);
Camera cam3(interp_pose3, sharedK);
SmartProjectionParams params(
gtsam::HESSIAN,
gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors
} // namespace vanillaPoseRS
LevenbergMarquardtParams lmParams;
@ -79,26 +86,29 @@ typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
/* ************************************************************************* */
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) {
SmartProjectionParams params;
using namespace vanillaPoseRS;
params.setRankTolerance(rankTol);
SmartFactorRS factor1(model, params);
SmartFactorRS factor1(model, Camera(Pose3::identity(), sharedK), params);
}
/* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, add) {
using namespace vanillaPose;
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
factor1->add(measurement1, x1, x2, interp_factor, sharedK, body_P_sensor);
using namespace vanillaPoseRS;
SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
factor1->add(measurement1, x1, x2, interp_factor);
}
/* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
using namespace vanillaPose;
using namespace vanillaPoseRS;
// create fake 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(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;
interp_factors.push_back(interp_factor1);
interp_factors.push_back(interp_factor2);
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
SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model));
factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations,
extrinsicCalibrations);
SmartFactorRS::shared_ptr factor2(
new SmartFactorRS(model, cameraRig, params));
factor2->add(measurements, key_pairs, interp_factors, cameraIds);
// create by adding a batch of measurements with a single calibrations
SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model));
factor3->add(measurements, key_pairs, interp_factors, sharedK, body_P_sensor);
SmartFactorRS::shared_ptr factor3(
new SmartFactorRS(model, cameraRig, params));
factor3->add(measurements, key_pairs, interp_factors, cameraIds);
{ // create equal factors and show equal returns true
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor);
factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor);
factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor);
SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, cameraRig, params));
factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
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(*factor3));
}
{ // create slightly different factors (different keys) and show equal
// returns false
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor);
factor1->add(measurement2, x2, x2, interp_factor2, sharedK,
body_P_sensor); // different!
factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor);
// returns false (use default cameraIds)
SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, cameraRig, params));
factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
factor1->add(measurement2, x2, x2, interp_factor2,
cameraId1); // different!
factor1->add(measurement3, x3, x4, interp_factor3, cameraId1);
EXPECT(!factor1->equals(*factor2));
EXPECT(!factor1->equals(*factor3));
}
{ // create slightly different factors (different extrinsics) and show equal
// returns false
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor);
factor1->add(measurement2, x2, x3, interp_factor2, sharedK,
body_P_sensor * body_P_sensor); // different!
factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor);
Cameras cameraRig2;
cameraRig2.push_back(Camera(body_P_sensor * body_P_sensor, sharedK));
SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, cameraRig2, params));
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(*factor3));
}
{ // create slightly different factors (different interp factors) and show
// equal returns false
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor);
factor1->add(measurement2, x2, x3, interp_factor1, sharedK,
body_P_sensor); // different!
factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor);
SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, cameraRig, params));
factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
factor1->add(measurement2, x2, x3, interp_factor1,
cameraId1); // different!
factor1->add(measurement3, x3, x4, interp_factor3, cameraId1);
EXPECT(!factor1->equals(*factor2));
EXPECT(!factor1->equals(*factor3));
@ -196,9 +226,9 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) {
Point2 level_uv_right = cam2.project(landmark1);
Pose3 body_P_sensorId = Pose3::identity();
SmartFactorRS factor(model);
factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId);
factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId);
SmartFactorRS factor(model, Camera(body_P_sensorId, sharedK), params);
factor.add(level_uv, x1, x2, interp_factor1);
factor.add(level_uv_right, x2, x3, interp_factor2);
Values values; // it's a pose factor, hence these are poses
values.insert(x1, level_pose);
@ -271,10 +301,9 @@ TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) {
Point2 level_uv_right = cam2.project(landmark1);
Pose3 body_P_sensorNonId = body_P_sensor;
SmartFactorRS factor(model);
factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId);
factor.add(level_uv_right, x2, x3, interp_factor2, sharedK,
body_P_sensorNonId);
SmartFactorRS factor(model, Camera(body_P_sensorNonId, sharedK), params);
factor.add(level_uv, x1, x2, interp_factor1);
factor.add(level_uv_right, x2, x3, interp_factor2);
Values values; // it's a pose factor, hence these are poses
values.insert(x1, level_pose);
@ -363,14 +392,17 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3);
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK);
SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK);
SmartFactorRS::shared_ptr smartFactor2(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK);
SmartFactorRS::shared_ptr smartFactor3(
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);
@ -410,6 +442,170 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
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) {
// 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
// test since in typical camera you would not have more than 1 measurement per
// landmark at each interpolated pose
using namespace vanillaPose;
using namespace vanillaPoseRS;
// Default cameras for simple derivatives
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(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
smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple,
body_P_sensorId);
smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor);
interp_factor = 1; // equivalent to measurement taken at pose 2
smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple,
body_P_sensorId);
smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor);
SmartFactor::Cameras cameras;
SmartFactorRS::Cameras cameras;
cameras.push_back(cam1);
cameras.push_back(cam2);
@ -533,14 +728,14 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) {
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
params.setEnableEPI(true);
SmartFactorRS smartFactor1(model, params);
smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK);
SmartFactorRS smartFactor1(model, Camera(Pose3::identity(), sharedK), params);
smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS smartFactor2(model, params);
smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK);
SmartFactorRS smartFactor2(model, Camera(Pose3::identity(), sharedK), params);
smartFactor2.add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS smartFactor3(model, params);
smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK);
SmartFactorRS smartFactor3(model, Camera(Pose3::identity(), sharedK), params);
smartFactor3.add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -593,18 +788,20 @@ TEST(SmartProjectionPoseFactorRollingShutter,
SmartProjectionParams params;
params.setRankTolerance(1.0);
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.setEnableEPI(false);
SmartFactorRS smartFactor1(model, params);
smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK);
SmartFactorRS smartFactor1(model, Camera(Pose3::identity(), sharedK), params);
smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS smartFactor2(model, params);
smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK);
SmartFactorRS smartFactor2(model, Camera(Pose3::identity(), sharedK), params);
smartFactor2.add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS smartFactor3(model, params);
smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK);
SmartFactorRS smartFactor3(model, Camera(Pose3::identity(), sharedK), params);
smartFactor3.add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -672,17 +869,21 @@ TEST(SmartProjectionPoseFactorRollingShutter,
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
params.setEnableEPI(false);
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, params));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK);
SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, params));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK);
SmartFactorRS::shared_ptr smartFactor2(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, params));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK);
SmartFactorRS::shared_ptr smartFactor3(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model, params));
smartFactor4->add(measurements_lmk4, key_pairs, interp_factors, sharedK);
SmartFactorRS::shared_ptr smartFactor4(
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);
@ -732,8 +933,9 @@ TEST(SmartProjectionPoseFactorRollingShutter,
interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3);
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK);
SmartFactorRS::shared_ptr smartFactor1(
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),
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_factor1);
SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model));
smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors,
sharedK);
SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
@ -1025,15 +1227,18 @@ TEST(SmartProjectionPoseFactorRollingShutter,
interp_factors_redundant.push_back(
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,
interp_factors_redundant, sharedK);
interp_factors_redundant);
SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK);
SmartFactorRS::shared_ptr smartFactor2(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK);
SmartFactorRS::shared_ptr smartFactor3(
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);
@ -1075,16 +1280,20 @@ TEST(SmartProjectionPoseFactorRollingShutter,
#ifndef DISABLE_TIMING
#include <gtsam/base/timing.h>
// -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0)
//| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min:
// 0 max: 0) | -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02
// children, min: 0 max: 0)
//-Total: 0 CPU (0 times, 0 wall, 0.21 children, min: 0 max: 0)
//| -SF RS LINEARIZE: 0.14 CPU
//(10000 times, 0.131202 wall, 0.14 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) {
using namespace vanillaPose;
// Default cameras for simple derivatives
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();
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(cam2.project(landmark1));
size_t nrTests = 1000;
size_t nrTests = 10000;
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
smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor,
sharedKSimple, body_P_sensorId);
smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor);
interp_factor = 1; // equivalent to measurement taken at pose 2
smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor,
sharedKSimple, body_P_sensorId);
smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor);
Values values;
values.insert(x1, pose1);
@ -1121,7 +1329,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) {
}
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[1], x2);
@ -1141,6 +1350,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) {
// spherical Camera with rolling shutter effect
namespace sphericalCameraRS {
typedef SphericalCamera Camera;
typedef CameraSet<Camera> Cameras;
typedef SmartProjectionPoseFactorRollingShutter<Camera> SmartFactorRS_spherical;
Pose3 interp_pose1 = interpolate<Pose3>(level_pose,pose_right,interp_factor1);
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_factor3);
SmartProjectionParams params;
SmartProjectionParams params(
gtsam::HESSIAN,
gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors
params.setRankTolerance(0.1);
SmartFactorRS_spherical::shared_ptr smartFactor1(
new SmartFactorRS_spherical(model,params));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, emptyK);
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), emptyK));
// SmartFactorRS_spherical::shared_ptr smartFactor2(
// new SmartFactorRS_spherical(model,params));
// smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, emptyK);
//
// SmartFactorRS_spherical::shared_ptr smartFactor3(
// new SmartFactorRS_spherical(model,params));
// smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, emptyK);
//
// 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);
// 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));
SmartFactorRS_spherical::shared_ptr smartFactor1(
new SmartFactorRS_spherical(model,cameraRig,params));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS_spherical::shared_ptr smartFactor2(
new SmartFactorRS_spherical(model,cameraRig,params));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS_spherical::shared_ptr smartFactor3(
new SmartFactorRS_spherical(model,cameraRig,params));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
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);
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));
}
/* ************************************************************************* */