22 #include <gtsam/base/VectorSpace.h> 23 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION 24 #include <boost/serialization/nvp.hpp> 40 enum { dimension = 3 };
46 uL_(0), uR_(0), v_(0) {
51 uL_(uL), uR_(uR), v_(v) {
56 uL_(v(0)), uR_(v(1)), v_(v(2)) {}
63 void print(
const std::string& s =
"")
const;
67 return (std::abs(uL_ - q.uL_) < tol && std::abs(uR_ - q.uR_) < tol
68 && std::abs(v_ - q.v_) < tol);
92 return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_);
97 return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_);
108 inline double uL()
const {
return uL_;}
111 inline double uR()
const {
return uR_;}
114 inline double v()
const {
return v_;}
118 return Vector3(uL_, uR_, v_);
136 inline Vector localCoordinates(
const StereoPoint2& t2)
const {
return Logmap(between(t2)); }
137 inline StereoPoint2 retract(
const Vector& v)
const {
return compose(Expmap(v)); }
143 GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os,
const StereoPoint2& p);
151 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION 153 friend class boost::serialization::access;
154 template<
class ARCHIVE>
155 void serialize(ARCHIVE & ar,
const unsigned int ) {
156 ar & BOOST_SERIALIZATION_NVP(uL_);
157 ar & BOOST_SERIALIZATION_NVP(uR_);
158 ar & BOOST_SERIALIZATION_NVP(v_);
166 typedef std::vector<StereoPoint2> StereoPoint2Vector;
VectorSpace provides both Testable and VectorSpaceTraits.
Definition: VectorSpace.h:207
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
Vector2 Point2
Definition: Point2.h:32
bool operator==(const Matrix &A, const Matrix &B)
Definition: Matrix.h:99
Point2 right() const
Definition: StereoPoint2.h:127
StereoPoint2(double uL, double uR, double v)
Definition: StereoPoint2.h:50
StereoPoint2 operator-() const
inverse
Definition: StereoPoint2.h:81
Vector3 vector() const
Definition: StereoPoint2.h:117
GTSAM_EXPORT void print(const Matrix &A, const std::string &s, std::ostream &stream)
double v() const
get v
Definition: StereoPoint2.h:114
StereoPoint2()
Definition: StereoPoint2.h:45
static StereoPoint2 Identity()
identity
Definition: StereoPoint2.h:76
Definition: chartTesting.h:28
Definition: StereoPoint2.h:34
double uL() const
get uL
Definition: StereoPoint2.h:108
GTSAM_EXPORT Errors operator-(const Errors &a, const Errors &b)
Subtraction.
StereoPoint2(const Vector3 &v)
construct from 3D vector
Definition: StereoPoint2.h:55
double uR() const
get uR
Definition: StereoPoint2.h:111
GTSAM_EXPORT Errors operator+(const Errors &a, const Errors &b)
Addition.
Point2 point2() const
Definition: StereoPoint2.h:122
bool equals(const StereoPoint2 &q, double tol=1e-9) const
Definition: StereoPoint2.h:66