12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Made StereoPoint2 a vector space just like Point2, removed old-style dimensions, and fixed indentation

release/4.3a0
cbeall3 2015-05-26 12:34:49 -04:00
parent 913affc755
commit c786943152
2 changed files with 111 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

232
gtsam/geometry/StereoPoint2.h
View File

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

2
gtsam/geometry/tests/testStereoPoint2.cpp
View File

@ -31,7 +31,9 @@ GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2)
//****************************************************************************** //******************************************************************************
TEST(StereoPoint2 , Concept) { TEST(StereoPoint2 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<StereoPoint2>));
BOOST_CONCEPT_ASSERT((IsManifold<StereoPoint2 >)); BOOST_CONCEPT_ASSERT((IsManifold<StereoPoint2 >));
BOOST_CONCEPT_ASSERT((IsVectorSpace<StereoPoint2>));
} }
/* ************************************************************************* */ /* ************************************************************************* */