285 lines
9.7 KiB
C++
285 lines
9.7 KiB
C++
|
|
/**
|
|
* @file InvDepthFactorVariant3.h
|
|
* @brief Inverse Depth Factor based on Civera09tro, Montiel06rss.
|
|
* Landmarks are parameterized as (theta,phi,rho). The factor involves
|
|
* two poses and a landmark. The first pose is the reference frame
|
|
* from which (theta, phi, rho) is measured.
|
|
* @author Chris Beall, Stephen Williams
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
|
#include <gtsam/geometry/PinholeCamera.h>
|
|
#include <gtsam/geometry/Cal3_S2.h>
|
|
#include <gtsam/geometry/Pose3.h>
|
|
#include <gtsam/geometry/Point2.h>
|
|
#include <gtsam/base/numericalDerivative.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* Binary factor representing the first visual measurement using an inverse-depth parameterization
|
|
*/
|
|
class InvDepthFactorVariant3a: public NoiseModelFactorN<Pose3, Vector3> {
|
|
protected:
|
|
|
|
// Keep a copy of measurement and calibration for I/O
|
|
Point2 measured_; ///< 2D measurement
|
|
Cal3_S2::shared_ptr K_; ///< shared pointer to calibration object
|
|
|
|
public:
|
|
|
|
/// shorthand for base class type
|
|
typedef NoiseModelFactor2<Pose3, Vector3> Base;
|
|
|
|
// Provide access to the Matrix& version of evaluateError:
|
|
using Base::evaluateError;
|
|
|
|
/// shorthand for this class
|
|
typedef InvDepthFactorVariant3a This;
|
|
|
|
/// shorthand for a smart pointer to a factor
|
|
typedef std::shared_ptr<This> shared_ptr;
|
|
|
|
/// Default constructor
|
|
InvDepthFactorVariant3a() :
|
|
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
|
|
*/
|
|
InvDepthFactorVariant3a(const Key poseKey, const Key landmarkKey,
|
|
const Point2& measured, const Cal3_S2::shared_ptr& K, const SharedNoiseModel& model) :
|
|
Base(model, poseKey, landmarkKey), measured_(measured), K_(K) {}
|
|
|
|
/** Virtual destructor */
|
|
~InvDepthFactorVariant3a() override {}
|
|
|
|
/**
|
|
* print
|
|
* @param s optional string naming the factor
|
|
* @param keyFormatter optional formatter useful for printing Symbols
|
|
*/
|
|
void print(const std::string& s = "InvDepthFactorVariant3a",
|
|
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);
|
|
}
|
|
|
|
Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const {
|
|
try {
|
|
// Calculate the 3D coordinates of the landmark in the Pose frame
|
|
double theta = landmark(0), phi = landmark(1), rho = landmark(2);
|
|
Point3 pose_P_landmark(cos(phi)*sin(theta)/rho, sin(phi)/rho, cos(phi)*cos(theta)/rho);
|
|
// Convert the landmark to world coordinates
|
|
Point3 world_P_landmark = pose.transformFrom(pose_P_landmark);
|
|
// Project landmark into Pose2
|
|
PinholeCamera<Cal3_S2> camera(pose, *K_);
|
|
return camera.project(world_P_landmark) - measured_;
|
|
} catch( CheiralityException& e) {
|
|
std::cout << e.what()
|
|
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<1>()) << "," << DefaultKeyFormatter(this->key<2>()) << "]"
|
|
<< " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) << "]"
|
|
<< std::endl;
|
|
return Vector::Ones(2) * 2.0 * K_->fx();
|
|
}
|
|
return (Vector(1) << 0.0).finished();
|
|
}
|
|
|
|
/// Evaluate error h(x)-z and optionally derivatives
|
|
Vector evaluateError(const Pose3& pose, const Vector3& landmark,
|
|
OptionalMatrixType H1, OptionalMatrixType H2) const override {
|
|
|
|
if(H1) {
|
|
(*H1) = numericalDerivative11<Vector, Pose3>(
|
|
std::bind(&InvDepthFactorVariant3a::inverseDepthError, this,
|
|
std::placeholders::_1, landmark),
|
|
pose);
|
|
}
|
|
if(H2) {
|
|
(*H2) = numericalDerivative11<Vector, Vector3>(
|
|
std::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose,
|
|
std::placeholders::_1),
|
|
landmark);
|
|
}
|
|
|
|
return inverseDepthError(pose, landmark);
|
|
}
|
|
|
|
/** return the measurement */
|
|
const Point2& imagePoint() const {
|
|
return measured_;
|
|
}
|
|
|
|
/** return the calibration object */
|
|
const Cal3_S2::shared_ptr calibration() const {
|
|
return K_;
|
|
}
|
|
|
|
private:
|
|
|
|
#if GTSAM_ENABLE_BOOST_SERIALIZATION ///
|
|
/// 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_);
|
|
}
|
|
#endif
|
|
};
|
|
|
|
/**
|
|
* Ternary factor representing a visual measurement using an inverse-depth parameterization
|
|
*/
|
|
class InvDepthFactorVariant3b: public NoiseModelFactorN<Pose3, Pose3, Vector3> {
|
|
protected:
|
|
|
|
// Keep a copy of measurement and calibration for I/O
|
|
Point2 measured_; ///< 2D measurement
|
|
Cal3_S2::shared_ptr K_; ///< shared pointer to calibration object
|
|
|
|
public:
|
|
|
|
/// shorthand for base class type
|
|
typedef NoiseModelFactorN<Pose3, Pose3, Vector3> Base;
|
|
|
|
/// shorthand for this class
|
|
typedef InvDepthFactorVariant3b This;
|
|
|
|
/// shorthand for a smart pointer to a factor
|
|
typedef std::shared_ptr<This> shared_ptr;
|
|
|
|
/// Default constructor
|
|
InvDepthFactorVariant3b() :
|
|
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
|
|
*/
|
|
InvDepthFactorVariant3b(const Key poseKey1, const Key poseKey2, const Key landmarkKey,
|
|
const Point2& measured, const Cal3_S2::shared_ptr& K, const SharedNoiseModel& model) :
|
|
Base(model, poseKey1, poseKey2, landmarkKey), measured_(measured), K_(K) {}
|
|
|
|
/** Virtual destructor */
|
|
~InvDepthFactorVariant3b() override {}
|
|
|
|
/**
|
|
* print
|
|
* @param s optional string naming the factor
|
|
* @param keyFormatter optional formatter useful for printing Symbols
|
|
*/
|
|
void print(const std::string& s = "InvDepthFactorVariant3",
|
|
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);
|
|
}
|
|
|
|
Vector inverseDepthError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark) const {
|
|
try {
|
|
// Calculate the 3D coordinates of the landmark in the Pose1 frame
|
|
double theta = landmark(0), phi = landmark(1), rho = landmark(2);
|
|
Point3 pose1_P_landmark(cos(phi)*sin(theta)/rho, sin(phi)/rho, cos(phi)*cos(theta)/rho);
|
|
// Convert the landmark to world coordinates
|
|
Point3 world_P_landmark = pose1.transformFrom(pose1_P_landmark);
|
|
// Project landmark into Pose2
|
|
PinholeCamera<Cal3_S2> camera(pose2, *K_);
|
|
return camera.project(world_P_landmark) - measured_;
|
|
} catch( CheiralityException& e) {
|
|
std::cout << e.what()
|
|
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<1>()) << "," << DefaultKeyFormatter(this->key<3>()) << "]"
|
|
<< " moved behind camera " << DefaultKeyFormatter(this->key<2>())
|
|
<< std::endl;
|
|
return Vector::Ones(2) * 2.0 * K_->fx();
|
|
}
|
|
return (Vector(1) << 0.0).finished();
|
|
}
|
|
|
|
/// Evaluate error h(x)-z and optionally derivatives
|
|
Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark,
|
|
OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override {
|
|
|
|
if(H1)
|
|
(*H1) = numericalDerivative11<Vector, Pose3>(
|
|
std::bind(&InvDepthFactorVariant3b::inverseDepthError, this,
|
|
std::placeholders::_1, pose2, landmark),
|
|
pose1);
|
|
|
|
if(H2)
|
|
(*H2) = numericalDerivative11<Vector, Pose3>(
|
|
std::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1,
|
|
std::placeholders::_1, landmark),
|
|
pose2);
|
|
|
|
if(H3)
|
|
(*H3) = numericalDerivative11<Vector, Vector3>(
|
|
std::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1,
|
|
pose2, std::placeholders::_1),
|
|
landmark);
|
|
|
|
return inverseDepthError(pose1, pose2, landmark);
|
|
}
|
|
|
|
/** return the measurement */
|
|
const Point2& imagePoint() const {
|
|
return measured_;
|
|
}
|
|
|
|
/** return the calibration object */
|
|
const Cal3_S2::shared_ptr calibration() const {
|
|
return K_;
|
|
}
|
|
|
|
private:
|
|
|
|
#if GTSAM_ENABLE_BOOST_SERIALIZATION
|
|
friend class boost::serialization::access;
|
|
/// Serialization function
|
|
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_);
|
|
}
|
|
#endif
|
|
};
|
|
|
|
} // \ namespace gtsam
|