Loading...
Searching...
No Matches
SmartFactorBase.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/slam/JacobianFactorQ.h>
#include <gtsam/slam/JacobianFactorSVD.h>
#include <gtsam/slam/RegularImplicitSchurFactor.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/RegularHessianFactor.h>
#include <gtsam/geometry/CameraSet.h>
#include <boost/optional.hpp>
#include <boost/serialization/optional.hpp>
#include <boost/make_shared.hpp>
#include <vector>
namespace gtsam {
template<class CAMERA>
class SmartFactorBase: public NonlinearFactor {
private:
typedef NonlinearFactor Base;
typedef SmartFactorBase<CAMERA> This;
typedef typename CAMERA::Measurement Z;
typedef typename CAMERA::MeasurementVector ZVector;
public:
static const int Dim = traits<CAMERA>::dimension;
static const int ZDim = traits<Z>::dimension;
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F blocks (derivatives wrpt camera)
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
protected:
SharedIsotropic noiseModel_;
ZVector measured_;
boost::optional<Pose3>
body_P_sensor_;
// Cache for Fblocks, to avoid a malloc ever time we re-linearize
mutable FBlocks Fs;
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
typedef boost::shared_ptr<This> shared_ptr;
typedef CameraSet<CAMERA> Cameras;
SmartFactorBase() {}
SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none,
size_t expectedNumberCameras = 10)
: body_P_sensor_(body_P_sensor), Fs(expectedNumberCameras) {
if (!sharedNoiseModel)
throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required");
SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast<
noiseModel::Isotropic>(sharedNoiseModel);
if (!sharedIsotropic)
throw std::runtime_error("SmartFactorBase: needs isotropic");
noiseModel_ = sharedIsotropic;
}
~SmartFactorBase() override {
}
void add(const Z& measured, const Key& key) {
if(std::find(keys_.begin(), keys_.end(), key) != keys_.end()) {
throw std::invalid_argument(
"SmartFactorBase::add: adding duplicate measurement for key.");
}
this->measured_.push_back(measured);
this->keys_.push_back(key);
}
void add(const ZVector& measurements, const KeyVector& cameraKeys) {
assert(measurements.size() == cameraKeys.size());
for (size_t i = 0; i < measurements.size(); i++) {
this->add(measurements[i], cameraKeys[i]);
}
}
template<class SFM_TRACK>
void add(const SFM_TRACK& trackToAdd) {
for (size_t k = 0; k < trackToAdd.numberMeasurements(); k++) {
this->measured_.push_back(trackToAdd.measurements[k].second);
this->keys_.push_back(trackToAdd.measurements[k].first);
}
}
size_t dim() const override { return ZDim * this->measured_.size(); }
const ZVector& measured() const { return measured_; }
virtual Cameras cameras(const Values& values) const {
Cameras cameras;
for(const Key& k: this->keys_)
cameras.push_back(values.at<CAMERA>(k));
return cameras;
}
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override {
std::cout << s << "SmartFactorBase, z = \n";
for (size_t k = 0; k < measured_.size(); ++k) {
std::cout << "measurement " << k<<", px = \n" << measured_[k] << "\n";
noiseModel_->print("noise model = ");
}
if(body_P_sensor_)
body_P_sensor_->print("body_P_sensor_:\n");
Base::print("", keyFormatter);
}
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
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;
}
}
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 {
// 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;
constexpr int pose_dim = traits<Pose3>::dimension;
for (size_t i = 0; i < Fs->size(); i++) {
const Pose3 world_P_body = cameras[i].pose() * sensor_P_body;
Eigen::Matrix<double, camera_dim, camera_dim> J;
J.setZero();
Eigen::Matrix<double, pose_dim, pose_dim> H;
// Call compose to compute Jacobian for camera extrinsics
world_P_body.compose(*body_P_sensor_, H);
// Assign extrinsics part of the Jacobian
J.template block<pose_dim, pose_dim>(0, 0) = H;
Fs->at(i) = Fs->at(i) * J;
}
}
// Correct the Jacobians in case some measurements are missing.
correctForMissingMeasurements(cameras, error, Fs, E);
return error;
}
virtual void correctForMissingMeasurements(
const Cameras& cameras, Vector& ue,
boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
boost::optional<Matrix&> E = boost::none) const {}
template<class POINT>
Vector whitenedError(const Cameras& cameras, const POINT& point) const {
Vector error = cameras.reprojectionError(point, measured_);
if (noiseModel_)
noiseModel_->whitenInPlace(error);
return error;
}
template<class POINT>
double totalReprojectionError(const Cameras& cameras,
const POINT& point) const {
Vector error = whitenedError(cameras, point);
return 0.5 * error.dot(error);
}
static Matrix PointCov(const Matrix& E) {
return (E.transpose() * E).inverse();
}
template<class POINT>
void computeJacobians(FBlocks& Fs, Matrix& E, Vector& b,
const Cameras& cameras, const POINT& point) const {
// Project into Camera set and calculate derivatives
// As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar)
// Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z|
// = |A*dx - (z-h(x_bar))|
b = -unwhitenedError(cameras, point, Fs, E);
}
template<class POINT>
void computeJacobiansSVD(FBlocks& Fs, Matrix& Enull,
Vector& b, const Cameras& cameras, const POINT& point) const {
Matrix E;
computeJacobians(Fs, E, b, cameras, point);
static const int N = FixedDimension<POINT>::value; // 2 (Unit3) or 3 (Point3)
// Do SVD on A.
Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
size_t m = this->keys_.size();
Enull = svd.matrixU().block(0, N, ZDim * m, ZDim * m - N); // last ZDim*m-N columns
}
// 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 {
Matrix E;
Vector b;
computeJacobians(Fs, E, b, cameras, point);
// build augmented hessian
SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fs, E, b);
return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
augmentedHessian);
}
void updateAugmentedHessian(const Cameras& cameras, const Point3& point,
const double lambda, bool diagonalDamping,
SymmetricBlockMatrix& augmentedHessian,
const KeyVector allKeys) const {
Matrix E;
Vector b;
computeJacobians(Fs, E, b, cameras, point);
Cameras::UpdateSchurComplement(Fs, E, b, allKeys, keys_, augmentedHessian);
}
void whitenJacobians(FBlocks& F, Matrix& E, Vector& b) const {
noiseModel_->WhitenSystem(E, b);
// TODO make WhitenInPlace work with any dense matrix type
for (size_t i = 0; i < F.size(); i++)
F[i] = noiseModel_->Whiten(F[i]);
}
boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > //
createRegularImplicitSchurFactor(const Cameras& cameras, const Point3& point,
double lambda = 0.0, bool diagonalDamping = false) const {
Matrix E;
Vector b;
FBlocks F;
computeJacobians(F, E, b, cameras, point);
whitenJacobians(F, E, b);
Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_, F, E,
P, b);
}
boost::shared_ptr<JacobianFactorQ<Dim, ZDim> > createJacobianQFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const {
Matrix E;
Vector b;
FBlocks F;
computeJacobians(F, E, b, cameras, point);
const size_t M = b.size();
Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma());
return boost::make_shared<JacobianFactorQ<Dim, ZDim> >(keys_, F, E, P, b, n);
}
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
size_t m = this->keys_.size();
FBlocks F;
Vector b;
const size_t M = ZDim * m;
Matrix E0(M, M - 3);
computeJacobiansSVD(F, E0, b, cameras, point);
SharedIsotropic n = noiseModel::Isotropic::Sigma(M - 3,
noiseModel_->sigma());
return boost::make_shared<JacobianFactorSVD<Dim, ZDim> >(keys_, F, E0, b, n);
}
static void FillDiagonalF(const FBlocks& Fs, Matrix& F) {
size_t m = Fs.size();
F.resize(ZDim * m, Dim * m);
F.setZero();
for (size_t i = 0; i < m; ++i)
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_;
else
return Pose3(); // if unspecified, the transformation is the identity
}
private:
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(noiseModel_);
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
};
// end class SmartFactorBase
// Definitions need to avoid link errors (above are only declarations)
template<class CAMERA> const int SmartFactorBase<CAMERA>::Dim;
template<class CAMERA> const int SmartFactorBase<CAMERA>::ZDim;
} // \ namespace gtsam
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
CameraSet.h
Base class to create smart factors on poses or cameras.
RegularHessianFactor.h
HessianFactor class with constant sized blocks.
NonlinearFactor.h
Non-linear factor base classes.
RegularImplicitSchurFactor.h
A subclass of GaussianFactor specialized to structureless SFM.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
gtsam::svd
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
SVD computes economy SVD A=U*S*V'.
Definition Matrix.cpp:560
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition CameraSet.h:36
gtsam::CameraSet::PointCov
static Matrix PointCov(const Matrix &E, const double lambda=0.0, bool diagonalDamping=false)
Computes Point Covariance P, with lambda parameter, dynamic version.
Definition CameraSet.h:331
gtsam::CameraSet::UpdateSchurComplement
static void UpdateSchurComplement(const FBlocks &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b, const KeyVector &allKeys, const KeyVector &keys, SymmetricBlockMatrix &augmentedHessian)
Applies Schur complement (exploiting block structure) to get a smart factor on cameras,...
Definition CameraSet.h:369
gtsam::CameraSet::SchurComplement
static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > > > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * ...
Definition CameraSet.h:150
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
gtsam::noiseModel::Isotropic
An isotropic noise model corresponds to a scaled diagonal covariance To construct,...
Definition NoiseModel.h:516
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
An isotropic noise model created by specifying a standard devation sigma.
Definition NoiseModel.cpp:597
gtsam::NonlinearFactor::equals
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
Check if two factors are equal.
Definition NonlinearFactor.cpp:47
gtsam::NonlinearFactor::NonlinearFactor
NonlinearFactor()
Default constructor for I/O only.
Definition NonlinearFactor.h:58
gtsam::NonlinearFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition NonlinearFactor.cpp:37
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
gtsam::SmartFactorBase::computeJacobians
void computeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const
Compute F, E, and b (called below in both vanilla and SVD versions), where F is a vector of derivativ...
Definition SmartFactorBase.h:285
gtsam::SmartFactorBase::add
void add(const Z &measured, const Key &key)
Add a new measurement and pose/camera key.
Definition SmartFactorBase.h:126
gtsam::SmartFactorBase::~SmartFactorBase
~SmartFactorBase() override
Virtual destructor, subclasses from NonlinearFactor.
Definition SmartFactorBase.h:118
gtsam::SmartFactorBase::updateAugmentedHessian
void updateAugmentedHessian(const Cameras &cameras, const Point3 &point, const double lambda, bool diagonalDamping, SymmetricBlockMatrix &augmentedHessian, const KeyVector allKeys) const
Add the contribution of the smart factor to a pre-allocated Hessian, using sparse linear algebra.
Definition SmartFactorBase.h:336
gtsam::SmartFactorBase::createRegularImplicitSchurFactor
boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > createRegularImplicitSchurFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
Return Jacobians as RegularImplicitSchurFactor with raw access.
Definition SmartFactorBase.h:356
gtsam::SmartFactorBase::noiseModel_
SharedIsotropic noiseModel_
As of Feb 22, 2015, the noise model is the same for all measurements and is isotropic.
Definition SmartFactorBase.h:72
gtsam::SmartFactorBase::Dim
static const int Dim
Camera dimension.
Definition SmartFactorBase.h:60
gtsam::SmartFactorBase::whitenedError
Vector whitenedError(const Cameras &cameras, const POINT &point) const
Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z], with the noise model ap...
Definition SmartFactorBase.h:251
gtsam::SmartFactorBase::cameras
virtual Cameras cameras(const Values &values) const
Collect all cameras: important that in key order.
Definition SmartFactorBase.h:162
gtsam::SmartFactorBase::FillDiagonalF
static void FillDiagonalF(const FBlocks &Fs, Matrix &F)
Create BIG block-diagonal matrix F from Fblocks.
Definition SmartFactorBase.h:400
gtsam::SmartFactorBase::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, const POINT &point) const
Calculate the error of the factor.
Definition SmartFactorBase.h:267
gtsam::SmartFactorBase::add
void add(const ZVector &measurements, const KeyVector &cameraKeys)
Add a bunch of measurements, together with the camera keys.
Definition SmartFactorBase.h:136
gtsam::SmartFactorBase::createHessianFactor
boost::shared_ptr< RegularHessianFactor< Dim > > createHessianFactor(const Cameras &cameras, const Point3 &point, const double lambda=0.0, bool diagonalDamping=false) const
Linearize to a Hessianfactor.
Definition SmartFactorBase.h:316
gtsam::SmartFactorBase::computeJacobiansSVD
void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
SVD version that produces smaller Jacobian matrices by doing an SVD decomposition on E,...
Definition SmartFactorBase.h:300
gtsam::SmartFactorBase::measured_
ZVector measured_
Measurements for each of the m views.
Definition SmartFactorBase.h:79
gtsam::SmartFactorBase::shared_ptr
GTSAM_MAKE_ALIGNED_OPERATOR_NEW typedef boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor.
Definition SmartFactorBase.h:91
gtsam::SmartFactorBase::unwhitenedError
Vector unwhitenedError(const Cameras &cameras, const POINT &point, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives.
Definition SmartFactorBase.h:204
gtsam::SmartFactorBase::SmartFactorBase
SmartFactorBase()
Default Constructor, for serialization.
Definition SmartFactorBase.h:97
gtsam::SmartFactorBase::whitenJacobians
void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
Whiten the Jacobians computed by computeJacobians using noiseModel_.
Definition SmartFactorBase.h:347
gtsam::SmartFactorBase::SmartFactorBase
SmartFactorBase(const SharedNoiseModel &sharedNoiseModel, boost::optional< Pose3 > body_P_sensor=boost::none, size_t expectedNumberCameras=10)
Construct with given noise model and optional arguments.
Definition SmartFactorBase.h:100
gtsam::SmartFactorBase::add
void add(const SFM_TRACK &trackToAdd)
Add an entire SfM_track (collection of cameras observing a single point).
Definition SmartFactorBase.h:148
gtsam::SmartFactorBase::measured
const ZVector & measured() const
Return the 2D measurements (ZDim, in general).
Definition SmartFactorBase.h:159
gtsam::SmartFactorBase::createJacobianSVDFactor
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
Return Jacobians as JacobianFactorSVD.
Definition SmartFactorBase.h:386
gtsam::SmartFactorBase::dim
size_t dim() const override
Return the dimension (number of rows!) of the factor.
Definition SmartFactorBase.h:156
gtsam::SmartFactorBase::createJacobianQFactor
boost::shared_ptr< JacobianFactorQ< Dim, ZDim > > createJacobianQFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
Return Jacobians as JacobianFactorQ.
Definition SmartFactorBase.h:369
gtsam::SmartFactorBase::access
friend class boost::serialization::access
Serialization function.
Definition SmartFactorBase.h:419
gtsam::SmartFactorBase::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartFactorBase.h:174
gtsam::SmartFactorBase::body_P_sensor_
boost::optional< Pose3 > body_P_sensor_
Pose of the camera in the body frame.
Definition SmartFactorBase.h:82
gtsam::SmartFactorBase::correctForMissingMeasurements
virtual void correctForMissingMeasurements(const Cameras &cameras, Vector &ue, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
This corrects the Jacobians for the case in which some 2D measurement is missing (nan).
Definition SmartFactorBase.h:241
gtsam::SmartFactorBase::ZDim
static const int ZDim
Measurement dimension.
Definition SmartFactorBase.h:61
gtsam::SmartFactorBase::PointCov
static Matrix PointCov(const Matrix &E)
Computes Point Covariance P from the "point Jacobian" E.
Definition SmartFactorBase.h:274
gtsam::SmartFactorBase::Cameras
CameraSet< CAMERA > Cameras
The CameraSet data structure is used to refer to a set of cameras.
Definition SmartFactorBase.h:94
gtsam::SmartFactorBase::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition SmartFactorBase.h:187