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

Fixed bug in DefaultChart: keeping a reference s never a good idea.

release/4.3a0
dellaert 2014-10-21 11:13:14 +02:00
parent 25ad9ade05
commit 0acffe5ae9
2 changed files with 28 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/base/Manifold.h
View File

@ -148,7 +148,7 @@ template<typename T>
struct DefaultChart { struct DefaultChart {
BOOST_STATIC_ASSERT(traits::is_manifold<T>::value); BOOST_STATIC_ASSERT(traits::is_manifold<T>::value);
typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector; typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
T const & t_; T t_;
DefaultChart(const T& t) : DefaultChart(const T& t) :
t_(t) { t_(t) {
} }
@ -186,6 +186,7 @@ template<typename T> struct Canonical {
template<> template<>
struct DefaultChart<double> { struct DefaultChart<double> {
typedef Eigen::Matrix<double, 1, 1> vector; typedef Eigen::Matrix<double, 1, 1> vector;
double t_;
DefaultChart(double t) : DefaultChart(double t) :
t_(t) { t_(t) {
} }
@ -197,8 +198,6 @@ struct DefaultChart<double> {
double retract(const vector& d) { double retract(const vector& d) {
return t_ + d[0]; return t_ + d[0];
} }
private:
double t_;
}; };
// Fixed size Eigen::Matrix type // Fixed size Eigen::Matrix type
@ -207,6 +206,7 @@ template<int M, int N, int Options>
struct DefaultChart<Eigen::Matrix<double, M, N, Options> > { struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
typedef Eigen::Matrix<double, M, N, Options> T; typedef Eigen::Matrix<double, M, N, Options> T;
typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector; typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
T t_;
DefaultChart(const T& t) : DefaultChart(const T& t) :
t_(t) { t_(t) {
} }
@ -219,8 +219,6 @@ struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
Eigen::Map<const T> map(d.data()); Eigen::Map<const T> map(d.data());
return t_ + map; return t_ + map;
} }
private:
T const & t_;
}; };
/** /**

32
gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp
View File

@ -202,7 +202,7 @@ TEST(Manifold, Canonical) {
EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0)); EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0));
Canonical<Vector2> chart2; Canonical<Vector2> chart2;
EXPECT(chart2.apply(Vector2(1,0))==Vector2(1,0)); EXPECT(assert_equal((Vector)chart2.apply(Vector2(1,0)),Vector2(1,0)));
EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0)); EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0));
Canonical<double> chart3; Canonical<double> chart3;
@ -211,12 +211,30 @@ TEST(Manifold, Canonical) {
EXPECT(chart3.apply(1)==v1); EXPECT(chart3.apply(1)==v1);
EXPECT(chart3.retract(v1)==1); EXPECT(chart3.retract(v1)==1);
// Dynamic does not work yet ! Canonical<Point3> chart4;
// Vector z = zero(2), v(2); Point3 point(1,2,3);
// v << 1, 0; Vector3 v3(1,2,3);
// Canonical<Vector> chart4(z); EXPECT(assert_equal((Vector)chart4.apply(point),v3));
// EXPECT(chart4.apply(v)==v); EXPECT(assert_equal(chart4.retract(v3),point));
// EXPECT(chart4.retract(v)==v);
Canonical<Pose3> chart5;
Pose3 pose(Rot3::identity(),point);
Vector6 v6; v6 << 0,0,0,1,2,3;
EXPECT(assert_equal((Vector)chart5.apply(pose),v6));
EXPECT(assert_equal(chart5.retract(v6),pose));
Canonical<Camera> chart6;
Cal3Bundler cal0(0,0,0);
Camera camera(Pose3(),cal0);
Vector9 z9 = Vector9::Zero();
EXPECT(assert_equal((Vector)chart6.apply(camera),z9));
EXPECT(assert_equal(chart6.retract(z9),camera));
Cal3Bundler cal; // Note !! Cal3Bundler() != zero<Cal3Bundler>::value()
Camera camera2(pose,cal);
Vector9 v9; v9 << 0,0,0,1,2,3,1,0,0;
EXPECT(assert_equal((Vector)chart6.apply(camera2),v9));
EXPECT(assert_equal(chart6.retract(v9),camera2));
} }
/* ************************************************************************* */ /* ************************************************************************* */