Made StereoPoint2 a vector space just like Point2, removed old-style dimensions, and fixed indentation
cbeall3
parent
913affc755
commit
c786943152
|
|
@ -18,165 +18,151 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/DerivedValue.h>
|
||||
#include <gtsam/base/VectorSpace.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A 2D stereo point, v will be same for rectified images
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT StereoPoint2 {
|
||||
public:
|
||||
static const size_t dimension = 3;
|
||||
private:
|
||||
double uL_, uR_, v_;
|
||||
/**
|
||||
* A 2D stereo point, v will be same for rectified images
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT StereoPoint2 {
|
||||
private:
|
||||
|
||||
public:
|
||||
double uL_, uR_, v_;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
public:
|
||||
enum { dimension = 3 };
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** default constructor */
|
||||
StereoPoint2() :
|
||||
/** default constructor */
|
||||
StereoPoint2() :
|
||||
uL_(0), uR_(0), v_(0) {
|
||||
}
|
||||
}
|
||||
|
||||
/** constructor */
|
||||
StereoPoint2(double uL, double uR, double v) :
|
||||
/** constructor */
|
||||
StereoPoint2(double uL, double uR, double v) :
|
||||
uL_(uL), uR_(uR), v_(v) {
|
||||
}
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
/// construct from 3D vector
|
||||
StereoPoint2(const Vector3& v) :
|
||||
uL_(v(0)), uR_(v(1)), v_(v(2)) {}
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s="") const;
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** equals */
|
||||
bool equals(const StereoPoint2& q, double tol=1e-9) const {
|
||||
return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol && fabs(v_
|
||||
- q.v_) < tol);
|
||||
}
|
||||
/** print */
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
/** equals */
|
||||
bool equals(const StereoPoint2& q, double tol = 1e-9) const {
|
||||
return (fabs(uL_ - q.uL_) < tol && fabs(uR_ - q.uR_) < tol
|
||||
&& fabs(v_ - q.v_) < tol);
|
||||
}
|
||||
|
||||
/// identity
|
||||
inline static StereoPoint2 identity() { return StereoPoint2(); }
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/// inverse
|
||||
inline StereoPoint2 inverse() const {
|
||||
return StereoPoint2()- (*this);
|
||||
}
|
||||
/// identity
|
||||
inline static StereoPoint2 identity() {
|
||||
return StereoPoint2();
|
||||
}
|
||||
|
||||
/// "Compose", just adds the coordinates of two points.
|
||||
inline StereoPoint2 compose(const StereoPoint2& p1) const {
|
||||
return *this + p1;
|
||||
}
|
||||
/// inverse
|
||||
StereoPoint2 operator-() const {
|
||||
return StereoPoint2(-uL_, -uR_, -v_);
|
||||
}
|
||||
|
||||
/// add two stereo points
|
||||
StereoPoint2 operator+(const StereoPoint2& b) const {
|
||||
return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_);
|
||||
}
|
||||
/// add
|
||||
inline StereoPoint2 operator +(const StereoPoint2& b) const {
|
||||
return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_);
|
||||
}
|
||||
|
||||
/// subtract two stereo points
|
||||
StereoPoint2 operator-(const StereoPoint2& b) const {
|
||||
return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
/// subtract
|
||||
inline StereoPoint2 operator -(const StereoPoint2& b) const {
|
||||
return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_);
|
||||
}
|
||||
|
||||
/// dimension of the variable - used to autodetect sizes */
|
||||
inline static size_t Dim() { return dimension; }
|
||||
/// @}
|
||||
/// @name Vector Space
|
||||
/// @{
|
||||
|
||||
/// return dimensionality of tangent space, DOF = 3
|
||||
inline size_t dim() const { return dimension; }
|
||||
// unit, norm, and distance don't really make sense for StereoPoint2
|
||||
|
||||
/// Updates a with tangent space delta
|
||||
inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); }
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// Returns inverse retraction
|
||||
inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); }
|
||||
/// equality
|
||||
inline bool operator ==(const StereoPoint2& q) const {return uL_== q.uL_ && uR_==q.uR_ && v_ == q.v_;}
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
/// get uL
|
||||
inline double uL() const {return uL_;}
|
||||
|
||||
/** Exponential map around identity - just create a Point2 from a vector */
|
||||
static inline StereoPoint2 Expmap(const Vector& d) {
|
||||
return StereoPoint2(d(0), d(1), d(2));
|
||||
}
|
||||
/// get uR
|
||||
inline double uR() const {return uR_;}
|
||||
|
||||
/** Log map around identity - just return the Point2 as a vector */
|
||||
static inline Vector Logmap(const StereoPoint2& p) {
|
||||
return p.vector();
|
||||
}
|
||||
/// get v
|
||||
inline double v() const {return v_;}
|
||||
|
||||
/** The difference between another point and this point */
|
||||
inline StereoPoint2 between(const StereoPoint2& p2) const {
|
||||
return p2 - *this;
|
||||
}
|
||||
/** convert to vector */
|
||||
Vector3 vector() const {
|
||||
return Vector3(uL_, uR_, v_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
/** convenient function to get a Point2 from the left image */
|
||||
Point2 point2() const {
|
||||
return Point2(uL_, v_);
|
||||
}
|
||||
|
||||
/// get uL
|
||||
inline double uL() const {return uL_;}
|
||||
/** convenient function to get a Point2 from the right image */
|
||||
Point2 right() const {
|
||||
return Point2(uR_, v_);
|
||||
}
|
||||
|
||||
/// get uR
|
||||
inline double uR() const {return uR_;}
|
||||
/// @name Deprecated
|
||||
/// @{
|
||||
inline StereoPoint2 inverse() const { return StereoPoint2()- (*this);}
|
||||
inline StereoPoint2 compose(const StereoPoint2& p1) const { return *this + p1;}
|
||||
inline StereoPoint2 between(const StereoPoint2& p2) const { return p2 - *this; }
|
||||
inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); }
|
||||
inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); }
|
||||
static inline Vector Logmap(const StereoPoint2& p) { return p.vector(); }
|
||||
static inline StereoPoint2 Expmap(const Vector& d) { return StereoPoint2(d(0), d(1), d(2)); }
|
||||
/// @}
|
||||
|
||||
/// get v
|
||||
inline double v() const {return v_;}
|
||||
/// Streaming
|
||||
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p);
|
||||
|
||||
/** convert to vector */
|
||||
Vector3 vector() const {
|
||||
return Vector3(uL_, uR_, v_);
|
||||
}
|
||||
private:
|
||||
|
||||
/** convenient function to get a Point2 from the left image */
|
||||
Point2 point2() const {
|
||||
return Point2(uL_, v_);
|
||||
}
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** convenient function to get a Point2 from the right image */
|
||||
Point2 right() const {
|
||||
return Point2(uR_, v_);
|
||||
}
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_NVP(uL_);
|
||||
ar & BOOST_SERIALIZATION_NVP(uR_);
|
||||
ar & BOOST_SERIALIZATION_NVP(v_);
|
||||
}
|
||||
|
||||
/// Streaming
|
||||
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p);
|
||||
/// @}
|
||||
|
||||
private:
|
||||
};
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
template<>
|
||||
struct traits<StereoPoint2> : public internal::VectorSpace<StereoPoint2> {};
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_NVP(uL_);
|
||||
ar & BOOST_SERIALIZATION_NVP(uR_);
|
||||
ar & BOOST_SERIALIZATION_NVP(v_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
template<>
|
||||
struct traits<StereoPoint2> : public internal::Manifold<StereoPoint2> {};
|
||||
|
||||
template<>
|
||||
struct traits<const StereoPoint2> : public internal::Manifold<StereoPoint2> {};
|
||||
template<>
|
||||
struct traits<const StereoPoint2> : public internal::VectorSpace<StereoPoint2> {};
|
||||
}
|
||||
|
|
|
|||
|
|
@ -31,7 +31,9 @@ GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2)
|
|||
|
||||
//******************************************************************************
|
||||
TEST(StereoPoint2 , Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<StereoPoint2>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<StereoPoint2 >));
|
||||
BOOST_CONCEPT_ASSERT((IsVectorSpace<StereoPoint2>));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue