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

bit more progress

release/4.3a0
Mike Bosse 2014-12-18 22:43:16 +01:00
parent 263d4e163c
commit 99d2203617
3 changed files with 21 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/base/concepts.h
View File

@ -389,7 +389,7 @@ struct traits_x< Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
// For Testable // For Testable
static void Print(const ManifoldType& m, const std::string& str = "") { static void Print(const ManifoldType& m, const std::string& str = "") {
gtsam::print(m, str); gtsam::print(Eigen::MatrixXd(m), str);
} }
static bool Equals(const ManifoldType& m1, static bool Equals(const ManifoldType& m1,
const ManifoldType& m2, const ManifoldType& m2,

3
gtsam/geometry/PinholeCamera.h
View File

@ -190,7 +190,8 @@ public:
/// return canonical coordinate /// return canonical coordinate
VectorK6 localCoordinates(const PinholeCamera& T2) const { VectorK6 localCoordinates(const PinholeCamera& T2) const {
VectorK6 d; // TODO: why does d.head<6>() not compile?? VectorK6 d;
// TODO: why does d.head<6>() not compile??
d.head(6) = pose().localCoordinates(T2.pose()); d.head(6) = pose().localCoordinates(T2.pose());
d.tail(DimK) = calibration().localCoordinates(T2.calibration()); d.tail(DimK) = calibration().localCoordinates(T2.calibration());
return d; return d;

32
tests/testManifold.cpp
View File

@ -46,6 +46,8 @@ TEST(Manifold, IsManifold) {
BOOST_CONCEPT_ASSERT((IsManifold<Point2>)); BOOST_CONCEPT_ASSERT((IsManifold<Point2>));
BOOST_CONCEPT_ASSERT((IsManifold<Matrix24>)); BOOST_CONCEPT_ASSERT((IsManifold<Matrix24>));
BOOST_CONCEPT_ASSERT((IsManifold<double>)); BOOST_CONCEPT_ASSERT((IsManifold<double>));
BOOST_CONCEPT_ASSERT((IsManifold<Camera>));
// dynamic not working yet // dynamic not working yet
//BOOST_CONCEPT_ASSERT((IsManifold<Vector>)); //BOOST_CONCEPT_ASSERT((IsManifold<Vector>));
//BOOST_CONCEPT_ASSERT((IsManifold<Matrix>)); //BOOST_CONCEPT_ASSERT((IsManifold<Matrix>));
@ -59,8 +61,8 @@ TEST(Manifold, _dimension) {
EXPECT_LONGS_EQUAL(2, traits_x<Point2>::dimension); EXPECT_LONGS_EQUAL(2, traits_x<Point2>::dimension);
EXPECT_LONGS_EQUAL(8, traits_x<Matrix24>::dimension); EXPECT_LONGS_EQUAL(8, traits_x<Matrix24>::dimension);
EXPECT_LONGS_EQUAL(1, traits_x<double>::dimension); EXPECT_LONGS_EQUAL(1, traits_x<double>::dimension);
EXPECT_LONGS_EQUAL(Eigen::Dynamic, traits_x<Vector>::dimension); // EXPECT_LONGS_EQUAL(Eigen::Dynamic, traits_x<Vector>::dimension);
EXPECT_LONGS_EQUAL(Eigen::Dynamic, traits_x<Matrix>::dimension); // EXPECT_LONGS_EQUAL(Eigen::Dynamic, traits_x<Matrix>::dimension);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -116,8 +118,8 @@ TEST(Manifold, DefaultChart) {
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(traits_x<Vector>::Local(z, v), v)); // EXPECT(assert_equal(traits_x<Vector>::Local(z, v), v));
EXPECT(assert_equal(traits_x<Vector>::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;
@ -143,16 +145,18 @@ TEST(Manifold, DefaultChart) {
// 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_x<Pose3>::Identity()));
// EXPECT(assert_equal(Cal3Bundler(), traits::identity<Cal3Bundler>::value())); // BOOST_CONCEPT_ASSERT((IsLieGroup<Cal3Bundler>));
// EXPECT(assert_equal(Camera(), traits::identity<Camera>::value())); // EXPECT(assert_equal(Cal3Bundler(), traits_x<Cal3Bundler>::Identity()));
// EXPECT(assert_equal(Point2(), traits::identity<Point2>::value())); // BOOST_CONCEPT_ASSERT((IsLieGroup<Camera>));
// EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::identity<Matrix24>::value()))); // EXPECT(assert_equal(Camera(), traits_x<Camera>::Identity())); // not a lie group
// EXPECT_DOUBLES_EQUAL(0.0, traits::identity<double>::value(), 0.0); EXPECT(assert_equal(Point2(), traits_x<Point2>::Identity()));
//} EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits_x<Matrix24>::Identity())));
EXPECT_DOUBLES_EQUAL(0.0, traits_x<double>::Identity(), 0.0);
}
/* ************************************************************************* */ /* ************************************************************************* */
// charts // charts