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

more progress, need to fix testSO3.cpp and testManifold.cpp

release/4.3a0
Mike Bosse 2014-12-17 22:53:56 +01:00
parent 121ae9f575
commit 263d4e163c
11 changed files with 114 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/base/LieVector.h
View File

@ -39,6 +39,9 @@ struct LieVector : public Vector {
/** initialize from a normal vector */ /** initialize from a normal vector */
LieVector(const Vector& v) : Vector(v) {} LieVector(const Vector& v) : Vector(v) {}
template <class V>
LieVector(const V& v) : Vector(v) {}
// Currently TMP constructor causes ICE on MSVS 2013 // Currently TMP constructor causes ICE on MSVS 2013
#if (_MSC_VER < 1800) #if (_MSC_VER < 1800)
/** initialize from a fixed size normal vector */ /** initialize from a fixed size normal vector */

3
gtsam/geometry/Rot2.h
View File

@ -111,7 +111,8 @@ namespace gtsam {
inline static Rot2 identity() { return Rot2(); } inline static Rot2 identity() { return Rot2(); }
/** The inverse rotation - negative angle */ /** The inverse rotation - negative angle */
Rot2 inverse() const { Rot2 inverse(OptionalJacobian<1,1> H = boost::none) const {
if (H) *H = -I_1x1;
return Rot2(c_, -s_); return Rot2(c_, -s_);
} }

2
gtsam/geometry/Rot3.h
View File

@ -213,7 +213,7 @@ namespace gtsam {
} }
/// derivative of inverse rotation R^T s.t. inverse(R)*R = identity /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
Rot3 inverse(boost::optional<Matrix3&> H1=boost::none) const; Rot3 inverse(OptionalJacobian<3,3> H1=boost::none) const;
/// Compose two rotations i.e., R= (*this) * R2 /// Compose two rotations i.e., R= (*this) * R2
Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none, Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none,

2
gtsam/geometry/Rot3M.cpp
View File

@ -158,7 +158,7 @@ Matrix3 Rot3::transpose() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix3 &> H1) const { Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const {
if (H1) *H1 = -rot_; if (H1) *H1 = -rot_;
return Rot3(Matrix3(transpose())); return Rot3(Matrix3(transpose()));
} }

4
gtsam/geometry/SO3.h
View File

@ -35,6 +35,7 @@ class SO3: public Matrix3 {
protected: protected:
public: public:
enum { dimension=3 };
/// Constructor from Eigen Matrix /// Constructor from Eigen Matrix
template<typename Derived> template<typename Derived>
@ -48,6 +49,9 @@ public:
} }
}; };
template<>
struct traits_x<SO3> : public internal::LieGroup<SO3, multiplicative_group_tag> {};
} // end namespace gtsam } // end namespace gtsam

10
gtsam/geometry/tests/testPose3.cpp
View File

@ -683,14 +683,14 @@ Vector testDerivExpmapInv(const Vector6& dxi) {
TEST( Pose3, dExpInv_TLN) { TEST( Pose3, dExpInv_TLN) {
Matrix res = Pose3::dExpInv_exp(xi); Matrix res = Pose3::dExpInv_exp(xi);
Matrix numericalDerivExpmapInv = numericalDerivative11<Vector, Vector6>( Matrix numericalDerivExpmapInv = numericalDerivative11<Vector6, Vector6>(
testDerivExpmapInv, Vector6::Zero(), 1e-5); testDerivExpmapInv, Vector6::Zero(), 1e-5);
EXPECT(assert_equal(numericalDerivExpmapInv,res,3e-1)); EXPECT(assert_equal(numericalDerivExpmapInv,res,3e-1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector testDerivAdjoint(const Vector6& xi, const Vector6& v) { Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) {
return Pose3::adjointMap(xi) * v; return Pose3::adjointMap(xi) * v;
} }
@ -700,7 +700,7 @@ TEST( Pose3, adjoint) {
Matrix actualH; Matrix actualH;
Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH); Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH);
Matrix numericalH = numericalDerivative21<Vector, Vector6, Vector6>( Matrix numericalH = numericalDerivative21<Vector6, Vector6, Vector6>(
testDerivAdjoint, screw::xi, screw::xi, 1e-5); testDerivAdjoint, screw::xi, screw::xi, 1e-5);
EXPECT(assert_equal(expected,actual,1e-5)); EXPECT(assert_equal(expected,actual,1e-5));
@ -708,7 +708,7 @@ TEST( Pose3, adjoint) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) { Vector6 testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) {
return Pose3::adjointMap(xi).transpose() * v; return Pose3::adjointMap(xi).transpose() * v;
} }
@ -720,7 +720,7 @@ TEST( Pose3, adjointTranspose) {
Matrix actualH; Matrix actualH;
Vector actual = Pose3::adjointTranspose(xi, v, actualH); Vector actual = Pose3::adjointTranspose(xi, v, actualH);
Matrix numericalH = numericalDerivative21<Vector, Vector6, Vector6>( Matrix numericalH = numericalDerivative21<Vector6, Vector6, Vector6>(
testDerivAdjointTranspose, xi, v, 1e-5); testDerivAdjointTranspose, xi, v, 1e-5);
EXPECT(assert_equal(expected,actual,1e-15)); EXPECT(assert_equal(expected,actual,1e-15));

11
gtsam/geometry/tests/testRot3.cpp
View File

@ -22,7 +22,7 @@
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h> #include <gtsam/base/lieProxies.h>
#include <gtsam/base/chartTesting.h> //#include <gtsam/base/chartTesting.h>
#include <boost/math/constants/constants.hpp> #include <boost/math/constants/constants.hpp>
@ -43,7 +43,6 @@ TEST( Rot3, chart)
{ {
Matrix R = (Matrix(3, 3) << 0, 1, 0, 1, 0, 0, 0, 0, -1).finished(); Matrix R = (Matrix(3, 3) << 0, 1, 0, 1, 0, 0, 0, 0, -1).finished();
Rot3 rot3(R); Rot3 rot3(R);
CHECK_CHART_CONCEPT(rot3);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -148,10 +147,10 @@ TEST( Rot3, retract)
Vector v = zero(3); Vector v = zero(3);
CHECK(assert_equal(R, R.retract(v))); CHECK(assert_equal(R, R.retract(v)));
// test Canonical coordinates // // test Canonical coordinates
Canonical<Rot3> chart; // Canonical<Rot3> chart;
Vector v2 = chart.local(R); // Vector v2 = chart.local(R);
CHECK(assert_equal(R, chart.retract(v2))); // CHECK(assert_equal(R, chart.retract(v2)));
} }
/* ************************************************************************* */ /* ************************************************************************* */

12
gtsam/geometry/tests/testSO3.cpp
View File

@ -23,6 +23,7 @@ using namespace gtsam;
typedef OptionalJacobian<3,3> SO3Jacobian; typedef OptionalJacobian<3,3> SO3Jacobian;
#if 0
//****************************************************************************** //******************************************************************************
TEST(SO3 , Concept) { TEST(SO3 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<SO3 >)); BOOST_CONCEPT_ASSERT((IsGroup<SO3 >));
@ -47,10 +48,9 @@ TEST(SO3 , Local) {
Vector3 z_axis(0, 0, 1); Vector3 z_axis(0, 0, 1);
SO3 q1(Eigen::AngleAxisd(0, z_axis)); SO3 q1(Eigen::AngleAxisd(0, z_axis));
SO3 q2(Eigen::AngleAxisd(0.1, z_axis)); SO3 q2(Eigen::AngleAxisd(0.1, z_axis));
typedef manifold::traits::DefaultChart<SO3>::type Chart;
SO3Jacobian H1,H2; SO3Jacobian H1,H2;
Vector3 expected(0, 0, 0.1); Vector3 expected(0, 0, 0.1);
Vector3 actual = Chart::Local(q1, q2, H1, H2); Vector3 actual = traits_x<SO3>::Local(q1, q2, H1, H2);
EXPECT(assert_equal((Vector)expected,actual)); EXPECT(assert_equal((Vector)expected,actual));
} }
@ -59,22 +59,26 @@ TEST(SO3 , Retract) {
Vector3 z_axis(0, 0, 1); Vector3 z_axis(0, 0, 1);
SO3 q(Eigen::AngleAxisd(0, z_axis)); SO3 q(Eigen::AngleAxisd(0, z_axis));
SO3 expected(Eigen::AngleAxisd(0.1, z_axis)); SO3 expected(Eigen::AngleAxisd(0.1, z_axis));
typedef manifold::traits::DefaultChart<SO3>::type Chart;
Vector3 v(0, 0, 0.1); Vector3 v(0, 0, 0.1);
SO3 actual = Chart::Retract(q, v); SO3 actual = traits_x<SO3>::Retract(q, v);
EXPECT(actual.isApprox(expected)); EXPECT(actual.isApprox(expected));
} }
#endif
//****************************************************************************** //******************************************************************************
TEST(SO3 , Compose) { TEST(SO3 , Compose) {
EXPECT(false);
} }
//****************************************************************************** //******************************************************************************
TEST(SO3 , Between) { TEST(SO3 , Between) {
EXPECT(false);
} }
//****************************************************************************** //******************************************************************************
TEST(SO3 , Inverse) { TEST(SO3 , Inverse) {
EXPECT(false);
} }
//****************************************************************************** //******************************************************************************

