bit more progress
parent
263d4e163c
commit
99d2203617
|
@ -389,7 +389,7 @@ struct traits_x< Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
|
|||
|
||||
// For Testable
|
||||
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,
|
||||
const ManifoldType& m2,
|
||||
|
|
|
@ -190,7 +190,8 @@ public:
|
|||
|
||||
/// return canonical coordinate
|
||||
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.tail(DimK) = calibration().localCoordinates(T2.calibration());
|
||||
return d;
|
||||
|
|
|
@ -46,6 +46,8 @@ TEST(Manifold, IsManifold) {
|
|||
BOOST_CONCEPT_ASSERT((IsManifold<Point2>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Matrix24>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<double>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<Camera>));
|
||||
|
||||
// dynamic not working yet
|
||||
//BOOST_CONCEPT_ASSERT((IsManifold<Vector>));
|
||||
//BOOST_CONCEPT_ASSERT((IsManifold<Matrix>));
|
||||
|
@ -59,8 +61,8 @@ TEST(Manifold, _dimension) {
|
|||
EXPECT_LONGS_EQUAL(2, traits_x<Point2>::dimension);
|
||||
EXPECT_LONGS_EQUAL(8, traits_x<Matrix24>::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<Matrix>::dimension);
|
||||
// EXPECT_LONGS_EQUAL(Eigen::Dynamic, traits_x<Vector>::dimension);
|
||||
// EXPECT_LONGS_EQUAL(Eigen::Dynamic, traits_x<Matrix>::dimension);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
@ -116,8 +118,8 @@ TEST(Manifold, DefaultChart) {
|
|||
Vector z = zero(2), v(2);
|
||||
v << 1, 0;
|
||||
//DefaultChart<Vector> chart4;
|
||||
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>::Local(z, v), v));
|
||||
// EXPECT(assert_equal(traits_x<Vector>::Retract(z, v), v));
|
||||
|
||||
Vector v3(3);
|
||||
v3 << 1, 1, 1;
|
||||
|
@ -143,16 +145,18 @@ TEST(Manifold, DefaultChart) {
|
|||
// EXPECT_DOUBLES_EQUAL(0.0, traits::zero<double>::value(), 0.0);
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//// identity
|
||||
//TEST(Manifold, _identity) {
|
||||
// EXPECT(assert_equal(Pose3(), traits::identity<Pose3>::value()));
|
||||
// EXPECT(assert_equal(Cal3Bundler(), traits::identity<Cal3Bundler>::value()));
|
||||
// EXPECT(assert_equal(Camera(), traits::identity<Camera>::value()));
|
||||
// EXPECT(assert_equal(Point2(), traits::identity<Point2>::value()));
|
||||
// EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::identity<Matrix24>::value())));
|
||||
// EXPECT_DOUBLES_EQUAL(0.0, traits::identity<double>::value(), 0.0);
|
||||
//}
|
||||
/* ************************************************************************* */
|
||||
// identity
|
||||
TEST(Manifold, _identity) {
|
||||
EXPECT(assert_equal(Pose3(), traits_x<Pose3>::Identity()));
|
||||
// BOOST_CONCEPT_ASSERT((IsLieGroup<Cal3Bundler>));
|
||||
// EXPECT(assert_equal(Cal3Bundler(), traits_x<Cal3Bundler>::Identity()));
|
||||
// BOOST_CONCEPT_ASSERT((IsLieGroup<Camera>));
|
||||
// EXPECT(assert_equal(Camera(), traits_x<Camera>::Identity())); // not a lie group
|
||||
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
|
||||
|
|
Loading…
Reference in New Issue