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

jacobians for logmap and expmap

release/4.3a0
Jing Dong 2016-10-03 19:11:44 -04:00
parent 557f08d72b
commit d7d35876f1
2 changed files with 43 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/base/ProductLieGroup.h
View File

@ -138,17 +138,27 @@ public:
return ProductLieGroup(g,h);
}
static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
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>());
Jacobian1 D_g_first; Jacobian2 D_h_second;
G g = traits<G>::Expmap(v.template head<dimension1>(), Hv ? &D_g_first : 0);
H h = traits<H>::Expmap(v.template tail<dimension2>(), Hv ? &D_h_second : 0);
if (Hv) {
Hv->setZero();
Hv->template topLeftCorner<dimension1,dimension1>() = D_g_first;
Hv->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
}
return ProductLieGroup(g,h);
}
static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) {
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);
Jacobian1 D_g_first; Jacobian2 D_h_second;
typename traits<G>::TangentVector v1 = traits<G>::Logmap(p.first, Hp ? &D_g_first : 0);
typename traits<H>::TangentVector v2 = traits<H>::Logmap(p.second, Hp ? &D_h_second : 0);
TangentVector v;
v << v1, v2;
if (Hp) {
Hp->setZero();
Hp->template topLeftCorner<dimension1,dimension1>() = D_g_first;
Hp->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
}
return v;
}
ProductLieGroup expmap(const TangentVector& v) const {

27
tests/testLie.cpp
View File

@ -102,6 +102,33 @@ TEST( testProduct, inverse ) {
EXPECT(assert_equal(numericH1, actH1, tol));
}
/* ************************************************************************* */
Product expmap_proxy(const Vector5& vec) {
return Product::Expmap(vec);
}
TEST( testProduct, Expmap ) {
Vector5 vec;
vec << 1, 2, 0.1, 0.2, 0.3;
Matrix actH;
Product::Expmap(vec, actH);
Matrix numericH = numericalDerivative11(expmap_proxy, vec);
EXPECT(assert_equal(numericH, actH, tol));
}
/* ************************************************************************* */
Vector5 logmap_proxy(const Product& p) {
return Product::Logmap(p);
}
TEST( testProduct, Logmap ) {
Product state(Point2(1, 2), Pose2(3, 4, 5));
Matrix actH;
Product::Logmap(state, actH);
Matrix numericH = numericalDerivative11(logmap_proxy, state);
EXPECT(assert_equal(numericH, actH, tol));
}
//******************************************************************************
int main() {
TestResult tr;