9
gtsam/geometry/tests/testStereoPoint2.cpp
View File

@ -26,9 +26,14 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2) GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2)
GTSAM_CONCEPT_LIE_INST(StereoPoint2) //GTSAM_CONCEPT_LIE_INST(StereoPoint2)
//******************************************************************************
TEST(StereoPoint2 , Concept) {
BOOST_CONCEPT_ASSERT((IsManifold<StereoPoint2 >));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(StereoPoint2, constructor) { TEST(StereoPoint2, constructor) {
StereoPoint2 p1(1, 2, 3), p2 = p1; StereoPoint2 p1(1, 2, 3), p2 = p1;
@ -49,7 +54,7 @@ TEST(StereoPoint2, Lie) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( StereoPoint2, expmap) { TEST( StereoPoint2, retract) {
Vector d(3); Vector d(3);
d(0) = 1; d(0) = 1;
d(1) = -1; d(1) = -1;

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

@ -115,13 +115,13 @@ TEST(Unit3, error) {
Matrix actual, expected; Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
{ {
expected = numericalDerivative11<Vector,Unit3>( expected = numericalDerivative11<Vector2,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), q); boost::bind(&Unit3::error, &p, _1, boost::none), q);
p.error(q, actual); p.error(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
} }
{ {
expected = numericalDerivative11<Vector,Unit3>( expected = numericalDerivative11<Vector2,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), r); boost::bind(&Unit3::error, &p, _1, boost::none), r);
p.error(r, actual); p.error(r, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); EXPECT(assert_equal(expected.transpose(), actual, 1e-9));

150
tests/testManifold.cpp
View File

@ -40,165 +40,167 @@ typedef PinholeCamera<Cal3Bundler> Camera;
/* ************************************************************************* */ /* ************************************************************************* */
// is_manifold // is_manifold
TEST(Manifold, _is_manifold) { TEST(Manifold, IsManifold) {
using namespace traits;
EXPECT(!is_manifold<int>::value); //BOOST_CONCEPT_ASSERT((IsManifold<int>)); // integer is not a manifold
EXPECT(is_manifold<Point2>::value); BOOST_CONCEPT_ASSERT((IsManifold<Point2>));
EXPECT(is_manifold<Matrix24>::value); BOOST_CONCEPT_ASSERT((IsManifold<Matrix24>));
EXPECT(is_manifold<double>::value); BOOST_CONCEPT_ASSERT((IsManifold<double>));
EXPECT(is_manifold<Vector>::value); // dynamic not working yet
EXPECT(is_manifold<Matrix>::value); //BOOST_CONCEPT_ASSERT((IsManifold<Vector>));
//BOOST_CONCEPT_ASSERT((IsManifold<Matrix>));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// dimension // dimension
TEST(Manifold, _dimension) { TEST(Manifold, _dimension) {
using namespace traits; //using namespace traits;
EXPECT_LONGS_EQUAL(2, dimension<Point2>::value); EXPECT_LONGS_EQUAL(2, traits_x<Point2>::dimension);
EXPECT_LONGS_EQUAL(8, dimension<Matrix24>::value); EXPECT_LONGS_EQUAL(8, traits_x<Matrix24>::dimension);
EXPECT_LONGS_EQUAL(1, dimension<double>::value); EXPECT_LONGS_EQUAL(1, traits_x<double>::dimension);
EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension<Vector>::value); EXPECT_LONGS_EQUAL(Eigen::Dynamic, traits_x<Vector>::dimension);
EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension<Matrix>::value); EXPECT_LONGS_EQUAL(Eigen::Dynamic, traits_x<Matrix>::dimension);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// charts // charts
TEST(Manifold, DefaultChart) { TEST(Manifold, DefaultChart) {
DefaultChart<Point2> chart1; //DefaultChart<Point2> chart1;
EXPECT(chart1.local(Point2(0, 0), Point2(1, 0)) == Vector2(1, 0)); EXPECT(traits_x<Point2>::Local(Point2(0, 0), Point2(1, 0)) == Vector2(1, 0));
EXPECT(chart1.retract(Point2(0, 0), Vector2(1, 0)) == Point2(1, 0)); EXPECT(traits_x<Point2>::Retract(Point2(0, 0), Vector2(1, 0)) == Point2(1, 0));
Vector v2(2); Vector v2(2);
v2 << 1, 0; v2 << 1, 0;
DefaultChart<Vector2> chart2; //DefaultChart<Vector2> chart2;
EXPECT(assert_equal(v2, chart2.local(Vector2(0, 0), Vector2(1, 0)))); EXPECT(assert_equal(v2, traits_x<Vector2>::Local(Vector2(0, 0), Vector2(1, 0))));
EXPECT(chart2.retract(Vector2(0, 0), v2) == Vector2(1, 0)); EXPECT(traits_x<Vector2>::Retract(Vector2(0, 0), v2) == Vector2(1, 0));
{ {
typedef Matrix2 ManifoldPoint; typedef Matrix2 ManifoldPoint;
ManifoldPoint m; ManifoldPoint m;
DefaultChart<ManifoldPoint> chart; //DefaultChart<ManifoldPoint> chart;
m << 1, 3, m << 1, 3,
2, 4; 2, 4;
// m as per default is in column-major storage mode. So this yields a linear representation of (1, 2, 3, 4)! // m as per default is in column-major storage mode. So this yields a linear representation of (1, 2, 3, 4)!
EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(chart.local(ManifoldPoint::Zero(), m)))); EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(traits_x<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
EXPECT(chart.retract(m, Vector4(1, 2, 3, 4)) == 2 * m); EXPECT(traits_x<ManifoldPoint>::Retract(m, Vector4(1, 2, 3, 4)) == 2 * m);
} }
{ {
typedef Eigen::Matrix<double, 1, 2> ManifoldPoint; typedef Eigen::Matrix<double, 1, 2> ManifoldPoint;
ManifoldPoint m; ManifoldPoint m;
DefaultChart<ManifoldPoint> chart; //DefaultChart<ManifoldPoint> chart;
m << 1, 2; m << 1, 2;
EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(chart.local(ManifoldPoint::Zero(), m)))); EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(traits_x<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
EXPECT(chart.retract(m, Vector2(1, 2)) == 2 * m); EXPECT(traits_x<ManifoldPoint>::Retract(m, Vector2(1, 2)) == 2 * m);
} }
{ {
typedef Eigen::Matrix<double, 1, 1> ManifoldPoint; typedef Eigen::Matrix<double, 1, 1> ManifoldPoint;
ManifoldPoint m; ManifoldPoint m;
DefaultChart<ManifoldPoint> chart; //DefaultChart<ManifoldPoint> chart;
m << 1; m << 1;
EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(chart.local(ManifoldPoint::Zero(), m)))); EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(traits_x<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
EXPECT(chart.retract(m, ManifoldPoint::Ones()) == 2 * m); EXPECT(traits_x<ManifoldPoint>::Retract(m, ManifoldPoint::Ones()) == 2 * m);
} }
DefaultChart<double> chart3; //DefaultChart<double> chart3;
Vector v1(1); Vector v1(1);
v1 << 1; v1 << 1;
EXPECT(assert_equal(v1, chart3.local(0, 1))); EXPECT(assert_equal(v1, traits_x<double>::Local(0, 1)));
EXPECT_DOUBLES_EQUAL(chart3.retract(0, v1), 1, 1e-9); EXPECT_DOUBLES_EQUAL(traits_x<double>::Retract(0, v1), 1, 1e-9);
// Dynamic does not work yet ! // Dynamic does not work yet !
Vector z = zero(2), v(2); Vector z = zero(2), v(2);
v << 1, 0; v << 1, 0;
DefaultChart<Vector> chart4; //DefaultChart<Vector> chart4;
EXPECT(assert_equal(chart4.local(z, v), v)); EXPECT(assert_equal(traits_x<Vector>::Local(z, v), v));
EXPECT(assert_equal(chart4.retract(z, v), v)); EXPECT(assert_equal(traits_x<Vector>::Retract(z, v), v));
Vector v3(3); Vector v3(3);
v3 << 1, 1, 1; v3 << 1, 1, 1;
Rot3 I = Rot3::identity(); Rot3 I = Rot3::identity();
Rot3 R = I.retract(v3); Rot3 R = I.retract(v3);
DefaultChart<Rot3> chart5; //DefaultChart<Rot3> chart5;
EXPECT(assert_equal(v3, chart5.local(I, R))); EXPECT(assert_equal(v3, traits_x<Rot3>::Local(I, R)));
EXPECT(assert_equal(chart5.retract(I, v3), R)); EXPECT(assert_equal(traits_x<Rot3>::Retract(I, v3), R));
// Check zero vector // Check zero vector
DefaultChart<Rot3> chart6; //DefaultChart<Rot3> chart6;
EXPECT(assert_equal(zero(3), chart6.local(R, R))); EXPECT(assert_equal(zero(3), traits_x<Rot3>::Local(R, R)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// zero // zero
TEST(Manifold, _zero) { //TEST(Manifold, _zero) {
EXPECT(assert_equal(Pose3(), traits::zero<Pose3>::value())); // EXPECT(assert_equal(Pose3(), traits::zero<Pose3>::value()));
Cal3Bundler cal(0, 0, 0); // Cal3Bundler cal(0, 0, 0);
EXPECT(assert_equal(cal, traits::zero<Cal3Bundler>::value())); // EXPECT(assert_equal(cal, traits::zero<Cal3Bundler>::value()));
EXPECT(assert_equal(Camera(Pose3(), cal), traits::zero<Camera>::value())); // EXPECT(assert_equal(Camera(Pose3(), cal), traits::zero<Camera>::value()));
EXPECT(assert_equal(Point2(), traits::zero<Point2>::value())); // EXPECT(assert_equal(Point2(), traits::zero<Point2>::value()));
EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::zero<Matrix24>::value()))); // EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::zero<Matrix24>::value())));
EXPECT_DOUBLES_EQUAL(0.0, traits::zero<double>::value(), 0.0); // EXPECT_DOUBLES_EQUAL(0.0, traits::zero<double>::value(), 0.0);
} //}
//
/* ************************************************************************* */ ///* ************************************************************************* */
// identity //// identity
TEST(Manifold, _identity) { //TEST(Manifold, _identity) {
EXPECT(assert_equal(Pose3(), traits::identity<Pose3>::value())); // EXPECT(assert_equal(Pose3(), traits::identity<Pose3>::value()));
EXPECT(assert_equal(Cal3Bundler(), traits::identity<Cal3Bundler>::value())); // EXPECT(assert_equal(Cal3Bundler(), traits::identity<Cal3Bundler>::value()));
EXPECT(assert_equal(Camera(), traits::identity<Camera>::value())); // EXPECT(assert_equal(Camera(), traits::identity<Camera>::value()));
EXPECT(assert_equal(Point2(), traits::identity<Point2>::value())); // EXPECT(assert_equal(Point2(), traits::identity<Point2>::value()));
EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::identity<Matrix24>::value()))); // EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::identity<Matrix24>::value())));
EXPECT_DOUBLES_EQUAL(0.0, traits::identity<double>::value(), 0.0); // EXPECT_DOUBLES_EQUAL(0.0, traits::identity<double>::value(), 0.0);
} //}
/* ************************************************************************* */ /* ************************************************************************* */
// charts // charts
TEST(Manifold, Canonical) { TEST(Manifold, Canonical) {
Canonical<Point2> chart1; Canonical<Point2> chart1;
EXPECT(chart1.local(Point2(1, 0))==Vector2(1, 0)); EXPECT(chart1.Local(Point2(1, 0))==Vector2(1, 0));
EXPECT(chart1.retract(Vector2(1, 0))==Point2(1, 0)); EXPECT(chart1.Retract(Vector2(1, 0))==Point2(1, 0));
Vector v2(2); Vector v2(2);
v2 << 1, 0; v2 << 1, 0;
Canonical<Vector2> chart2; Canonical<Vector2> chart2;
EXPECT(assert_equal(v2, chart2.local(Vector2(1, 0)))); EXPECT(assert_equal(v2, chart2.Local(Vector2(1, 0))));
EXPECT(chart2.retract(v2)==Vector2(1, 0)); EXPECT(chart2.Retract(v2)==Vector2(1, 0));
Canonical<double> chart3; Canonical<double> chart3;
Eigen::Matrix<double, 1, 1> v1; Eigen::Matrix<double, 1, 1> v1;
v1 << 1; v1 << 1;
EXPECT(chart3.local(1)==v1); EXPECT(chart3.Local(1)==v1);
EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9); EXPECT_DOUBLES_EQUAL(chart3.Retract(v1), 1, 1e-9);
Canonical<Point3> chart4; Canonical<Point3> chart4;
Point3 point(1, 2, 3); Point3 point(1, 2, 3);
Vector v3(3); Vector v3(3);
v3 << 1, 2, 3; v3 << 1, 2, 3;
EXPECT(assert_equal(v3, chart4.local(point))); EXPECT(assert_equal(v3, chart4.Local(point)));
EXPECT(assert_equal(chart4.retract(v3), point)); EXPECT(assert_equal(chart4.Retract(v3), point));
Canonical<Pose3> chart5; Canonical<Pose3> chart5;
Pose3 pose(Rot3::identity(), point); Pose3 pose(Rot3::identity(), point);
Vector v6(6); Vector v6(6);
v6 << 0, 0, 0, 1, 2, 3; v6 << 0, 0, 0, 1, 2, 3;
EXPECT(assert_equal(v6, chart5.local(pose))); EXPECT(assert_equal(v6, chart5.Local(pose)));
EXPECT(assert_equal(chart5.retract(v6), pose)); EXPECT(assert_equal(chart5.Retract(v6), pose));
Canonical<Camera> chart6; Canonical<Camera> chart6;
Cal3Bundler cal0(0, 0, 0); Cal3Bundler cal0(0, 0, 0);
Camera camera(Pose3(), cal0); Camera camera(Pose3(), cal0);
Vector z9 = Vector9::Zero(); Vector z9 = Vector9::Zero();
EXPECT(assert_equal(z9, chart6.local(camera))); EXPECT(assert_equal(z9, chart6.Local(camera)));
EXPECT(assert_equal(chart6.retract(z9), camera)); EXPECT(assert_equal(chart6.Retract(z9), camera));
Cal3Bundler cal; // Note !! Cal3Bundler() != zero<Cal3Bundler>::value() Cal3Bundler cal; // Note !! Cal3Bundler() != zero<Cal3Bundler>::value()
Camera camera2(pose, cal); Camera camera2(pose, cal);
Vector v9(9); Vector v9(9);
v9 << 0, 0, 0, 1, 2, 3, 1, 0, 0; v9 << 0, 0, 0, 1, 2, 3, 1, 0, 0;
EXPECT(assert_equal(v9, chart6.local(camera2))); EXPECT(assert_equal(v9, chart6.Local(camera2)));
EXPECT(assert_equal(chart6.retract(v9), camera2)); EXPECT(assert_equal(chart6.Retract(v9), camera2));
} }
/* ************************************************************************* */ /* ************************************************************************* */