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

move print function to cpp file for LieVector to solve the "same-name-different-type" problem in matlab wrapper.

release/4.3a0
Duy-Nguyen Ta 2013-04-30 17:15:42 +00:00
parent 26c63ec122
commit e4b29b8d8b
2 changed files with 24 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam/base/LieVector.cpp
View File

@ -32,13 +32,18 @@ LieVector::LieVector(size_t m, const double* const data)
LieVector::LieVector(size_t m, ...) LieVector::LieVector(size_t m, ...)
: Vector(m) : Vector(m)
{ {
va_list ap; va_list ap;
va_start(ap, m); va_start(ap, m);
for( size_t i = 0 ; i < m ; i++) { for( size_t i = 0 ; i < m ; i++) {
double value = va_arg(ap, double); double value = va_arg(ap, double);
(*this)(i) = value; (*this)(i) = value;
} }
va_end(ap); va_end(ap);
}
/* ************************************************************************* */
void LieVector::print(const std::string& name) const {
gtsam::print(vector(), name);
} }
} // \namespace gtsam } // \namespace gtsam

26
gtsam/base/LieVector.h
View File

@ -48,35 +48,33 @@ struct LieVector : public Vector, public DerivedValue<LieVector> {
GTSAM_EXPORT LieVector(size_t m, ...); GTSAM_EXPORT LieVector(size_t m, ...);
/** get the underlying vector */ /** get the underlying vector */
inline Vector vector() const { Vector vector() const {
return static_cast<Vector>(*this); return static_cast<Vector>(*this);
} }
/** print @param name optional string naming the object */ /** print @param name optional string naming the object */
inline void print(const std::string& name="") const { void print(const std::string& name="") const;
gtsam::print(vector(), name);
}
/** equality up to tolerance */ /** equality up to tolerance */
inline bool equals(const LieVector& expected, double tol=1e-5) const { bool equals(const LieVector& expected, double tol=1e-5) const {
return gtsam::equal(vector(), expected.vector(), tol); return gtsam::equal(vector(), expected.vector(), tol);
} }
// Manifold requirements // Manifold requirements
/** Returns dimensionality of the tangent space */ /** Returns dimensionality of the tangent space */
inline size_t dim() const { return this->size(); } size_t dim() const { return this->size(); }
/** Update the LieVector with a tangent space update */ /** Update the LieVector with a tangent space update */
inline LieVector retract(const Vector& v) const { return LieVector(vector() + v); } LieVector retract(const Vector& v) const { return LieVector(vector() + v); }
/** @return the local coordinates of another object */ /** @return the local coordinates of another object */
inline Vector localCoordinates(const LieVector& t2) const { return LieVector(t2 - vector()); } Vector localCoordinates(const LieVector& t2) const { return LieVector(t2 - vector()); }
// Group requirements // Group requirements
/** identity - NOTE: no known size at compile time - so zero length */ /** identity - NOTE: no known size at compile time - so zero length */
inline static LieVector identity() { static LieVector identity() {
throw std::runtime_error("LieVector::identity(): Don't use this function"); throw std::runtime_error("LieVector::identity(): Don't use this function");
return LieVector(); return LieVector();
} }
@ -86,7 +84,7 @@ struct LieVector : public Vector, public DerivedValue<LieVector> {
// instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector // instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector
// as the other geometry objects (Point3, Rot3, etc.) have this problem // as the other geometry objects (Point3, Rot3, etc.) have this problem
/** compose with another object */ /** compose with another object */
inline LieVector compose(const LieVector& p, LieVector compose(const LieVector& p,
boost::optional<gtsam::Matrix&> H1=boost::none, boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const { boost::optional<gtsam::Matrix&> H2=boost::none) const {
if(H1) *H1 = eye(dim()); if(H1) *H1 = eye(dim());
@ -96,7 +94,7 @@ struct LieVector : public Vector, public DerivedValue<LieVector> {
} }
/** between operation */ /** between operation */
inline LieVector between(const LieVector& l2, LieVector between(const LieVector& l2,
boost::optional<gtsam::Matrix&> H1=boost::none, boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const { boost::optional<gtsam::Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(dim()); if(H1) *H1 = -eye(dim());
@ -105,7 +103,7 @@ struct LieVector : public Vector, public DerivedValue<LieVector> {
} }
/** invert the object and yield a new one */ /** invert the object and yield a new one */
inline LieVector inverse(boost::optional<gtsam::Matrix&> H=boost::none) const { LieVector inverse(boost::optional<gtsam::Matrix&> H=boost::none) const {
if(H) *H = -eye(dim()); if(H) *H = -eye(dim());
return LieVector(-1.0 * vector()); return LieVector(-1.0 * vector());
@ -114,10 +112,10 @@ struct LieVector : public Vector, public DerivedValue<LieVector> {
// Lie functions // Lie functions
/** Expmap around identity */ /** Expmap around identity */
static inline LieVector Expmap(const Vector& v) { return LieVector(v); } static LieVector Expmap(const Vector& v) { return LieVector(v); }
/** Logmap around identity - just returns with default cast back */ /** Logmap around identity - just returns with default cast back */
static inline Vector Logmap(const LieVector& p) { return p; } static Vector Logmap(const LieVector& p) { return p; }
private: private: