Added several experimental versions of an Inverse Depth Factor
Stephen Williams
parent
5bd9ffc49e
commit
54808f6d44
|
|
@ -0,0 +1,139 @@
|
|||
|
||||
/**
|
||||
* @file InvDepthFactorVariant1.h
|
||||
* @brief Inverse Depth Factor based on Civera09tro, Montiel06rss.
|
||||
* Landmarks are parameterized as (x,y,z,theta,phi,rho). The factor involves
|
||||
* a single pose and a landmark. The landmark parameterization contains the
|
||||
* reference point internally (and will thus be updated as well during optimization).
|
||||
* @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/LieVector.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Binary factor representing a visual measurement using an inverse-depth parameterization
|
||||
*/
|
||||
class InvDepthFactorVariant1: public NoiseModelFactor2<Pose3, LieVector> {
|
||||
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, LieVector> Base;
|
||||
|
||||
/// shorthand for this class
|
||||
typedef InvDepthFactorVariant1 This;
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/// Default constructor
|
||||
InvDepthFactorVariant1() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param poseKey is the index of the camera pose
|
||||
* @param pointKey is the index of the landmark
|
||||
* @param measured is the 2 dimensional location of point in image (the measurement)
|
||||
* @param K shared pointer to the constant calibration
|
||||
* @param model is the standard deviation
|
||||
*/
|
||||
InvDepthFactorVariant1(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 */
|
||||
virtual ~InvDepthFactorVariant1() {}
|
||||
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
* @param keyFormatter optional formatter useful for printing Symbols
|
||||
*/
|
||||
void print(const std::string& s = "InvDepthFactorVariant1",
|
||||
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
|
||||
Base::print(s, keyFormatter);
|
||||
measured_.print(s + ".z");
|
||||
}
|
||||
|
||||
/// equals
|
||||
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
|
||||
const This *e = dynamic_cast<const This*>(&p);
|
||||
return e
|
||||
&& Base::equals(p, tol)
|
||||
&& this->measured_.equals(e->measured_, tol)
|
||||
&& this->K_->equals(*e->K_, tol);
|
||||
}
|
||||
|
||||
Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const {
|
||||
try {
|
||||
// Calculate the 3D coordinates of the landmark in the world frame
|
||||
double x = landmark(0), y = landmark(1), z = landmark(2);
|
||||
double theta = landmark(3), phi = landmark(4), rho = landmark(5);
|
||||
Point3 world_P_landmark = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
|
||||
// Project landmark into Pose2
|
||||
PinholeCamera<Cal3_S2> camera(pose, *K_);
|
||||
gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
|
||||
return reprojectionError.vector();
|
||||
} catch( CheiralityException& e) {
|
||||
std::cout << e.what()
|
||||
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
|
||||
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
|
||||
<< std::endl;
|
||||
return gtsam::ones(2) * 2.0 * K_->fx();
|
||||
}
|
||||
return gtsam::Vector_(1, 0.0);
|
||||
}
|
||||
|
||||
/// Evaluate error h(x)-z and optionally derivatives
|
||||
Vector evaluateError(const Pose3& pose, const LieVector& landmark,
|
||||
boost::optional<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> H2=boost::none) const {
|
||||
|
||||
if(H1) {
|
||||
(*H1) = numericalDerivative11<Pose3>(boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, _1, landmark), pose);
|
||||
}
|
||||
if(H2) {
|
||||
(*H2) = numericalDerivative11<LieVector>(boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, _1), landmark);
|
||||
}
|
||||
|
||||
return inverseDepthError(pose, landmark);
|
||||
}
|
||||
|
||||
/** return the measurement */
|
||||
const gtsam::Point2& imagePoint() const {
|
||||
return measured_;
|
||||
}
|
||||
|
||||
/** return the calibration object */
|
||||
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
|
||||
|
|
@ -0,0 +1,148 @@
|
|||
|
||||
/**
|
||||
* @file InvDepthFactorVariant2.h
|
||||
* @brief Inverse Depth Factor based on Civera09tro, Montiel06rss.
|
||||
* Landmarks are parameterized as (theta,phi,rho) with the reference point
|
||||
* created at landmark construction and then never updated (i.e. the point
|
||||
* [x,y,z] is treated as fixed and not part of the optimization). The factor
|
||||
* involves a single pose and a landmark.
|
||||
* @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/LieVector.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Binary factor representing a visual measurement using an inverse-depth parameterization
|
||||
*/
|
||||
class InvDepthFactorVariant2: public NoiseModelFactor2<Pose3, LieVector> {
|
||||
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
|
||||
Point3 referencePoint_; ///< the reference point/origin for this landmark
|
||||
|
||||
public:
|
||||
|
||||
/// shorthand for base class type
|
||||
typedef NoiseModelFactor2<Pose3, LieVector> Base;
|
||||
|
||||
/// shorthand for this class
|
||||
typedef InvDepthFactorVariant2 This;
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/// Default constructor
|
||||
InvDepthFactorVariant2() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param poseKey is the index of the camera pose
|
||||
* @param pointKey is the index of the landmark
|
||||
* @param measured is the 2 dimensional location of point in image (the measurement)
|
||||
* @param K shared pointer to the constant calibration
|
||||
* @param model is the standard deviation
|
||||
*/
|
||||
InvDepthFactorVariant2(const Key poseKey, const Key landmarkKey,
|
||||
const Point2& measured, const Cal3_S2::shared_ptr& K, const Point3 referencePoint,
|
||||
const SharedNoiseModel& model) :
|
||||
Base(model, poseKey, landmarkKey), measured_(measured), K_(K), referencePoint_(referencePoint) {}
|
||||
|
||||
/** Virtual destructor */
|
||||
virtual ~InvDepthFactorVariant2() {}
|
||||
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
* @param keyFormatter optional formatter useful for printing Symbols
|
||||
*/
|
||||
void print(const std::string& s = "InvDepthFactorVariant2",
|
||||
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
|
||||
Base::print(s, keyFormatter);
|
||||
measured_.print(s + ".z");
|
||||
}
|
||||
|
||||
/// equals
|
||||
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
|
||||
const This *e = dynamic_cast<const This*>(&p);
|
||||
return e
|
||||
&& Base::equals(p, tol)
|
||||
&& this->measured_.equals(e->measured_, tol)
|
||||
&& this->K_->equals(*e->K_, tol)
|
||||
&& this->referencePoint_.equals(e->referencePoint_, tol);
|
||||
}
|
||||
|
||||
Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const {
|
||||
try {
|
||||
// Calculate the 3D coordinates of the landmark in the world frame
|
||||
double theta = landmark(0), phi = landmark(1), rho = landmark(2);
|
||||
Point3 world_P_landmark = referencePoint_ + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
|
||||
// Project landmark into Pose2
|
||||
PinholeCamera<Cal3_S2> camera(pose, *K_);
|
||||
gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
|
||||
return reprojectionError.vector();
|
||||
} catch( CheiralityException& e) {
|
||||
std::cout << e.what()
|
||||
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
|
||||
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
|
||||
<< std::endl;
|
||||
return gtsam::ones(2) * 2.0 * K_->fx();
|
||||
}
|
||||
return gtsam::Vector_(1, 0.0);
|
||||
}
|
||||
|
||||
/// Evaluate error h(x)-z and optionally derivatives
|
||||
Vector evaluateError(const Pose3& pose, const LieVector& landmark,
|
||||
boost::optional<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> H2=boost::none) const {
|
||||
|
||||
if(H1) {
|
||||
(*H1) = numericalDerivative11<Pose3>(boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, _1, landmark), pose);
|
||||
}
|
||||
if(H2) {
|
||||
(*H2) = numericalDerivative11<LieVector>(boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, _1), landmark);
|
||||
}
|
||||
|
||||
return inverseDepthError(pose, landmark);
|
||||
}
|
||||
|
||||
/** return the measurement */
|
||||
const gtsam::Point2& imagePoint() const {
|
||||
return measured_;
|
||||
}
|
||||
|
||||
/** return the calibration object */
|
||||
const Cal3_S2::shared_ptr calibration() const {
|
||||
return K_;
|
||||
}
|
||||
|
||||
/** return the calibration object */
|
||||
const Point3& referencePoint() const {
|
||||
return referencePoint_;
|
||||
}
|
||||
|
||||
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_);
|
||||
ar & BOOST_SERIALIZATION_NVP(referencePoint_);
|
||||
}
|
||||
};
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
|
@ -0,0 +1,265 @@
|
|||
|
||||
/**
|
||||
* @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/LieVector.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Binary factor representing the first visual measurement using an inverse-depth parameterization
|
||||
*/
|
||||
class InvDepthFactorVariant3a: public NoiseModelFactor2<Pose3, LieVector> {
|
||||
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, LieVector> Base;
|
||||
|
||||
/// shorthand for this class
|
||||
typedef InvDepthFactorVariant3a This;
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/// Default constructor
|
||||
InvDepthFactorVariant3a() : 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 */
|
||||
virtual ~InvDepthFactorVariant3a() {}
|
||||
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
* @param keyFormatter optional formatter useful for printing Symbols
|
||||
*/
|
||||
void print(const std::string& s = "InvDepthFactorVariant3a",
|
||||
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
|
||||
Base::print(s, keyFormatter);
|
||||
measured_.print(s + ".z");
|
||||
}
|
||||
|
||||
/// equals
|
||||
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
|
||||
const This *e = dynamic_cast<const This*>(&p);
|
||||
return e
|
||||
&& Base::equals(p, tol)
|
||||
&& this->measured_.equals(e->measured_, tol)
|
||||
&& this->K_->equals(*e->K_, tol);
|
||||
}
|
||||
|
||||
Vector inverseDepthError(const Pose3& pose, const LieVector& 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.transform_from(pose_P_landmark);
|
||||
// Project landmark into Pose2
|
||||
PinholeCamera<Cal3_S2> camera(pose, *K_);
|
||||
gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
|
||||
return reprojectionError.vector();
|
||||
} catch( CheiralityException& e) {
|
||||
std::cout << e.what()
|
||||
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << "]"
|
||||
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) << "]"
|
||||
<< std::endl;
|
||||
return gtsam::ones(2) * 2.0 * K_->fx();
|
||||
}
|
||||
return gtsam::Vector_(1, 0.0);
|
||||
}
|
||||
|
||||
/// Evaluate error h(x)-z and optionally derivatives
|
||||
Vector evaluateError(const Pose3& pose, const LieVector& landmark,
|
||||
boost::optional<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> H2=boost::none) const {
|
||||
|
||||
if(H1) {
|
||||
(*H1) = numericalDerivative11<Pose3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose);
|
||||
}
|
||||
if(H2) {
|
||||
(*H2) = numericalDerivative11<LieVector>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark);
|
||||
}
|
||||
|
||||
return inverseDepthError(pose, landmark);
|
||||
}
|
||||
|
||||
/** return the measurement */
|
||||
const gtsam::Point2& imagePoint() const {
|
||||
return measured_;
|
||||
}
|
||||
|
||||
/** return the calibration object */
|
||||
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_);
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Ternary factor representing a visual measurement using an inverse-depth parameterization
|
||||
*/
|
||||
class InvDepthFactorVariant3b: public NoiseModelFactor3<Pose3, Pose3, LieVector> {
|
||||
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 NoiseModelFactor3<Pose3, Pose3, LieVector> Base;
|
||||
|
||||
/// shorthand for this class
|
||||
typedef InvDepthFactorVariant3b This;
|
||||
|
||||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/// Default constructor
|
||||
InvDepthFactorVariant3b() : 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 */
|
||||
virtual ~InvDepthFactorVariant3b() {}
|
||||
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
* @param keyFormatter optional formatter useful for printing Symbols
|
||||
*/
|
||||
void print(const std::string& s = "InvDepthFactorVariant3",
|
||||
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
|
||||
Base::print(s, keyFormatter);
|
||||
measured_.print(s + ".z");
|
||||
}
|
||||
|
||||
/// equals
|
||||
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
|
||||
const This *e = dynamic_cast<const This*>(&p);
|
||||
return e
|
||||
&& Base::equals(p, tol)
|
||||
&& this->measured_.equals(e->measured_, tol)
|
||||
&& this->K_->equals(*e->K_, tol);
|
||||
}
|
||||
|
||||
Vector inverseDepthError(const Pose3& pose1, const Pose3& pose2, const LieVector& 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.transform_from(pose1_P_landmark);
|
||||
// Project landmark into Pose2
|
||||
PinholeCamera<Cal3_S2> camera(pose2, *K_);
|
||||
gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
|
||||
return reprojectionError.vector();
|
||||
} catch( CheiralityException& e) {
|
||||
std::cout << e.what()
|
||||
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key3()) << "]"
|
||||
<< " moved behind camera " << DefaultKeyFormatter(this->key2())
|
||||
<< std::endl;
|
||||
return gtsam::ones(2) * 2.0 * K_->fx();
|
||||
}
|
||||
return gtsam::Vector_(1, 0.0);
|
||||
}
|
||||
|
||||
/// Evaluate error h(x)-z and optionally derivatives
|
||||
Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const LieVector& landmark,
|
||||
boost::optional<gtsam::Matrix&> H1=boost::none,
|
||||
boost::optional<gtsam::Matrix&> H2=boost::none,
|
||||
boost::optional<gtsam::Matrix&> H3=boost::none) const {
|
||||
|
||||
if(H1) {
|
||||
(*H1) = numericalDerivative11<Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1);
|
||||
}
|
||||
if(H2) {
|
||||
(*H2) = numericalDerivative11<Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2);
|
||||
}
|
||||
if(H3) {
|
||||
(*H3) = numericalDerivative11<LieVector>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark);
|
||||
}
|
||||
|
||||
return inverseDepthError(pose1, pose2, landmark);
|
||||
}
|
||||
|
||||
/** return the measurement */
|
||||
const gtsam::Point2& imagePoint() const {
|
||||
return measured_;
|
||||
}
|
||||
|
||||
/** return the calibration object */
|
||||
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
|
||||
|
|
@ -0,0 +1,126 @@
|
|||
/*
|
||||
* testInvDepthFactorVariant1.cpp
|
||||
*
|
||||
* Created on: Apr 13, 2012
|
||||
* Author: cbeall3
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam_unstable/slam/InvDepthFactorVariant1.h>
|
||||
#include <gtsam/nonlinear/NonlinearEquality.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InvDepthFactorVariant1, optimize) {
|
||||
|
||||
// Create two poses looking in the x-direction
|
||||
Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0));
|
||||
Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5));
|
||||
|
||||
// Create a landmark 5 meters in front of pose1 (camera center at (0,0,1))
|
||||
Point3 landmark(5, 0, 1);
|
||||
|
||||
// Create image observations
|
||||
Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
|
||||
PinholeCamera<Cal3_S2> camera1(pose1, *K);
|
||||
PinholeCamera<Cal3_S2> camera2(pose2, *K);
|
||||
Point2 pixel1 = camera1.project(landmark);
|
||||
Point2 pixel2 = camera2.project(landmark);
|
||||
|
||||
// Create expected landmark
|
||||
double x = pose1.translation().x();
|
||||
double y = pose1.translation().y();
|
||||
double z = pose1.translation().z();
|
||||
Point3 ray = landmark - pose1.translation();
|
||||
double theta = atan2(ray.y(), ray.x());
|
||||
double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
|
||||
double rho = 1./ray.norm();
|
||||
LieVector expected(6, x, y, z, theta, phi, rho);
|
||||
|
||||
|
||||
|
||||
// Create a factor graph with two inverse depth factors and two pose priors
|
||||
Key poseKey1(1);
|
||||
Key poseKey2(2);
|
||||
Key landmarkKey(100);
|
||||
SharedNoiseModel sigma(noiseModel::Unit::Create(2));
|
||||
NonlinearFactor::shared_ptr factor1(new NonlinearEquality<Pose3>(poseKey1, pose1, 100000));
|
||||
NonlinearFactor::shared_ptr factor2(new NonlinearEquality<Pose3>(poseKey2, pose2, 100000));
|
||||
NonlinearFactor::shared_ptr factor3(new InvDepthFactorVariant1(poseKey1, landmarkKey, pixel1, K, sigma));
|
||||
NonlinearFactor::shared_ptr factor4(new InvDepthFactorVariant1(poseKey2, landmarkKey, pixel2, K, sigma));
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(factor1);
|
||||
graph.push_back(factor2);
|
||||
graph.push_back(factor3);
|
||||
graph.push_back(factor4);
|
||||
|
||||
// Create a values with slightly incorrect initial conditions
|
||||
Values values;
|
||||
values.insert(poseKey1, pose1.retract(Vector_(6, +0.01, -0.02, +0.03, -0.10, +0.20, -0.30)));
|
||||
values.insert(poseKey2, pose2.retract(Vector_(6, +0.01, +0.02, -0.03, -0.10, +0.20, +0.30)));
|
||||
values.insert(landmarkKey, expected.retract(Vector_(6, -0.20, +0.20, -0.35, +0.02, -0.04, +0.05)));
|
||||
|
||||
// Optimize the graph to recover the actual landmark position
|
||||
LevenbergMarquardtParams params;
|
||||
Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
|
||||
LieVector actual = result.at<LieVector>(landmarkKey);
|
||||
|
||||
|
||||
values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
|
||||
result.at<Pose3>(poseKey1).print("Pose1 After:\n");
|
||||
pose1.print("Pose1 Truth:\n");
|
||||
std::cout << std::endl << std::endl;
|
||||
values.at<Pose3>(poseKey2).print("Pose2 Before:\n");
|
||||
result.at<Pose3>(poseKey2).print("Pose2 After:\n");
|
||||
pose2.print("Pose2 Truth:\n");
|
||||
std::cout << std::endl << std::endl;
|
||||
values.at<LieVector>(landmarkKey).print("Landmark Before:\n");
|
||||
result.at<LieVector>(landmarkKey).print("Landmark After:\n");
|
||||
expected.print("Landmark Truth:\n");
|
||||
std::cout << std::endl << std::endl;
|
||||
|
||||
// Calculate world coordinates of landmark versions
|
||||
Point3 world_landmarkBefore;
|
||||
{
|
||||
LieVector landmarkBefore = values.at<LieVector>(landmarkKey);
|
||||
double x = landmarkBefore(0), y = landmarkBefore(1), z = landmarkBefore(2);
|
||||
double theta = landmarkBefore(3), phi = landmarkBefore(4), rho = landmarkBefore(5);
|
||||
world_landmarkBefore = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
|
||||
}
|
||||
Point3 world_landmarkAfter;
|
||||
{
|
||||
LieVector landmarkAfter = result.at<LieVector>(landmarkKey);
|
||||
double x = landmarkAfter(0), y = landmarkAfter(1), z = landmarkAfter(2);
|
||||
double theta = landmarkAfter(3), phi = landmarkAfter(4), rho = landmarkAfter(5);
|
||||
world_landmarkAfter = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
|
||||
}
|
||||
|
||||
world_landmarkBefore.print("World Landmark Before:\n");
|
||||
world_landmarkAfter.print("World Landmark After:\n");
|
||||
landmark.print("World Landmark Truth:\n");
|
||||
std::cout << std::endl << std::endl;
|
||||
|
||||
|
||||
|
||||
// Test that the correct landmark parameters have been recovered
|
||||
// However, since this is an over-parameterization, there can be
|
||||
// many 6D landmark values that equate to the same 3D world position
|
||||
// Instead, directly test the recovered 3D landmark position
|
||||
//EXPECT(assert_equal(expected, actual, 1e-9));
|
||||
EXPECT(assert_equal(landmark, world_landmarkAfter, 1e-9));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,118 @@
|
|||
/*
|
||||
* testInvDepthFactorVariant1.cpp
|
||||
*
|
||||
* Created on: Apr 13, 2012
|
||||
* Author: cbeall3
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam_unstable/slam/InvDepthFactorVariant2.h>
|
||||
#include <gtsam/nonlinear/NonlinearEquality.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InvDepthFactorVariant2, optimize) {
|
||||
|
||||
// Create two poses looking in the x-direction
|
||||
Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0));
|
||||
Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5));
|
||||
|
||||
// Create a landmark 5 meters in front of pose1 (camera center at (0,0,1))
|
||||
Point3 landmark(5, 0, 1);
|
||||
|
||||
// Create image observations
|
||||
Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
|
||||
PinholeCamera<Cal3_S2> camera1(pose1, *K);
|
||||
PinholeCamera<Cal3_S2> camera2(pose2, *K);
|
||||
Point2 pixel1 = camera1.project(landmark);
|
||||
Point2 pixel2 = camera2.project(landmark);
|
||||
|
||||
// Create expected landmark
|
||||
Point3 referencePoint = pose1.translation();
|
||||
Point3 ray = landmark - referencePoint;
|
||||
double theta = atan2(ray.y(), ray.x());
|
||||
double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
|
||||
double rho = 1./ray.norm();
|
||||
LieVector expected(3, theta, phi, rho);
|
||||
|
||||
|
||||
|
||||
// Create a factor graph with two inverse depth factors and two pose priors
|
||||
Key poseKey1(1);
|
||||
Key poseKey2(2);
|
||||
Key landmarkKey(100);
|
||||
SharedNoiseModel sigma(noiseModel::Unit::Create(2));
|
||||
NonlinearFactor::shared_ptr factor1(new NonlinearEquality<Pose3>(poseKey1, pose1, 100000));
|
||||
NonlinearFactor::shared_ptr factor2(new NonlinearEquality<Pose3>(poseKey2, pose2, 100000));
|
||||
NonlinearFactor::shared_ptr factor3(new InvDepthFactorVariant2(poseKey1, landmarkKey, pixel1, K, referencePoint, sigma));
|
||||
NonlinearFactor::shared_ptr factor4(new InvDepthFactorVariant2(poseKey2, landmarkKey, pixel2, K, referencePoint, sigma));
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(factor1);
|
||||
graph.push_back(factor2);
|
||||
graph.push_back(factor3);
|
||||
graph.push_back(factor4);
|
||||
|
||||
// Create a values with slightly incorrect initial conditions
|
||||
Values values;
|
||||
values.insert(poseKey1, pose1.retract(Vector_(6, +0.01, -0.02, +0.03, -0.10, +0.20, -0.30)));
|
||||
values.insert(poseKey2, pose2.retract(Vector_(6, +0.01, +0.02, -0.03, -0.10, +0.20, +0.30)));
|
||||
values.insert(landmarkKey, expected.retract(Vector_(3, +0.02, -0.04, +0.05)));
|
||||
|
||||
// Optimize the graph to recover the actual landmark position
|
||||
LevenbergMarquardtParams params;
|
||||
Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
|
||||
LieVector actual = result.at<LieVector>(landmarkKey);
|
||||
|
||||
|
||||
values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
|
||||
result.at<Pose3>(poseKey1).print("Pose1 After:\n");
|
||||
pose1.print("Pose1 Truth:\n");
|
||||
std::cout << std::endl << std::endl;
|
||||
values.at<Pose3>(poseKey2).print("Pose2 Before:\n");
|
||||
result.at<Pose3>(poseKey2).print("Pose2 After:\n");
|
||||
pose2.print("Pose2 Truth:\n");
|
||||
std::cout << std::endl << std::endl;
|
||||
values.at<LieVector>(landmarkKey).print("Landmark Before:\n");
|
||||
result.at<LieVector>(landmarkKey).print("Landmark After:\n");
|
||||
expected.print("Landmark Truth:\n");
|
||||
std::cout << std::endl << std::endl;
|
||||
|
||||
// Calculate world coordinates of landmark versions
|
||||
Point3 world_landmarkBefore;
|
||||
{
|
||||
LieVector landmarkBefore = values.at<LieVector>(landmarkKey);
|
||||
double theta = landmarkBefore(0), phi = landmarkBefore(1), rho = landmarkBefore(2);
|
||||
world_landmarkBefore = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
|
||||
}
|
||||
Point3 world_landmarkAfter;
|
||||
{
|
||||
LieVector landmarkAfter = result.at<LieVector>(landmarkKey);
|
||||
double theta = landmarkAfter(0), phi = landmarkAfter(1), rho = landmarkAfter(2);
|
||||
world_landmarkAfter = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
|
||||
}
|
||||
|
||||
world_landmarkBefore.print("World Landmark Before:\n");
|
||||
world_landmarkAfter.print("World Landmark After:\n");
|
||||
landmark.print("World Landmark Truth:\n");
|
||||
std::cout << std::endl << std::endl;
|
||||
|
||||
|
||||
|
||||
// Test that the correct landmark parameters have been recovered
|
||||
EXPECT(assert_equal(expected, actual, 1e-9));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,85 @@
|
|||
/*
|
||||
* testInvDepthFactorVariant3.cpp
|
||||
*
|
||||
* Created on: Apr 13, 2012
|
||||
* Author: cbeall3
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam_unstable/slam/InvDepthFactorVariant3.h>
|
||||
#include <gtsam/nonlinear/NonlinearEquality.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( InvDepthFactorVariant3, optimize) {
|
||||
|
||||
// Create two poses looking in the x-direction
|
||||
Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0));
|
||||
Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5));
|
||||
|
||||
// Create a landmark 5 meters in front of pose1 (camera center at (0,0,1))
|
||||
Point3 landmark(5, 0, 1);
|
||||
|
||||
// Create image observations
|
||||
Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
|
||||
PinholeCamera<Cal3_S2> camera1(pose1, *K);
|
||||
PinholeCamera<Cal3_S2> camera2(pose2, *K);
|
||||
Point2 pixel1 = camera1.project(landmark);
|
||||
Point2 pixel2 = camera2.project(landmark);
|
||||
|
||||
// Create expected landmark
|
||||
Point3 landmark_p1 = pose1.transform_to(landmark);
|
||||
landmark_p1.print("Landmark in Pose1 Frame:\n");
|
||||
double theta = atan2(landmark_p1.x(), landmark_p1.z());
|
||||
double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z()));
|
||||
double rho = 1./landmark_p1.norm();
|
||||
LieVector expected(3, theta, phi, rho);
|
||||
|
||||
|
||||
|
||||
// Create a factor graph with two inverse depth factors and two pose priors
|
||||
Key poseKey1(1);
|
||||
Key poseKey2(2);
|
||||
Key landmarkKey(100);
|
||||
SharedNoiseModel sigma(noiseModel::Unit::Create(2));
|
||||
NonlinearFactor::shared_ptr factor1(new NonlinearEquality<Pose3>(poseKey1, pose1, 100000));
|
||||
NonlinearFactor::shared_ptr factor2(new NonlinearEquality<Pose3>(poseKey2, pose2, 100000));
|
||||
NonlinearFactor::shared_ptr factor3(new InvDepthFactorVariant3a(poseKey1, landmarkKey, pixel1, K, sigma));
|
||||
NonlinearFactor::shared_ptr factor4(new InvDepthFactorVariant3b(poseKey1, poseKey2, landmarkKey, pixel2, K, sigma));
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(factor1);
|
||||
graph.push_back(factor2);
|
||||
graph.push_back(factor3);
|
||||
graph.push_back(factor4);
|
||||
|
||||
// Create a values with slightly incorrect initial conditions
|
||||
Values values;
|
||||
values.insert(poseKey1, pose1.retract(Vector_(6, +0.01, -0.02, +0.03, -0.10, +0.20, -0.30)));
|
||||
values.insert(poseKey2, pose2.retract(Vector_(6, +0.01, +0.02, -0.03, -0.10, +0.20, +0.30)));
|
||||
values.insert(landmarkKey, expected.retract(Vector_(3, +0.02, -0.04, +0.05)));
|
||||
|
||||
// Optimize the graph to recover the actual landmark position
|
||||
LevenbergMarquardtParams params;
|
||||
Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
|
||||
LieVector actual = result.at<LieVector>(landmarkKey);
|
||||
|
||||
|
||||
|
||||
// Test that the correct landmark parameters have been recovered
|
||||
EXPECT(assert_equal(expected, actual, 1e-9));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue