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

Small changes, fixed matrices

release/4.3a0
dellaert 2014-12-21 15:57:21 +01:00
parent be356947ca
commit 505c876588
2 changed files with 15 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/geometry/Point2.h
View File

@ -53,9 +53,7 @@ public:
/// @{
/// construct from 2D vector
Point2(const Vector& v) {
if(v.size() != 2)
throw std::invalid_argument("Point2 constructor from Vector requires that the Vector have dimension 2");
Point2(const Vector2& v) {
x_ = v(0);
y_ = v(1);
}
@ -178,30 +176,28 @@ 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) {
static inline Point2 Expmap(const Vector2& v) { return Point2(v); }
static Point2 Expmap(const Vector2& 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();
/// Logmap around identity
static inline Vector2 Logmap(const Point2& dp) {
return Vector2(dp.x(), dp.y());
}
/// Version with Jacobian
static Vector Logmap(const Point2& dp, OptionalJacobian<2,2> H) {
static inline Vector2 Logmap(const Point2& dp, OptionalJacobian<2,2> H) {
CONCEPT_NOT_IMPLEMENTED;
return (Vector(2) << dp.x(), dp.y()).finished();
}
/// Left-trivialized derivative of the exponential map
static Matrix dexpL(const Vector2& v) {
return eye(2);
static Matrix2 dexpL(const Vector2& v) {
return I_2x2;
}
/// Left-trivialized derivative inverse of the exponential map
static Matrix dexpInvL(const Vector2& v) {
return eye(2);
static Matrix2 dexpInvL(const Vector2& v) {
return I_2x2;
}
/// @}

4
gtsam/geometry/tests/testPoint2.cpp
View File

@ -108,6 +108,10 @@ TEST( Point2, norm ) {
EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual, 1e-9);
expectedH = numericalDerivative11(norm_proxy, x2);
EXPECT(assert_equal(expectedH,actualH));
// analytical
expectedH = (Matrix(1, 2) << x2.x()/actual, x2.y()/actual).finished();
EXPECT(assert_equal(expectedH,actualH));
}
/* ************************************************************************* */