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

Merged in feature/ProductLieGroupJacobians (pull request #285) 

Minor fixes of gtsam::traits

Approved-by: Chris Beall
release/4.3a0
Jing Dong 2017-03-07 23:16:52 +00:00 committed by Chris Beall
parent 687ae3d251 c0c55c4a21
commit 3095fb7e1f
3 changed files with 45 additions and 8 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 {

4
gtsam/base/VectorSpace.h
View File

@ -138,14 +138,14 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
}
static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1,
ChartJacobian H2) {
ChartJacobian H2 = boost::none) {
if (H1) *H1 = Eye(v1);
if (H2) *H2 = Eye(v2);
return v1 + v2;
}
static Class Between(const Class& v1, const Class& v2, ChartJacobian H1,
ChartJacobian H2) {
ChartJacobian H2 = boost::none) {
if (H1) *H1 = - Eye(v1);
if (H2) *H2 = Eye(v2);
return v2 - v1;

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;