125 lines
4.3 KiB
C++
125 lines
4.3 KiB
C++
|
|
/**
|
|
* @file InvDepthFactor3.h
|
|
* @brief Inverse Depth Factor based on Civera09tro, Montiel06rss.
|
|
* Landmarks are initialized from the first camera observation with
|
|
* (x,y,z,theta,phi,inv_depth), where x,y,z are the coordinates of
|
|
* the camera. InvDepthCamera provides methods to initialize inverse
|
|
* depth landmarks (backproject), and to convert inverse depth
|
|
* landmarks to cartesian coordinates (Point3) for visualization, etc.
|
|
* The inverse depth parameterization is split into (x,y,z,theta,phi),
|
|
* (inv_depth) to make it easy to add a prior on inverse depth alone
|
|
* @author Chris Beall
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam/geometry/Cal3_S2.h>
|
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
|
#include <gtsam_unstable/geometry/InvDepthCamera3.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* Ternary factor representing a visual measurement that includes inverse depth
|
|
*/
|
|
template<class POSE, class LANDMARK, class INVDEPTH>
|
|
class InvDepthFactor3: public NoiseModelFactorN<POSE, LANDMARK, INVDEPTH> {
|
|
protected:
|
|
|
|
// Keep a copy of measurement and calibration for I/O
|
|
Point2 measured_; ///< 2D measurement
|
|
boost::shared_ptr<Cal3_S2> K_; ///< shared pointer to calibration object
|
|
|
|
public:
|
|
|
|
/// shorthand for base class type
|
|
typedef NoiseModelFactorN<POSE, LANDMARK, INVDEPTH> Base;
|
|
|
|
/// shorthand for this class
|
|
typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This;
|
|
|
|
/// shorthand for a smart pointer to a factor
|
|
typedef boost::shared_ptr<This> shared_ptr;
|
|
|
|
/// Default constructor
|
|
InvDepthFactor3() :
|
|
measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
|
|
}
|
|
|
|
/**
|
|
* Constructor
|
|
* TODO: Mark argument order standard (keys, measurement, parameters)
|
|
* @param measured is the 2 dimensional location of point in image (the measurement)
|
|
* @param model is the standard deviation
|
|
* @param poseKey is the index of the camera pose
|
|
* @param pointKey is the index of the landmark
|
|
* @param invDepthKey is the index of inverse depth
|
|
* @param K shared pointer to the constant calibration
|
|
*/
|
|
InvDepthFactor3(const Point2& measured, const SharedNoiseModel& model,
|
|
const Key poseKey, Key pointKey, Key invDepthKey, const Cal3_S2::shared_ptr& K) :
|
|
Base(model, poseKey, pointKey, invDepthKey), measured_(measured), K_(K) {}
|
|
|
|
/** Virtual destructor */
|
|
~InvDepthFactor3() override {}
|
|
|
|
/**
|
|
* print
|
|
* @param s optional string naming the factor
|
|
* @param keyFormatter optional formatter useful for printing Symbols
|
|
*/
|
|
void print(const std::string& s = "InvDepthFactor3",
|
|
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
|
|
Base::print(s, keyFormatter);
|
|
traits<Point2>::Print(measured_, s + ".z");
|
|
}
|
|
|
|
/// 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) && traits<Point2>::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol);
|
|
}
|
|
|
|
/// Evaluate error h(x)-z and optionally derivatives
|
|
Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
|
|
boost::optional<Matrix&> H1=boost::none,
|
|
boost::optional<Matrix&> H2=boost::none,
|
|
boost::optional<Matrix&> H3=boost::none) const override {
|
|
try {
|
|
InvDepthCamera3<Cal3_S2> camera(pose, K_);
|
|
return camera.project(point, invDepth, H1, H2, H3) - measured_;
|
|
} catch( CheiralityException& e) {
|
|
if (H1) *H1 = Matrix::Zero(2,6);
|
|
if (H2) *H2 = Matrix::Zero(2,5);
|
|
if (H3) *H3 = Matrix::Zero(2,1);
|
|
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
|
|
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
|
|
return Vector::Ones(2) * 2.0 * K_->fx();
|
|
}
|
|
return (Vector(1) << 0.0).finished();
|
|
}
|
|
|
|
/** return the measurement */
|
|
const Point2& imagePoint() const {
|
|
return measured_;
|
|
}
|
|
|
|
/** return the calibration object */
|
|
inline const Cal3_S2::shared_ptr calibration() const {
|
|
return K_;
|
|
}
|
|
|
|
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(measured_);
|
|
ar & BOOST_SERIALIZATION_NVP(K_);
|
|
}
|
|
};
|
|
} // \ namespace gtsam
|