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

Working compose/between/inverse derivatives

release/4.3a0
dellaert 2015-05-26 01:08:27 -07:00
parent 69c9017b36
commit d385984f26
2 changed files with 134 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
gtsam/base/ProductLieGroup.h
View File

@ -23,11 +23,10 @@
namespace gtsam {
/// Template to construct the product Lie group of two other Lie groups, G and H
/// Assumes manifold structure from G and H, and binary constructor
/// Template to construct the product Lie group of two other Lie groups
/// Assumes Lie group structure for G and H
template<typename G, typename H>
class ProductLieGroup: public std::pair<G, H>, public LieGroup<
ProductLieGroup<G, H>, traits<G>::dimension + traits<H>::dimension> {
class ProductLieGroup: public std::pair<G, H> {
BOOST_CONCEPT_ASSERT((IsLieGroup<G>));
BOOST_CONCEPT_ASSERT((IsLieGroup<H>));
typedef std::pair<G, H> Base;
@ -58,9 +57,15 @@ public:
ProductLieGroup inverse() const {
return ProductLieGroup(this->first.inverse(), this->second.inverse());
}
ProductLieGroup compose(const ProductLieGroup& g) const {
return (*this) * g;
}
ProductLieGroup between(const ProductLieGroup& g) const {
return this->inverse() * g;
}
/// @}
/// @name Manifold (but with derivatives)
/// @name Manifold
/// @{
enum {dimension = dimension1 + dimension2};
inline static size_t Dim() {return dimension;}
@ -68,26 +73,74 @@ public:
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
static ProductLieGroup Retract(const TangentVector& v) {
return ProductLieGroup::ChartAtOrigin::Retract(v);
}
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));
}
/// @}
/// @name Lie Group
/// @{
Eigen::Matrix<double,dimension,dimension> AdjointMap() const {
Eigen::Matrix<double,dimension,dimension> A;
A.setIdentity();
throw std::runtime_error("ProductLieGroup::derivatives not implemented yet");
// A.template topLeftCorner<dimension1, dimension1>() = this->first.AdjointMap();
// A.template bottomRightCorner<dimension2, dimension2>() = this->second.AdjointMap();
return A;
protected:
typedef Eigen::Matrix<double, dimension, dimension> Jacobian;
typedef Eigen::Matrix<double, dimension1, dimension1> Jacobian1;
typedef Eigen::Matrix<double, dimension2, dimension2> Jacobian2;
public:
ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1,
ChartJacobian H2 = boost::none) const {
Jacobian1 D_g_first; Jacobian2 D_h_second;
G g = traits<G>::Compose(this->first,other.first, H1 ? &D_g_first : 0);
H h = traits<H>::Compose(this->second,other.second, H1 ? &D_h_second : 0);
if (H1) {
H1->setZero();
H1->template topLeftCorner<dimension1,dimension1>() = D_g_first;
H1->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
}
if (H2) *H2 = Jacobian::Identity();
return ProductLieGroup(g,h);
}
ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1,
ChartJacobian H2 = boost::none) const {
Jacobian1 D_g_first; Jacobian2 D_h_second;
G g = traits<G>::Between(this->first,other.first, H1 ? &D_g_first : 0);
H h = traits<H>::Between(this->second,other.second, H1 ? &D_h_second : 0);
if (H1) {
H1->setZero();
H1->template topLeftCorner<dimension1,dimension1>() = D_g_first;
H1->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
}
if (H2) *H2 = Jacobian::Identity();
return ProductLieGroup(g,h);
}
ProductLieGroup inverse(ChartJacobian D) const {
Jacobian1 D_g_first; Jacobian2 D_h_second;
G g = traits<G>::Inverse(this->first, D ? &D_g_first : 0);
H h = traits<H>::Inverse(this->second, D ? &D_h_second : 0);
if (D) {
D->setZero();
D->template topLeftCorner<dimension1,dimension1>() = D_g_first;
D->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
}
return ProductLieGroup(g,h);
}
static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
if (Hv) throw std::runtime_error("ProductLieGroup::derivatives not implemented yet");
if (Hv) throw std::runtime_error("ProductLieGroup::Expmap derivatives not implemented yet");
G g = traits<G>::Expmap(v.template head<dimension1>());
H h = traits<H>::Expmap(v.template tail<dimension2>());
return ProductLieGroup(g,h);
}
static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) {
if (Hp) throw std::runtime_error("ProductLieGroup::derivatives not implemented yet");
if (Hp) throw std::runtime_error("ProductLieGroup::Logmap derivatives not implemented yet");
typename traits<G>::TangentVector v1 = traits<G>::Logmap(p.first);
typename traits<H>::TangentVector v2 = traits<H>::Logmap(p.second);
TangentVector v;
@ -102,8 +155,37 @@ public:
return Expmap(v, Hv);
}
};
using LieGroup<ProductLieGroup,dimension>::inverse; // with derivative
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

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

@ -120,6 +120,43 @@ TEST( testPoseRTV, dynamics_identities ) {
}
/* ************************************************************************* */
PoseRTV compose_proxy(const PoseRTV& A, const PoseRTV& B) { return A.compose(B); }
TEST( testPoseRTV, compose ) {
PoseRTV state1(pt, rot, vel), state2 = state1;
Matrix actH1, actH2;
state1.compose(state2, actH1, actH2);
Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2);
Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2);
EXPECT(assert_equal(numericH1, actH1, tol));
EXPECT(assert_equal(numericH2, actH2, tol));
}
/* ************************************************************************* */
PoseRTV between_proxy(const PoseRTV& A, const PoseRTV& B) { return A.between(B); }
TEST( testPoseRTV, between ) {
PoseRTV state1(pt, rot, vel), state2 = state1;
Matrix actH1, actH2;
state1.between(state2, actH1, actH2);
Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2);
Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2);
EXPECT(assert_equal(numericH1, actH1, tol));
EXPECT(assert_equal(numericH2, actH2, tol));
}
/* ************************************************************************* */
PoseRTV inverse_proxy(const PoseRTV& A) { return A.inverse(); }
TEST( testPoseRTV, inverse ) {
PoseRTV state1(pt, rot, vel);
Matrix actH1;
state1.inverse(actH1);
Matrix numericH1 = numericalDerivative11(inverse_proxy, state1);
EXPECT(assert_equal(numericH1, actH1, tol));
}
/* ************************************************************************* */
double range_proxy(const PoseRTV& A, const PoseRTV& B) { return A.range(B); }
TEST( testPoseRTV, range ) {