Loading...
Searching...
No Matches
GeneralSFMFactor.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/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/BinaryJacobianFactor.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/concepts.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/types.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/timing.h>
#include <boost/none.hpp>
#include <boost/optional/optional.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/smart_ptr/shared_ptr.hpp>
#include <iostream>
#include <string>
namespace boost {
namespace serialization {
class access;
} /* namespace serialization */
} /* namespace boost */
namespace gtsam {
template<class CAMERA, class LANDMARK>
class GeneralSFMFactor: public NoiseModelFactorN<CAMERA, LANDMARK> {
GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA)
GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK)
static const int DimC = FixedDimension<CAMERA>::value;
static const int DimL = FixedDimension<LANDMARK>::value;
typedef Eigen::Matrix<double, 2, DimC> JacobianC;
typedef Eigen::Matrix<double, 2, DimL> JacobianL;
protected:
Point2 measured_;
public:
typedef GeneralSFMFactor<CAMERA, LANDMARK> This;
typedef NoiseModelFactorN<CAMERA, LANDMARK> Base;
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model,
Key cameraKey, Key landmarkKey)
: Base(model, cameraKey, landmarkKey), measured_(measured) {}
GeneralSFMFactor() : measured_(0.0, 0.0) {}
///< constructor that takes a Point2
GeneralSFMFactor(const Point2& p) : measured_(p) {}
GeneralSFMFactor(double x, double y) : measured_(x, y) {}
~GeneralSFMFactor() override {}
/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
*/
void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter);
traits<Point2>::Print(measured_, s + ".z");
}
*/
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) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
}
/** h(x)-z */
Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const override {
try {
return camera.project2(point,H1,H2) - measured_;
}
catch( CheiralityException& e) {
if (H1) *H1 = JacobianC::Zero();
if (H2) *H2 = JacobianL::Zero();
//TODO Print the exception via logging
return Z_2x1;
}
}
/// Linearize using fixed-size matrices
boost::shared_ptr<GaussianFactor> linearize(const Values& values) const override {
// Only linearize if the factor is active
if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();
const Key key1 = this->key1(), key2 = this->key2();
JacobianC H1;
JacobianL H2;
Vector2 b;
try {
const CAMERA& camera = values.at<CAMERA>(key1);
const LANDMARK& point = values.at<LANDMARK>(key2);
b = measured() - camera.project2(point, H1, H2);
} catch (CheiralityException& e) {
H1.setZero();
H2.setZero();
b.setZero();
//TODO Print the exception via logging
}
// Whiten the system if needed
const SharedNoiseModel& noiseModel = this->noiseModel();
if (noiseModel && !noiseModel->isUnit()) {
// TODO: implement WhitenSystem for fixed size matrices and include
// above
H1 = noiseModel->Whiten(H1);
H2 = noiseModel->Whiten(H2);
b = noiseModel->Whiten(b);
}
// Create new (unit) noiseModel, preserving constraints if applicable
SharedDiagonal model;
if (noiseModel && noiseModel->isConstrained()) {
model = boost::static_pointer_cast<noiseModel::Constrained>(noiseModel)->unit();
}
return boost::make_shared<BinaryJacobianFactor<2, DimC, DimL> >(key1, H1, key2, H2, b, model);
}
/** return the measured */
inline const Point2 measured() const {
return measured_;
}
private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
// NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};
template<class CAMERA, class LANDMARK>
struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
GeneralSFMFactor<CAMERA, LANDMARK> > {
};
template<class CALIBRATION>
class GeneralSFMFactor2: public NoiseModelFactorN<Pose3, Point3, CALIBRATION> {
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
static const int DimK = FixedDimension<CALIBRATION>::value;
protected:
Point2 measured_;
public:
typedef GeneralSFMFactor2<CALIBRATION> This;
typedef PinholeCamera<CALIBRATION> Camera;
typedef NoiseModelFactorN<Pose3, Point3, CALIBRATION> Base;
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) :
Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {}
GeneralSFMFactor2():measured_(0.0,0.0) {}
~GeneralSFMFactor2() override {}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter);
traits<Point2>::Print(measured_, s + ".z");
}
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) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
}
Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none,
boost::optional<Matrix&> H3=boost::none) const override
{
try {
Camera camera(pose3,calib);
return camera.project(point, H1, H2, H3) - measured_;
}
catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2, 6);
if (H2) *H2 = Matrix::Zero(2, 3);
if (H3) *H3 = Matrix::Zero(2, DimK);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
<< " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
}
return Z_2x1;
}
inline const Point2 measured() const {
return measured_;
}
private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
// NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
};
template<class CALIBRATION>
struct traits<GeneralSFMFactor2<CALIBRATION> > : Testable<
GeneralSFMFactor2<CALIBRATION> > {
};
} //namespace
timing.h
Timing utilities.
Testable.h
Concept check for values that can be used in unit tests.
Matrix.h
typedef and functions to augment Eigen's MatrixXd
types.h
Typedefs for easier changing of types.
Vector.h
typedef and functions to augment Eigen's VectorXd
Manifold.h
Base class and basic functions for Manifold types.
SymmetricBlockMatrix.h
Access to matrices via blocks of pre-defined sizes.
Point2.h
2D Point
Point3.h
3D Point
PinholeCamera.h
Base class for all pinhole cameras.
Pose3.h
3D Pose
NoiseModel.h
BinaryJacobianFactor.h
A binary JacobianFactor specialization that uses fixed matrix math for speed.
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
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::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::noiseModel
All noise models live in the noiseModel namespace.
Definition LossFunctions.cpp:27
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::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::CheiralityException
Definition CalibratedCamera.h:32
gtsam::PinholeCamera
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
gtsam::PinholeBaseK::project
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
Definition PinholePose.h:118
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
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::NoiseModelFactorN< CAMERA, LANDMARK >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
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::GeneralSFMFactor
Non-linear factor for a constraint derived from a 2D measurement.
Definition GeneralSFMFactor.h:60
gtsam::GeneralSFMFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
Linearize using fixed-size matrices.
Definition GeneralSFMFactor.h:138
gtsam::GeneralSFMFactor::evaluateError
Vector evaluateError(const CAMERA &camera, const LANDMARK &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
h(x)-z
Definition GeneralSFMFactor.h:124
gtsam::GeneralSFMFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition GeneralSFMFactor.h:101
gtsam::GeneralSFMFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition GeneralSFMFactor.h:118
gtsam::GeneralSFMFactor::measured
const Point2 measured() const
return the measured
Definition GeneralSFMFactor.h:177
gtsam::GeneralSFMFactor::GeneralSFMFactor
GeneralSFMFactor(const Point2 &measured, const SharedNoiseModel &model, Key cameraKey, Key landmarkKey)
Constructor.
Definition GeneralSFMFactor.h:89
gtsam::GeneralSFMFactor::GeneralSFMFactor
GeneralSFMFactor()
default constructor
Definition GeneralSFMFactor.h:93
gtsam::GeneralSFMFactor::This
GeneralSFMFactor< CAMERA, LANDMARK > This
typedef for this object
Definition GeneralSFMFactor.h:76
gtsam::GeneralSFMFactor::~GeneralSFMFactor
~GeneralSFMFactor() override
destructor
Definition GeneralSFMFactor.h:98
gtsam::GeneralSFMFactor::Base
NoiseModelFactorN< CAMERA, LANDMARK > Base
typedef for the base class
Definition GeneralSFMFactor.h:77
gtsam::GeneralSFMFactor::print
void print(const std::string &s="SFMFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition GeneralSFMFactor.h:110
gtsam::GeneralSFMFactor::access
friend class boost::serialization::access
Serialization function.
Definition GeneralSFMFactor.h:183
gtsam::GeneralSFMFactor::measured_
Point2 measured_
the 2D measurement
Definition GeneralSFMFactor.h:72
gtsam::GeneralSFMFactor2
Non-linear factor for a constraint derived from a 2D measurement.
Definition GeneralSFMFactor.h:204
gtsam::GeneralSFMFactor2::GeneralSFMFactor2
GeneralSFMFactor2()
default constructor
Definition GeneralSFMFactor.h:232
gtsam::GeneralSFMFactor2::~GeneralSFMFactor2
~GeneralSFMFactor2() override
destructor
Definition GeneralSFMFactor.h:234
gtsam::GeneralSFMFactor2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition GeneralSFMFactor.h:237
gtsam::GeneralSFMFactor2::Base
NoiseModelFactorN< Pose3, Point3, CALIBRATION > Base
typedef for the base class
Definition GeneralSFMFactor.h:217
gtsam::GeneralSFMFactor2::evaluateError
Vector evaluateError(const Pose3 &pose3, const Point3 &point, const CALIBRATION &calib, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override
h(x)-z
Definition GeneralSFMFactor.h:260
gtsam::GeneralSFMFactor2::print
void print(const std::string &s="SFMFactor2", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition GeneralSFMFactor.h:246
gtsam::GeneralSFMFactor2::measured_
Point2 measured_
the 2D measurement
Definition GeneralSFMFactor.h:211
gtsam::GeneralSFMFactor2::measured
const Point2 measured() const
return the measured
Definition GeneralSFMFactor.h:280
gtsam::GeneralSFMFactor2::Camera
PinholeCamera< CALIBRATION > Camera
typedef for camera type
Definition GeneralSFMFactor.h:216
gtsam::GeneralSFMFactor2::GeneralSFMFactor2
GeneralSFMFactor2(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key landmarkKey, Key calibKey)
Constructor.
Definition GeneralSFMFactor.h:230
gtsam::GeneralSFMFactor2::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition GeneralSFMFactor.h:254
gtsam::GeneralSFMFactor2::access
friend class boost::serialization::access
Serialization function.
Definition GeneralSFMFactor.h:286