Loading...
Searching...
No Matches
SmartProjectionFactor.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/SmartFactorBase.h>
#include <gtsam/slam/SmartFactorParams.h>
#include <gtsam/geometry/triangulation.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
#include <boost/optional.hpp>
#include <boost/make_shared.hpp>
#include <vector>
namespace gtsam {
template<class CAMERA>
class SmartProjectionFactor: public SmartFactorBase<CAMERA> {
public:
private:
typedef SmartFactorBase<CAMERA> Base;
typedef SmartProjectionFactor<CAMERA> This;
typedef SmartProjectionFactor<CAMERA> SmartProjectionCameraFactor;
protected:
SmartProjectionParams params_;
mutable TriangulationResult result_;
mutable std::vector<Pose3, Eigen::aligned_allocator<Pose3> >
cameraPosesTriangulation_;
public:
typedef boost::shared_ptr<This> shared_ptr;
typedef CAMERA Camera;
typedef CameraSet<CAMERA> Cameras;
SmartProjectionFactor() {}
SmartProjectionFactor(
const SharedNoiseModel& sharedNoiseModel,
const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel),
params_(params),
result_(TriangulationResult::Degenerate()) {}
~SmartProjectionFactor() override {
}
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override {
std::cout << s << "SmartProjectionFactor\n";
std::cout << "linearizationMode: " << params_.linearizationMode
<< std::endl;
std::cout << "triangulationParameters:\n" << params_.triangulation
<< std::endl;
std::cout << "result:\n" << result_ << std::endl;
Base::print("", keyFormatter);
}
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
const This *e = dynamic_cast<const This*>(&p);
return e && params_.linearizationMode == e->params_.linearizationMode
&& Base::equals(p, tol);
}
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.
size_t m = cameras.size();
bool retriangulate = false;
// 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],
params_.retriangulationThreshold)) {
retriangulate = true; // at least two poses are different, hence we retriangulate
break;
}
}
}
// 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++)
// cameraPosesTriangulation_[i] = cameras[i].pose();
cameraPosesTriangulation_.push_back(cameras[i].pose());
}
return retriangulate;
}
TriangulationResult triangulateSafe(const Cameras& cameras) const {
size_t m = cameras.size();
if (m < 2) // if we have a single pose the corresponding factor is uninformative
return TriangulationResult::Degenerate();
bool retriangulate = decideIfTriangulate(cameras);
if (retriangulate)
result_ = gtsam::triangulateSafe(cameras, this->measured_,
params_.triangulation);
return result_;
}
bool triangulateForLinearize(const Cameras& cameras) const {
triangulateSafe(cameras); // imperative, might reset result_
return bool(result_);
}
boost::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor(
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;
std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
std::vector<Vector> gs(numKeys);
if (this->measured_.size() != cameras.size())
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);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
Gs, gs, 0.0);
}
// Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
typename Base::FBlocks Fs;
Matrix E;
Vector b;
computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
// Whiten using noise model
Base::whitenJacobians(Fs, E, b);
// build augmented hessian
SymmetricBlockMatrix augmentedHessian = //
Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(
this->keys_, augmentedHessian);
}
// Create RegularImplicitSchurFactor factor.
boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > createRegularImplicitSchurFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda);
else
// failed: return empty
return boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> >();
}
boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createJacobianQFactor(cameras, *result_, lambda);
else
// failed: return empty
return boost::make_shared<JacobianFactorQ<Base::Dim, 2> >(this->keys_);
}
boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
const Values& values, double lambda) const {
return createJacobianQFactor(this->cameras(values), lambda);
}
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, *result_, lambda);
else
// failed: return empty
return boost::make_shared<JacobianFactorSVD<Base::Dim, 2> >(this->keys_);
}
virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
const Values& values, double lambda = 0.0) const {
return createHessianFactor(this->cameras(values), lambda);
}
virtual boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > linearizeToImplicit(
const Values& values, double lambda = 0.0) const {
return createRegularImplicitSchurFactor(this->cameras(values), lambda);
}
virtual boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > linearizeToJacobian(
const Values& values, double lambda = 0.0) const {
return createJacobianQFactor(this->cameras(values), lambda);
}
boost::shared_ptr<GaussianFactor> linearizeDamped(const Cameras& cameras,
const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different linear factors
switch (params_.linearizationMode) {
case HESSIAN:
return createHessianFactor(cameras, lambda);
case IMPLICIT_SCHUR:
return createRegularImplicitSchurFactor(cameras, lambda);
case JACOBIAN_SVD:
return createJacobianSVDFactor(cameras, lambda);
case JACOBIAN_Q:
return createJacobianQFactor(cameras, lambda);
default:
throw std::runtime_error("SmartFactorlinearize: unknown mode");
}
}
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
Cameras cameras = this->cameras(values);
return linearizeDamped(cameras, lambda);
}
boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const override {
return linearizeDamped(values);
}
bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const {
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
cameras.project2(*result_, boost::none, E);
return nonDegenerate;
}
bool triangulateAndComputeE(Matrix& E, const Values& values) const {
Cameras cameras = this->cameras(values);
return triangulateAndComputeE(E, cameras);
}
void computeJacobiansWithTriangulatedPoint(
typename Base::FBlocks& Fs, Matrix& E, Vector& b,
const Cameras& cameras) const {
if (!result_) {
// Handle degeneracy
// TODO check flag whether we should do this
Unit3 backProjected = cameras[0].backprojectPointAtInfinity(
this->measured_.at(0));
Base::computeJacobians(Fs, E, b, cameras, backProjected);
} else {
// valid result: just return Base version
Base::computeJacobians(Fs, E, b, cameras, *result_);
}
}
bool triangulateAndComputeJacobians(
typename Base::FBlocks& Fs, Matrix& E, Vector& b,
const Values& values) const {
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
return nonDegenerate;
}
bool triangulateAndComputeJacobiansSVD(
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(Fs, Enull, b, cameras, *result_);
return nonDegenerate;
}
Vector reprojectionErrorAfterTriangulation(const Values& values) const {
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
return Base::unwhitenedError(cameras, *result_);
else
return Vector::Zero(cameras.size() * 2);
}
double totalReprojectionError(const Cameras& cameras,
boost::optional<Point3> externalPoint = boost::none) const {
if (externalPoint)
result_ = TriangulationResult(*externalPoint);
else
result_ = triangulateSafe(cameras);
if (result_)
// All good, just use version in base class
return Base::totalReprojectionError(cameras, *result_);
else if (params_.degeneracyMode == HANDLE_INFINITY) {
// Otherwise, manage the exceptions with rotation-only factors
Unit3 backprojected = cameras.front().backprojectPointAtInfinity(
this->measured_.at(0));
return Base::totalReprojectionError(cameras, backprojected);
} else
// if we don't want to manage the exceptions we discard the factor
return 0.0;
}
double error(const Values& values) const override {
if (this->active(values)) {
return totalReprojectionError(Base::cameras(values));
} else { // else of active flag
return 0.0;
}
}
TriangulationResult point() const {
return result_;
}
TriangulationResult point(const Values& values) const {
Cameras cameras = this->cameras(values);
return triangulateSafe(cameras);
}
bool isValid() const { return result_.valid(); }
bool isDegenerate() const { return result_.degenerate(); }
bool isPointBehindCamera() const { return result_.behindCamera(); }
bool isOutlier() const { return result_.outlier(); }
bool isFarPoint() const { return result_.farPoint(); }
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(params_);
ar & BOOST_SERIALIZATION_NVP(result_);
ar & BOOST_SERIALIZATION_NVP(cameraPosesTriangulation_);
}
}
;
template<class CAMERA>
struct traits<SmartProjectionFactor<CAMERA> > : public Testable<
SmartProjectionFactor<CAMERA> > {
};
} // \ namespace gtsam
triangulation.h
Functions for triangulation.
dataset.h
utility functions for loading datasets
SmartFactorParams.h
Collect common parameters for SmartProjection and SmartStereoProjection factors.
SmartFactorBase.h
Base class to create smart factors on poses or cameras.
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::triangulateSafe
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
triangulateSafe: extensive checking of the outcome
Definition triangulation.h:680
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
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::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::TriangulationResult
TriangulationResult is an optional point, along with the reasons why it is invalid.
Definition triangulation.h:626
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
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::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::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::Dim
static const int Dim
Camera dimension.
Definition SmartFactorBase.h:60
gtsam::SmartFactorBase::cameras
virtual Cameras cameras(const Values &values) const
Collect all cameras: important that in key order.
Definition SmartFactorBase.h:162
gtsam::SmartFactorBase::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, const POINT &point) const
Calculate the error of the factor.
Definition SmartFactorBase.h:267
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::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::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::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::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartFactorBase.h:174
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
gtsam::SmartProjectionParams
Definition SmartFactorParams.h:42
gtsam::SmartProjectionParams::linearizationMode
LinearizationMode linearizationMode
How to linearize the factor.
Definition SmartFactorParams.h:44
gtsam::SmartProjectionFactor
SmartProjectionFactor: triangulates point and keeps an estimate of it around.
Definition SmartProjectionFactor.h:45
gtsam::SmartProjectionFactor::decideIfTriangulate
bool decideIfTriangulate(const Cameras &cameras) const
Check if the new linearization point is the same as the one used for previous triangulation.
Definition SmartProjectionFactor.h:128
gtsam::SmartProjectionFactor::createJacobianQFactor
boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor(const Values &values, double lambda) const
Create JacobianFactorQ factor, takes values.
Definition SmartProjectionFactor.h:261
gtsam::SmartProjectionFactor::createJacobianSVDFactor
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, double lambda) const
Different (faster) way to compute a JacobianFactorSVD factor.
Definition SmartProjectionFactor.h:267
gtsam::SmartProjectionFactor::computeJacobiansWithTriangulatedPoint
void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
Compute F, E only (called below in both vanilla and SVD versions) Assumes the point has been computed...
Definition SmartProjectionFactor.h:357
gtsam::SmartProjectionFactor::linearizeToHessian
virtual boost::shared_ptr< RegularHessianFactor< Base::Dim > > linearizeToHessian(const Values &values, double lambda=0.0) const
Linearize to a Hessianfactor.
Definition SmartProjectionFactor.h:277
gtsam::SmartProjectionFactor::isOutlier
bool isOutlier() const
return the outlier state
Definition SmartProjectionFactor.h:462
gtsam::SmartProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartProjectionFactor.h:103
gtsam::SmartProjectionFactor::triangulateAndComputeE
bool triangulateAndComputeE(Matrix &E, const Values &values) const
Triangulate and compute derivative of error with respect to point.
Definition SmartProjectionFactor.h:349
gtsam::SmartProjectionFactor::linearizeDamped
boost::shared_ptr< GaussianFactor > linearizeDamped(const Cameras &cameras, const double lambda=0.0) const
Linearize to Gaussian Factor.
Definition SmartProjectionFactor.h:299
gtsam::SmartProjectionFactor::isFarPoint
bool isFarPoint() const
return the farPoint state
Definition SmartProjectionFactor.h:465
gtsam::SmartProjectionFactor< Camera >::result_
TriangulationResult result_
Definition SmartProjectionFactor.h:63
gtsam::SmartProjectionFactor::createHessianFactor
boost::shared_ptr< RegularHessianFactor< Base::Dim > > createHessianFactor(const Cameras &cameras, const double lambda=0.0, bool diagonalDamping=false) const
Create a Hessianfactor that is an approximation of error(p).
Definition SmartProjectionFactor.h:199
gtsam::SmartProjectionFactor::triangulateSafe
TriangulationResult triangulateSafe(const Cameras &cameras) const
Call gtsam::triangulateSafe iff we need to re-triangulate.
Definition SmartProjectionFactor.h:174
gtsam::SmartProjectionFactor::~SmartProjectionFactor
~SmartProjectionFactor() override
Virtual destructor.
Definition SmartProjectionFactor.h:95
gtsam::SmartProjectionFactor::error
double error(const Values &values) const override
Calculate total reprojection error.
Definition SmartProjectionFactor.h:433
gtsam::SmartProjectionFactor::isValid
bool isValid() const
Is result valid?
Definition SmartProjectionFactor.h:453
gtsam::SmartProjectionFactor< Camera >::cameraPosesTriangulation_
std::vector< Pose3, Eigen::aligned_allocator< Pose3 > > cameraPosesTriangulation_
Definition SmartProjectionFactor.h:65
gtsam::SmartProjectionFactor::triangulateAndComputeE
bool triangulateAndComputeE(Matrix &E, const Cameras &cameras) const
Triangulate and compute derivative of error with respect to point.
Definition SmartProjectionFactor.h:338
gtsam::SmartProjectionFactor::linearizeToImplicit
virtual boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > linearizeToImplicit(const Values &values, double lambda=0.0) const
Linearize to an Implicit Schur factor.
Definition SmartProjectionFactor.h:283
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::linearizeToJacobian
virtual boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > linearizeToJacobian(const Values &values, double lambda=0.0) const
Linearize to a JacobianfactorQ.
Definition SmartProjectionFactor.h:289
gtsam::SmartProjectionFactor::isDegenerate
bool isDegenerate() const
return the degenerate state
Definition SmartProjectionFactor.h:456
gtsam::SmartProjectionFactor::triangulateAndComputeJacobians
bool triangulateAndComputeJacobians(typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Values &values) const
Version that takes values, and creates the point.
Definition SmartProjectionFactor.h:374
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::createJacobianQFactor
boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor(const Cameras &cameras, double lambda) const
Create JacobianFactorQ factor.
Definition SmartProjectionFactor.h:251
gtsam::SmartProjectionFactor::point
TriangulationResult point(const Values &values) const
COMPUTE the landmark.
Definition SmartProjectionFactor.h:447
gtsam::SmartProjectionFactor::SmartProjectionFactor
SmartProjectionFactor(const SharedNoiseModel &sharedNoiseModel, const SmartProjectionParams &params=SmartProjectionParams())
Constructor.
Definition SmartProjectionFactor.h:87
gtsam::SmartProjectionFactor::isPointBehindCamera
bool isPointBehindCamera() const
return the cheirality status flag
Definition SmartProjectionFactor.h:459
gtsam::SmartProjectionFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
Definition SmartProjectionFactor.h:329
gtsam::SmartProjectionFactor::triangulateAndComputeJacobiansSVD
bool triangulateAndComputeJacobiansSVD(typename Base::FBlocks &Fs, Matrix &Enull, Vector &b, const Values &values) const
takes values
Definition SmartProjectionFactor.h:385
gtsam::SmartProjectionFactor::reprojectionErrorAfterTriangulation
Vector reprojectionErrorAfterTriangulation(const Values &values) const
Calculate vector of re-projection errors, before applying noise model.
Definition SmartProjectionFactor.h:396
gtsam::SmartProjectionFactor::point
TriangulationResult point() const
return the landmark
Definition SmartProjectionFactor.h:442
gtsam::SmartProjectionFactor::triangulateForLinearize
bool triangulateForLinearize(const Cameras &cameras) const
Possibly re-triangulate before calculating Jacobians.
Definition SmartProjectionFactor.h:193
gtsam::SmartProjectionFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition SmartProjectionFactor.h:71
gtsam::SmartProjectionFactor::SmartProjectionFactor
SmartProjectionFactor()
Default constructor, only for serialization.
Definition SmartProjectionFactor.h:80
gtsam::SmartProjectionFactor::linearizeDamped
boost::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double lambda=0.0) const
Linearize to Gaussian Factor.
Definition SmartProjectionFactor.h:321
Symbol.h