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

stub jacobians for point2 and point3

release/4.3a0
Mike Bosse 2014-12-17 21:18:23 +01:00
parent e17baac774
commit 98c5189392
2 changed files with 36 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/geometry/Point2.h
View File

@ -33,7 +33,6 @@ namespace gtsam {
* \nosubgrouping
*/
class GTSAM_EXPORT Point2 {
private:
double x_, y_;
@ -118,7 +117,10 @@ public:
}
/// "Inverse" - negates each coordinate such that compose(p,inverse(p)) == identity()
inline Point2 inverse() const { return Point2(-x_, -y_); }
inline Point2 inverse(OptionalJacobian<2,2> H=boost::none) const {
if (H) *H = -I_2x2;
return Point2(-x_, -y_);
}
/// syntactic sugar for inverse, i.e., -p == inverse(p)
inline Point2 operator- () const {return Point2(-x_,-y_);}
@ -159,9 +161,17 @@ public:
/// Updates a with tangent space delta
inline Point2 retract(const Vector& v) const { return *this + Point2(v); }
inline Point2 retract(const Vector& v, OptionalJacobian<2,2> H1, OptionalJacobian<2,2> H2) const {
CONCEPT_NOT_IMPLEMENTED;
return *this + Point2(v);
}
/// Local coordinates of manifold neighborhood around current value
inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); }
inline Vector localCoordinates(const Point2& t2, OptionalJacobian<2,2> H1, OptionalJacobian<2,2> H2) const {
CONCEPT_NOT_IMPLEMENTED;
return Logmap(between(t2));
}
/// @}
/// @name Lie Group
@ -169,9 +179,17 @@ public:
/// Exponential map around identity - just create a Point2 from a vector
static inline Point2 Expmap(const Vector& v) { return Point2(v); }
static Point2 Expmap(const Vector& v, OptionalJacobian<2,2> H) {
CONCEPT_NOT_IMPLEMENTED;
}
/// Log map around identity - just return the Point2 as a vector
static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()).finished(); }
static Vector Logmap(const Point2& dp, OptionalJacobian<2,2> H) {
CONCEPT_NOT_IMPLEMENTED;
return (Vector(2) << dp.x(), dp.y()).finished();
}
/// @}
/// @name Vector Space

19
gtsam/geometry/Point3.h
View File

@ -87,7 +87,10 @@ namespace gtsam {
}
/// "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3()
inline Point3 inverse() const { return Point3(-x_, -y_, -z_); }
inline Point3 inverse(OptionalJacobian<3,3> H=boost::none) const {
if (H) *H = -I_3x3;
return Point3(-x_, -y_, -z_);
}
/// syntactic sugar for inverse, i.e., -p == inverse(p)
Point3 operator - () const { return Point3(-x_,-y_,-z_);}
@ -128,6 +131,10 @@ namespace gtsam {
/// Updates a with tangent space delta
inline Point3 retract(const Vector& v) const { return Point3(*this + v); }
inline Point3 retract(const Vector& v, OptionalJacobian<3,3> Horigin, OptionalJacobian<3,3> Hv) const {
CONCEPT_NOT_IMPLEMENTED;
return Point3(*this + v);
}
/// Returns inverse retraction
inline Vector3 localCoordinates(const Point3& q) const { return (q -*this).vector(); }
@ -141,10 +148,16 @@ namespace gtsam {
/// @{
/** Exponential map at identity - just create a Point3 from x,y,z */
static inline Point3 Expmap(const Vector& v) { return Point3(v); }
static inline Point3 Expmap(const Vector& v, OptionalJacobian<3,3> H=boost::none) {
if (H) *H = I_3x3;
return Point3(v);
}
/** Log map at identity - return the x,y,z of this point */
static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); }
static inline Vector3 Logmap(const Point3& dp, OptionalJacobian<3,3> H=boost::none) {
if (H) *H = I_3x3;
return Vector3(dp.x(), dp.y(), dp.z());
}
/// Left-trivialized derivative of the exponential map
static Matrix3 dexpL(const Vector& v) {