Loading...
Searching...
No Matches
SmartProjectionRigFactor.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/SmartProjectionFactor.h>
namespace gtsam {
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;
static const int ZDim = 2;
protected:
KeyVector nonUniqueKeys_;
boost::shared_ptr<typename Base::Cameras> cameraRig_;
FastVector<size_t> cameraIds_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef CAMERA Camera;
typedef CameraSet<CAMERA> Cameras;
typedef boost::shared_ptr<This> shared_ptr;
SmartProjectionRigFactor() {}
SmartProjectionRigFactor(
const SharedNoiseModel& sharedNoiseModel,
const boost::shared_ptr<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");
}
~SmartProjectionRigFactor() override = default;
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);
}
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]);
}
}
const KeyVector& nonUniqueKeys() const { return nonUniqueKeys_; }
const boost::shared_ptr<Cameras>& cameraRig() const { return cameraRig_; }
const FastVector<size_t>& cameraIds() const { return cameraIds_; }
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);
}
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());
}
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;
}
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;
}
}
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;
}
}
}
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);
}
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");
}
}
boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const override {
return this->linearizeDamped(values);
}
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(nonUniqueKeys_);
// ar& BOOST_SERIALIZATION_NVP(cameraRig_);
// ar& BOOST_SERIALIZATION_NVP(cameraIds_);
}
};
// end of class declaration
template <class CAMERA>
struct traits<SmartProjectionRigFactor<CAMERA> >
: public Testable<SmartProjectionRigFactor<CAMERA> > {};
} // namespace gtsam
SmartProjectionFactor.h
Smart factor on cameras (pose + calibration).
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:33
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::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > make_shared(Args &&... args)
Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the...
Definition make_shared.h:57
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::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition CameraSet.h:36
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::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
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::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::measured_
ZVector measured_
Measurements for each of the m views.
Definition SmartFactorBase.h:79
gtsam::SmartFactorBase::measured
const ZVector & measured() const
Return the 2D measurements (ZDim, in general).
Definition SmartFactorBase.h:159
gtsam::SmartFactorBase::PointCov
static Matrix PointCov(const Matrix &E)
Computes Point Covariance P from the "point Jacobian" E.
Definition SmartFactorBase.h:274
gtsam::SmartProjectionParams
Definition SmartFactorParams.h:42
gtsam::SmartProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartProjectionFactor.h:103
gtsam::SmartProjectionFactor::result_
TriangulationResult result_
result from triangulateSafe
Definition SmartProjectionFactor.h:63
gtsam::SmartProjectionFactor::triangulateSafe
TriangulationResult triangulateSafe(const Cameras &cameras) const
Call gtsam::triangulateSafe iff we need to re-triangulate.
Definition SmartProjectionFactor.h:174
gtsam::SmartProjectionFactor::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
Calculate the error of the factor.
Definition SmartProjectionFactor.h:411
gtsam::SmartProjectionFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition SmartProjectionFactor.h:115
gtsam::SmartProjectionFactor::Camera
CAMERA Camera
shorthand for a set of cameras
Definition SmartProjectionFactor.h:74
gtsam::SmartProjectionFactor::SmartProjectionFactor
SmartProjectionFactor()
Default constructor, only for serialization.
Definition SmartProjectionFactor.h:80
gtsam::SmartProjectionRigFactor
If you are using the factor, please cite: L.
Definition SmartProjectionRigFactor.h:52
gtsam::SmartProjectionRigFactor::nonUniqueKeys_
KeyVector nonUniqueKeys_
vector of keys (one for each observation) with potentially repeated keys
Definition SmartProjectionRigFactor.h:65
gtsam::SmartProjectionRigFactor::cameraIds_
FastVector< size_t > cameraIds_
vector of camera Ids (one for each observation, in the same order), identifying which camera took the...
Definition SmartProjectionRigFactor.h:72
gtsam::SmartProjectionRigFactor::cameraIds
const FastVector< size_t > & cameraIds() const
return the calibration object
Definition SmartProjectionRigFactor.h:177
gtsam::SmartProjectionRigFactor::createHessianFactor
boost::shared_ptr< RegularHessianFactor< DimPose > > createHessianFactor(const Values &values, const double &lambda=0.0, bool diagonalDamping=false) const
linearize and return a Hessianfactor that is an approximation of error(p)
Definition SmartProjectionRigFactor.h:265
gtsam::SmartProjectionRigFactor::add
void add(const MEASUREMENT &measured, const Key &poseKey, const size_t &cameraId=0)
add a new measurement, corresponding to an observation from pose "poseKey" and taken from the camera ...
Definition SmartProjectionRigFactor.h:123
gtsam::SmartProjectionRigFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition SmartProjectionRigFactor.h:81
gtsam::SmartProjectionRigFactor::computeJacobiansWithTriangulatedPoint
void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
Compute jacobian F, E and error vector at a given linearization point.
Definition SmartProjectionRigFactor.h:247
gtsam::SmartProjectionRigFactor::add
void add(const MEASUREMENTS &measurements, const KeyVector &poseKeys, const FastVector< size_t > &cameraIds=FastVector< size_t >())
Variant of the previous "add" function in which we include multiple measurements.
Definition SmartProjectionRigFactor.h:149
gtsam::SmartProjectionRigFactor::SmartProjectionRigFactor
SmartProjectionRigFactor()
Default constructor, only for serialization.
Definition SmartProjectionRigFactor.h:84
gtsam::SmartProjectionRigFactor::error
double error(const Values &values) const override
error calculates the error of the factor.
Definition SmartProjectionRigFactor.h:230
gtsam::SmartProjectionRigFactor::cameras
Base::Cameras cameras(const Values &values) const override
Collect all cameras involved in this factor.
Definition SmartProjectionRigFactor.h:212
gtsam::SmartProjectionRigFactor::~SmartProjectionRigFactor
~SmartProjectionRigFactor() override=default
Virtual destructor.
gtsam::SmartProjectionRigFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartProjectionRigFactor.h:184
gtsam::SmartProjectionRigFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
Definition SmartProjectionRigFactor.h:349
gtsam::SmartProjectionRigFactor::access
friend class boost::serialization::access
Serialization function.
Definition SmartProjectionRigFactor.h:356
gtsam::SmartProjectionRigFactor::nonUniqueKeys
const KeyVector & nonUniqueKeys() const
return (for each observation) the (possibly non unique) keys involved in the measurements
Definition SmartProjectionRigFactor.h:171
gtsam::SmartProjectionRigFactor::cameraRig_
boost::shared_ptr< typename Base::Cameras > cameraRig_
cameras in the rig (fixed poses wrt body and intrinsics, for each camera)
Definition SmartProjectionRigFactor.h:68
gtsam::SmartProjectionRigFactor::SmartProjectionRigFactor
SmartProjectionRigFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< Cameras > &cameraRig, const SmartProjectionParams &params=SmartProjectionParams())
Constructor.
Definition SmartProjectionRigFactor.h:94
gtsam::SmartProjectionRigFactor::cameraRig
const boost::shared_ptr< Cameras > & cameraRig() const
return the calibration object
Definition SmartProjectionRigFactor.h:174
gtsam::SmartProjectionRigFactor::linearizeDamped
boost::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double &lambda=0.0) const
Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM).
Definition SmartProjectionRigFactor.h:335
gtsam::SmartProjectionRigFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition SmartProjectionRigFactor.h:198