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

Fixed product retract/localCoordinates and corresponding tests

release/4.3a0
dellaert 2015-07-16 23:48:56 -07:00
parent 233fe13e60
commit d1271fd9d5
3 changed files with 30 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
gtsam/base/ProductLieGroup.h
View File

@ -74,17 +74,21 @@ public:
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
static ProductLieGroup Retract(const TangentVector& v) {
return ProductLieGroup::ChartAtOrigin::Retract(v);
ProductLieGroup retract(const TangentVector& v, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const {
if (H1||H2) throw std::runtime_error("ProductLieGroup::retract derivatives not implemented yet");
G g = traits<G>::Retract(this->first, v.template head<dimension1>());
H h = traits<H>::Retract(this->second, v.template tail<dimension2>());
return ProductLieGroup(g,h);
}
static TangentVector LocalCoordinates(const ProductLieGroup& g) {
return ProductLieGroup::ChartAtOrigin::Local(g);
}
ProductLieGroup retract(const TangentVector& v) const {
return compose(ProductLieGroup::ChartAtOrigin::Retract(v));
}
TangentVector localCoordinates(const ProductLieGroup& g) const {
return ProductLieGroup::ChartAtOrigin::Local(between(g));
TangentVector localCoordinates(const ProductLieGroup& g, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const {
if (H1||H2) throw std::runtime_error("ProductLieGroup::localCoordinates derivatives not implemented yet");
typename traits<G>::TangentVector v1 = traits<G>::Local(this->first, g.first);
typename traits<H>::TangentVector v2 = traits<H>::Local(this->second, g.second);
TangentVector v;
v << v1, v2;
return v;
}
/// @}
@ -147,51 +151,19 @@ public:
v << v1, v2;
return v;
}
struct ChartAtOrigin {
static TangentVector Local(const ProductLieGroup& m, ChartJacobian Hm = boost::none) {
return Logmap(m, Hm);
}
static ProductLieGroup Retract(const TangentVector& v, ChartJacobian Hv = boost::none) {
return Expmap(v, Hv);
}
};
ProductLieGroup expmap(const TangentVector& v) const {
return compose(ProductLieGroup::Expmap(v));
}
TangentVector logmap(const ProductLieGroup& g) const {
return ProductLieGroup::Logmap(between(g));
}
static ProductLieGroup Retract(const TangentVector& v, ChartJacobian H1) {
return ProductLieGroup::ChartAtOrigin::Retract(v,H1);
}
static TangentVector LocalCoordinates(const ProductLieGroup& g, ChartJacobian H1) {
return ProductLieGroup::ChartAtOrigin::Local(g,H1);
}
ProductLieGroup retract(const TangentVector& v, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
Jacobian D_g_v;
ProductLieGroup g = ProductLieGroup::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0);
ProductLieGroup h = compose(g,H1,H2);
if (H2) *H2 = (*H2) * D_g_v;
return h;
}
TangentVector localCoordinates(const ProductLieGroup& g, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
ProductLieGroup h = between(g,H1,H2);
Jacobian D_v_h;
TangentVector v = ProductLieGroup::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
if (H1) *H1 = D_v_h * (*H1);
if (H2) *H2 = D_v_h * (*H2);
return v;
}
/// @}
};
// Define any direct product group to be a model of the multiplicative Group concept
template<typename G, typename H>
struct traits<ProductLieGroup<G, H> > : internal::LieGroupTraits<
ProductLieGroup<G, H> > {
};
struct traits<ProductLieGroup<G, H> > : internal::LieGroupTraits<ProductLieGroup<G, H> > {};
} // namespace gtsam

22
gtsam_unstable/dynamics/tests/testPoseRTV.cpp
View File

@ -86,21 +86,23 @@ TEST( testPoseRTV, Lie ) {
Vector delta(9);
delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3;
Rot3 rot2 = rot.retract(repeat(3, 0.1));
Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4);
Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3);
PoseRTV state2(pt2, rot2, vel2);
EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta), 1e-1));
EXPECT(assert_equal(delta, state1.localCoordinates(state2), 1e-1));
EXPECT(assert_equal(delta, -state2.localCoordinates(state1), 1e-1));
Pose3 pose2 = Pose3(rot, pt).retract(delta.head<6>());
Velocity3 vel2 = vel + Velocity3(-0.1, -0.2, -0.3);
PoseRTV state2(pose2.translation(), pose2.rotation(), vel2);
EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta), tol));
EXPECT(assert_equal(delta, state1.localCoordinates(state2), tol));
// roundtrip from state2 to state3 and back
PoseRTV state3 = state2.retract(delta);
EXPECT(assert_equal(delta, state2.localCoordinates(state3), 1e-1));
EXPECT(assert_equal(delta, state2.localCoordinates(state3), tol));
// roundtrip from state3 to state4 and back, with expmap
// roundtrip from state3 to state4 and back, with expmap.
PoseRTV state4 = state3.expmap(delta);
EXPECT(assert_equal(delta, state3.logmap(state4), 1e-1));
EXPECT(assert_equal(delta, state3.logmap(state4), tol));
// For the expmap/logmap (not necessarily retract/local) -delta goes other way
EXPECT(assert_equal(state3, (PoseRTV)state4.expmap(-delta), tol));
EXPECT(assert_equal(delta, -state4.logmap(state3), tol));
}
/* ************************************************************************* */

4
tests/testLie.cpp
View File

@ -54,9 +54,9 @@ TEST(Lie, ProductLieGroup) {
Vector5 d;
d << 1, 2, 0.1, 0.2, 0.3;
Product expected(Point2(1, 2), Pose2::Expmap(Vector3(0.1, 0.2, 0.3)));
Product pair2 = pair1.retract(d);
Product pair2 = pair1.expmap(d);
EXPECT(assert_equal(expected, pair2, 1e-9));
EXPECT(assert_equal(d, pair1.localCoordinates(pair2), 1e-9));
EXPECT(assert_equal(d, pair1.logmap(pair2), 1e-9));
}
/* ************************************************************************* */