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

add cpp file for LieScalar. Try to debug the "same-name-different-type" problem in matlab wrapper.

release/4.3a0
Duy-Nguyen Ta 2013-04-12 15:43:36 +00:00
parent 693a0f991a
commit ed1f5f7576
2 changed files with 29 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/base/LieScalar.cpp Normal file
View File

@ -0,0 +1,17 @@
/*
* LieScalar.cpp
*
* Created on: Apr 12, 2013
* Author: thduynguyen
*/
#include <gtsam/base/LieScalar.h>
namespace gtsam {
void LieScalar::print(const std::string& name) const {
std::cout << name << ": " << d_ << std::endl;
}
}

26
gtsam/base/LieScalar.h
View File

@ -40,41 +40,39 @@ namespace gtsam {
operator double() const { return d_; } operator double() const { return d_; }
/** 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;
std::cout << name << ": " << d_ << std::endl;
}
/** equality up to tolerance */ /** equality up to tolerance */
inline bool equals(const LieScalar& expected, double tol=1e-5) const { bool equals(const LieScalar& expected, double tol=1e-5) const {
return fabs(expected.d_ - d_) <= tol; return fabs(expected.d_ - d_) <= tol;
} }
// Manifold requirements // Manifold requirements
/** Returns dimensionality of the tangent space */ /** Returns dimensionality of the tangent space */
inline size_t dim() const { return 1; } size_t dim() const { return 1; }
inline static size_t Dim() { return 1; } static size_t Dim() { return 1; }
/** Update the LieScalar with a tangent space update */ /** Update the LieScalar with a tangent space update */
inline LieScalar retract(const Vector& v) const { return LieScalar(value() + v(0)); } LieScalar retract(const Vector& v) const { return LieScalar(value() + v(0)); }
/** @return the local coordinates of another object */ /** @return the local coordinates of another object */
inline Vector localCoordinates(const LieScalar& t2) const { return Vector_(1,(t2.value() - value())); } Vector localCoordinates(const LieScalar& t2) const { return Vector_(1,(t2.value() - value())); }
// Group requirements // Group requirements
/** identity */ /** identity */
inline static LieScalar identity() { static LieScalar identity() {
return LieScalar(); return LieScalar();
} }
/** compose with another object */ /** compose with another object */
inline LieScalar compose(const LieScalar& p) const { LieScalar compose(const LieScalar& p) const {
return LieScalar(d_ + p.d_); return LieScalar(d_ + p.d_);
} }
/** between operation */ /** between operation */
inline LieScalar between(const LieScalar& l2, LieScalar between(const LieScalar& l2,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const { boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(1); if(H1) *H1 = -eye(1);
@ -83,17 +81,17 @@ namespace gtsam {
} }
/** invert the object and yield a new one */ /** invert the object and yield a new one */
inline LieScalar inverse() const { LieScalar inverse() const {
return LieScalar(-1.0 * value()); return LieScalar(-1.0 * value());
} }
// Lie functions // Lie functions
/** Expmap around identity */ /** Expmap around identity */
static inline LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); } static LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); }
/** Logmap around identity - just returns with default cast back */ /** Logmap around identity - just returns with default cast back */
static inline Vector Logmap(const LieScalar& p) { return Vector_(1,p.value()); } static Vector Logmap(const LieScalar& p) { return Vector_(1,p.value()); }
private: private:
double d_; double d_;