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

formatting

release/4.3a0
acxz 2021-06-20 19:29:27 -04:00
parent 155fafabf2
commit 944b3aea29
5 changed files with 336 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

260
gtsam/base/numericalDerivative.h
View File

@ -150,7 +150,8 @@ typename internal::FixedSizeMatrix<Y, X>::type numericalDerivative11(
template<class Y, class X>
typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const X&), const X& x,
double delta = 1e-5) {
return numericalDerivative11<Y, X>(boost::bind(h, boost::placeholders::_1), x, delta);
return numericalDerivative11<Y, X>(boost::bind(h, boost::placeholders::_1), x,
delta);
}
/**
@ -169,14 +170,17 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const boost
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(boost::bind(h, boost::placeholders::_1, boost::cref(x2)), x1, delta);
return numericalDerivative11<Y, X1, N>(
boost::bind(h, boost::placeholders::_1, boost::cref(x2)), x1, delta);
}
/** use a raw C++ function pointer */
template<class Y, class X1, class X2>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1,
const X2& x2, double delta = 1e-5) {
return numericalDerivative21<Y, X1, X2>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2, delta);
return numericalDerivative21<Y, X1, X2>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2,
delta);
}
/**
@ -195,14 +199,17 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(boost::func
// "Template argument X1 must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), boost::placeholders::_1), x2, delta);
return numericalDerivative11<Y, X2, N>(
boost::bind(h, boost::cref(x1), boost::placeholders::_1), x2, delta);
}
/** use a raw C++ function pointer */
template<class Y, class X1, class X2>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1,
const X2& x2, double delta = 1e-5) {
return numericalDerivative22<Y, X1, X2>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2, delta);
return numericalDerivative22<Y, X1, X2>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2), x1, x2,
delta);
}
/**
@ -224,14 +231,18 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3)), x1, delta);
return numericalDerivative11<Y, X1, N>(
boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3)),
x1, delta);
}
template<class Y, class X1, class X2, class X3>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalDerivative31<Y, X1, X2, X3>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3), x1,
x2, x3, delta);
return numericalDerivative31<Y, X1, X2, X3>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3),
x1, x2, x3, delta);
}
/**
@ -253,14 +264,18 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3)), x2, delta);
return numericalDerivative11<Y, X2, N>(
boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3)),
x2, delta);
}
template<class Y, class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalDerivative32<Y, X1, X2, X3>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3), x1,
x2, x3, delta);
return numericalDerivative32<Y, X1, X2, X3>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3),
x1, x2, x3, delta);
}
/**
@ -282,14 +297,18 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
"Template argument X3 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1), x3, delta);
return numericalDerivative11<Y, X3, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1),
x3, delta);
}
template<class Y, class X1, class X2, class X3>
inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalDerivative33<Y, X1, X2, X3>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3), x1,
x2, x3, delta);
return numericalDerivative33<Y, X1, X2, X3>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3),
x1, x2, x3, delta);
}
/**
@ -311,13 +330,19 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta);
return numericalDerivative11<Y, X1, N>(
boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3),
boost::cref(x4)),
x1, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(Y (*h)(const X1&, const X2&, const X3&, const X4&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
return numericalDerivative41<Y, X1, X2, X3, X4>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4), x1, x2, x3, x4);
return numericalDerivative41<Y, X1, X2, X3, X4>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4),
x1, x2, x3, x4);
}
/**
@ -339,13 +364,19 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3), boost::cref(x4)), x2, delta);
return numericalDerivative11<Y, X2, N>(
boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3),
boost::cref(x4)),
x2, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(Y (*h)(const X1&, const X2&, const X3&, const X4&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
return numericalDerivative42<Y, X1, X2, X3, X4>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4), x1, x2, x3, x4);
return numericalDerivative42<Y, X1, X2, X3, X4>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4),
x1, x2, x3, x4);
}
/**
@ -367,13 +398,19 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
"Template argument X3 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1, boost::cref(x4)), x3, delta);
return numericalDerivative11<Y, X3, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1,
boost::cref(x4)),
x3, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(Y (*h)(const X1&, const X2&, const X3&, const X4&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
return numericalDerivative43<Y, X1, X2, X3, X4>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4), x1, x2, x3, x4);
return numericalDerivative43<Y, X1, X2, X3, X4>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4),
x1, x2, x3, x4);
}
/**
@ -395,13 +432,19 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X4>::structure_category>::value),
"Template argument X4 must be a manifold type.");
return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::placeholders::_1), x4, delta);
return numericalDerivative11<Y, X4, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
boost::placeholders::_1),
x4, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(Y (*h)(const X1&, const X2&, const X3&, const X4&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
return numericalDerivative44<Y, X1, X2, X3, X4>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4), x1, x2, x3, x4);
return numericalDerivative44<Y, X1, X2, X3, X4>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4),
x1, x2, x3, x4);
}
/**
@ -424,13 +467,20 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta);
return numericalDerivative11<Y, X1, N>(
boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3),
boost::cref(x4), boost::cref(x5)),
x1, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative51<Y, X1, X2, X3, X4, X5>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5);
return numericalDerivative51<Y, X1, X2, X3, X4, X5>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5),
x1, x2, x3, x4, x5);
}
/**
@ -453,13 +503,20 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta);
return numericalDerivative11<Y, X2, N>(
boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3),
boost::cref(x4), boost::cref(x5)),
x2, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative52<Y, X1, X2, X3, X4, X5>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5);
return numericalDerivative52<Y, X1, X2, X3, X4, X5>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5),
x1, x2, x3, x4, x5);
}
/**
@ -482,13 +539,20 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1, boost::cref(x4), boost::cref(x5)), x3, delta);
return numericalDerivative11<Y, X3, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1,
boost::cref(x4), boost::cref(x5)),
x3, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative53<Y, X1, X2, X3, X4, X5>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5);
return numericalDerivative53<Y, X1, X2, X3, X4, X5>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5),
x1, x2, x3, x4, x5);
}
/**
@ -511,13 +575,20 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::placeholders::_1, boost::cref(x5)), x4, delta);
return numericalDerivative11<Y, X4, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
boost::placeholders::_1, boost::cref(x5)),
x4, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative54<Y, X1, X2, X3, X4, X5>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5);
return numericalDerivative54<Y, X1, X2, X3, X4, X5>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5),
x1, x2, x3, x4, x5);
}
/**
@ -540,13 +611,20 @@ typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X5, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::placeholders::_1), x5, delta);
return numericalDerivative11<Y, X5, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
boost::cref(x4), boost::placeholders::_1),
x5, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
return numericalDerivative55<Y, X1, X2, X3, X4, X5>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5), x1, x2, x3, x4, x5);
return numericalDerivative55<Y, X1, X2, X3, X4, X5>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5),
x1, x2, x3, x4, x5);
}
/**
@ -570,13 +648,20 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1, N>(boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x1, delta);
return numericalDerivative11<Y, X1, N>(
boost::bind(h, boost::placeholders::_1, boost::cref(x2), boost::cref(x3),
boost::cref(x4), boost::cref(x5), boost::cref(x6)),
x1, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
return numericalDerivative61<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6);
return numericalDerivative61<Y, X1, X2, X3, X4, X5, X6>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5, boost::placeholders::_6),
x1, x2, x3, x4, x5, x6);
}
/**
@ -600,13 +685,20 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::cref(x6)), x2, delta);
return numericalDerivative11<Y, X2, N>(
boost::bind(h, boost::cref(x1), boost::placeholders::_1, boost::cref(x3),
boost::cref(x4), boost::cref(x5), boost::cref(x6)),
x2, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
return numericalDerivative62<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6);
return numericalDerivative62<Y, X1, X2, X3, X4, X5, X6>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5, boost::placeholders::_6),
x1, x2, x3, x4, x5, x6);
}
/**
@ -630,13 +722,20 @@ typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1, boost::cref(x4), boost::cref(x5), boost::cref(x6)), x3, delta);
return numericalDerivative11<Y, X3, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::placeholders::_1,
boost::cref(x4), boost::cref(x5), boost::cref(x6)),
x3, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
return numericalDerivative63<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6);
return numericalDerivative63<Y, X1, X2, X3, X4, X5, X6>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5, boost::placeholders::_6),
x1, x2, x3, x4, x5, x6);
}
/**
@ -660,13 +759,20 @@ typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::placeholders::_1, boost::cref(x5), boost::cref(x6)), x4, delta);
return numericalDerivative11<Y, X4, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
boost::placeholders::_1, boost::cref(x5), boost::cref(x6)),
x4, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
return numericalDerivative64<Y, X1, X2, X3, X4, X5>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6);
return numericalDerivative64<Y, X1, X2, X3, X4, X5>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5, boost::placeholders::_6),
x1, x2, x3, x4, x5, x6);
}
/**
@ -690,13 +796,20 @@ typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X5, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::placeholders::_1, boost::cref(x6)), x5, delta);
return numericalDerivative11<Y, X5, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
boost::cref(x4), boost::placeholders::_1, boost::cref(x6)),
x5, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
return numericalDerivative65<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6);
return numericalDerivative65<Y, X1, X2, X3, X4, X5, X6>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5, boost::placeholders::_6),
x1, x2, x3, x4, x5, x6);
}
/**
@ -721,13 +834,20 @@ typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X6, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), boost::placeholders::_1), x6, delta);
return numericalDerivative11<Y, X6, N>(
boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3),
boost::cref(x4), boost::cref(x5), boost::placeholders::_1),
x6, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
inline typename internal::FixedSizeMatrix<Y,X6>::type numericalDerivative66(Y (*h)(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&),
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
return numericalDerivative66<Y, X1, X2, X3, X4, X5, X6>(boost::bind(h, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5, boost::placeholders::_6), x1, x2, x3, x4, x5, x6);
return numericalDerivative66<Y, X1, X2, X3, X4, X5, X6>(
boost::bind(h, boost::placeholders::_1, boost::placeholders::_2,
boost::placeholders::_3, boost::placeholders::_4,
boost::placeholders::_5, boost::placeholders::_6),
x1, x2, x3, x4, x5, x6);
}
/**
@ -747,8 +867,8 @@ inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(boost::fun
typedef boost::function<double(const X&)> F;
typedef boost::function<VectorD(F, const X&, double)> G;
G ng = static_cast<G>(numericalGradient<X> );
return numericalDerivative11<VectorD, X>(boost::bind(ng, f, boost::placeholders::_1, delta), x,
delta);
return numericalDerivative11<VectorD, X>(
boost::bind(ng, f, boost::placeholders::_1, delta), x, delta);
}
template<class X>
@ -773,7 +893,8 @@ public:
f_(f), x1_(x1), delta_(delta) {
}
Vector operator()(const X2& x2) {
return numericalGradient<X1>(boost::bind(f_, boost::placeholders::_1, boost::cref(x2)), x1_, delta_);
return numericalGradient<X1>(
boost::bind(f_, boost::placeholders::_1, boost::cref(x2)), x1_, delta_);
}
};
@ -785,7 +906,8 @@ inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(
G_x1<X1, X2> g_x1(f, x1, delta);
return numericalDerivative11<Vector, X2>(
boost::function<Vector(const X2&)>(
boost::bind<Vector>(boost::ref(g_x1), boost::placeholders::_1)), x2, delta);
boost::bind<Vector>(boost::ref(g_x1), boost::placeholders::_1)),
x2, delta);
}
template<class X1, class X2>
@ -804,10 +926,12 @@ inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(
Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
double) = &numericalGradient<X1>;
boost::function<double(const X1&)> f2(boost::bind(f, boost::placeholders::_1, boost::cref(x2)));
boost::function<double(const X1&)> f2(
boost::bind(f, boost::placeholders::_1, boost::cref(x2)));
return numericalDerivative11<Vector, X1>(
boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
boost::function<Vector(const X1&)>(
boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
x1, delta);
}
@ -825,10 +949,12 @@ inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(
typedef typename internal::FixedSizeMatrix<X2>::type Vector;
Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
double) = &numericalGradient<X2>;
boost::function<double(const X2&)> f2(boost::bind(f, boost::cref(x1), boost::placeholders::_1));
boost::function<double(const X2&)> f2(
boost::bind(f, boost::cref(x1), boost::placeholders::_1));
return numericalDerivative11<Vector, X2>(
boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
boost::function<Vector(const X2&)>(
boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
x2, delta);
}
@ -850,10 +976,12 @@ inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(
typedef typename internal::FixedSizeMatrix<X1>::type Vector;
Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
double) = &numericalGradient<X1>;
boost::function<double(const X1&)> f2(boost::bind(f, boost::placeholders::_1, boost::cref(x2), boost::cref(x3)));
boost::function<double(const X1&)> f2(boost::bind(
f, boost::placeholders::_1, boost::cref(x2), boost::cref(x3)));
return numericalDerivative11<Vector, X1>(
boost::function<Vector(const X1&)>(boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
boost::function<Vector(const X1&)>(
boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
x1, delta);
}
@ -873,10 +1001,12 @@ inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(
typedef typename internal::FixedSizeMatrix<X2>::type Vector;
Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
double) = &numericalGradient<X2>;
boost::function<double(const X2&)> f2(boost::bind(f, boost::cref(x1), boost::placeholders::_1, boost::cref(x3)));
boost::function<double(const X2&)> f2(boost::bind(
f, boost::cref(x1), boost::placeholders::_1, boost::cref(x3)));
return numericalDerivative11<Vector, X2>(
boost::function<Vector(const X2&)>(boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
boost::function<Vector(const X2&)>(
boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
x2, delta);
}
@ -896,10 +1026,12 @@ inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(
typedef typename internal::FixedSizeMatrix<X3>::type Vector;
Vector (*numGrad)(boost::function<double(const X3&)>, const X3&,
double) = &numericalGradient<X3>;
boost::function<double(const X3&)> f2(boost::bind(f, boost::cref(x1), boost::cref(x2), boost::placeholders::_1));
boost::function<double(const X3&)> f2(boost::bind(
f, boost::cref(x1), boost::cref(x2), boost::placeholders::_1));
return numericalDerivative11<Vector, X3>(
boost::function<Vector(const X3&)>(boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
boost::function<Vector(const X3&)>(
boost::bind(numGrad, f2, boost::placeholders::_1, delta)),
x3, delta);
}
@ -917,7 +1049,9 @@ inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian212<X1, X2>(
boost::function<double(const X1&, const X2&)>(boost::bind(f, boost::placeholders::_1, boost::placeholders::_2, boost::cref(x3))),
boost::function<double(const X1&, const X2&)>(
boost::bind(f, boost::placeholders::_1, boost::placeholders::_2,
boost::cref(x3))),
x1, x2, delta);
}
@ -926,7 +1060,9 @@ inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian212<X1, X3>(
boost::function<double(const X1&, const X3&)>(boost::bind(f, boost::placeholders::_1, boost::cref(x2), boost::placeholders::_2)),
boost::function<double(const X1&, const X3&)>(
boost::bind(f, boost::placeholders::_1, boost::cref(x2),
boost::placeholders::_2)),
x1, x3, delta);
}
@ -935,7 +1071,9 @@ inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian212<X2, X3>(
boost::function<double(const X2&, const X3&)>(boost::bind(f, boost::cref(x1), boost::placeholders::_1, boost::placeholders::_2)),
boost::function<double(const X2&, const X3&)>(
boost::bind(f, boost::cref(x1), boost::placeholders::_1,
boost::placeholders::_2)),
x2, x3, delta);
}

91
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -308,38 +308,68 @@ public:
// TODO: Write analytical derivative calculations
// Jacobian w.r.t. Pose1
if (H1){
Matrix H1_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1);
Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1);
Matrix H1_Pose = numericalDerivative11<POSE, POSE>(
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
Pose1);
Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
Pose1);
*H1 = stack(2, &H1_Pose, &H1_Vel);
}
// Jacobian w.r.t. Vel1
if (H2){
if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
Matrix H2_Pose = numericalDerivative11<POSE, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), Vel1);
Matrix H2_Vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), Vel1);
Matrix H2_Pose = numericalDerivative11<POSE, Vector3>(
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2),
Vel1);
Matrix H2_Vel = numericalDerivative11<Vector3, Vector3>(
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2),
Vel1);
*H2 = stack(2, &H2_Pose, &H2_Vel);
}
// Jacobian w.r.t. IMUBias1
if (H3){
Matrix H3_Pose = numericalDerivative11<POSE, IMUBIAS>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), Bias1);
Matrix H3_Vel = numericalDerivative11<VELOCITY, IMUBIAS>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), Bias1);
Matrix H3_Pose = numericalDerivative11<POSE, IMUBIAS>(
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2),
Bias1);
Matrix H3_Vel = numericalDerivative11<VELOCITY, IMUBIAS>(
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2),
Bias1);
*H3 = stack(2, &H3_Pose, &H3_Vel);
}
// Jacobian w.r.t. Pose2
if (H4){
Matrix H4_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), Pose2);
Matrix H4_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), Pose2);
Matrix H4_Pose = numericalDerivative11<POSE, POSE>(
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2),
Pose2);
Matrix H4_Vel = numericalDerivative11<VELOCITY, POSE>(
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2),
Pose2);
*H4 = stack(2, &H4_Pose, &H4_Vel);
}
// Jacobian w.r.t. Vel2
if (H5){
if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
Matrix H5_Pose = numericalDerivative11<POSE, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), Vel2);
Matrix H5_Vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), Vel2);
Matrix H5_Pose = numericalDerivative11<POSE, Vector3>(
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError,
this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1),
Vel2);
Matrix H5_Vel = numericalDerivative11<Vector3, Vector3>(
boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError,
this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1),
Vel2);
*H5 = stack(2, &H5_Pose, &H5_Vel);
}
@ -438,18 +468,45 @@ public:
Matrix Z_3x3 = Z_3x3;
Matrix I_3x3 = I_3x3;
Matrix H_pos_pos = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, boost::placeholders::_1, delta_vel_in_t0), delta_pos_in_t0);
Matrix H_pos_vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, boost::placeholders::_1), delta_vel_in_t0);
Matrix H_pos_pos = numericalDerivative11<Vector3, Vector3>(
boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt,
boost::placeholders::_1, delta_vel_in_t0),
delta_pos_in_t0);
Matrix H_pos_vel = numericalDerivative11<Vector3, Vector3>(
boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt,
delta_pos_in_t0, boost::placeholders::_1),
delta_vel_in_t0);
Matrix H_pos_angles = Z_3x3;
Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3);
Matrix H_vel_vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, boost::placeholders::_1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0);
Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, boost::placeholders::_1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
Matrix H_vel_bias = numericalDerivative11<Vector3, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, boost::placeholders::_1), Bias_t0);
Matrix H_vel_vel = numericalDerivative11<Vector3, Vector3>(
boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t,
msr_acc_t, msr_dt, delta_angles, boost::placeholders::_1,
flag_use_body_P_sensor, body_P_sensor, Bias_t0),
delta_vel_in_t0);
Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(
boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t,
msr_acc_t, msr_dt, boost::placeholders::_1, delta_vel_in_t0,
flag_use_body_P_sensor, body_P_sensor, Bias_t0),
delta_angles);
Matrix H_vel_bias = numericalDerivative11<Vector3, IMUBIAS>(
boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t,
msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0,
flag_use_body_P_sensor, body_P_sensor,
boost::placeholders::_1),
Bias_t0);
Matrix H_vel_pos = Z_3x3;
Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, boost::placeholders::_1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
Matrix H_angles_bias = numericalDerivative11<Vector3, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, boost::placeholders::_1), Bias_t0);
Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(
boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t,
msr_dt, boost::placeholders::_1, flag_use_body_P_sensor,
body_P_sensor, Bias_t0),
delta_angles);
Matrix H_angles_bias = numericalDerivative11<Vector3, IMUBIAS>(
boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t,
msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor,
boost::placeholders::_1),
Bias_t0);
Matrix H_angles_pos = Z_3x3;
Matrix H_angles_vel = Z_3x3;

50
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -235,38 +235,68 @@ public:
// TODO: Write analytical derivative calculations
// Jacobian w.r.t. Pose1
if (H1){
Matrix H1_Pose = gtsam::numericalDerivative11<POSE, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1);
Matrix H1_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2), Pose1);
Matrix H1_Pose = gtsam::numericalDerivative11<POSE, POSE>(
boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
Pose1);
Matrix H1_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(
boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
this, boost::placeholders::_1, Vel1, Bias1, Pose2, Vel2),
Pose1);
*H1 = stack(2, &H1_Pose, &H1_Vel);
}
// Jacobian w.r.t. Vel1
if (H2){
if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
Matrix H2_Pose = gtsam::numericalDerivative11<POSE, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), Vel1);
Matrix H2_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2), Vel1);
Matrix H2_Pose = gtsam::numericalDerivative11<POSE, Vector3>(
boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2),
Vel1);
Matrix H2_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(
boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
this, Pose1, boost::placeholders::_1, Bias1, Pose2, Vel2),
Vel1);
*H2 = stack(2, &H2_Pose, &H2_Vel);
}
// Jacobian w.r.t. IMUBias1
if (H3){
Matrix H3_Pose = gtsam::numericalDerivative11<POSE, IMUBIAS>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), Bias1);
Matrix H3_Vel = gtsam::numericalDerivative11<VELOCITY, IMUBIAS>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2), Bias1);
Matrix H3_Pose = gtsam::numericalDerivative11<POSE, IMUBIAS>(
boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2),
Bias1);
Matrix H3_Vel = gtsam::numericalDerivative11<VELOCITY, IMUBIAS>(
boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
this, Pose1, Vel1, boost::placeholders::_1, Pose2, Vel2),
Bias1);
*H3 = stack(2, &H3_Pose, &H3_Vel);
}
// Jacobian w.r.t. Pose2
if (H4){
Matrix H4_Pose = gtsam::numericalDerivative11<POSE, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), Pose2);
Matrix H4_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2), Pose2);
Matrix H4_Pose = gtsam::numericalDerivative11<POSE, POSE>(
boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2),
Pose2);
Matrix H4_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(
boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
this, Pose1, Vel1, Bias1, boost::placeholders::_1, Vel2),
Pose2);
*H4 = stack(2, &H4_Pose, &H4_Vel);
}
// Jacobian w.r.t. Vel2
if (H5){
if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
Matrix H5_Pose = gtsam::numericalDerivative11<POSE, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), Vel2);
Matrix H5_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1), Vel2);
Matrix H5_Pose = gtsam::numericalDerivative11<POSE, Vector3>(
boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError,
this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1),
Vel2);
Matrix H5_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(
boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError,
this, Pose1, Vel1, Bias1, Pose2, boost::placeholders::_1),
Vel2);
*H5 = stack(2, &H5_Pose, &H5_Vel);
}

5
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

@ -108,8 +108,9 @@ public:
if (H1) {
(*H1) = numericalDerivative11<Vector, Pose3>(
boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, boost::placeholders::_1,
landmark), pose);
boost::bind(&InvDepthFactorVariant1::inverseDepthError, this,
boost::placeholders::_1, landmark),
pose);
}
if (H2) {
(*H2) = numericalDerivative11<Vector, Vector6>(

25
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -110,10 +110,16 @@ public:
boost::optional<Matrix&> H2=boost::none) const override {
if(H1) {
(*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, boost::placeholders::_1, landmark), pose);
(*H1) = numericalDerivative11<Vector, Pose3>(
boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this,
boost::placeholders::_1, landmark),
pose);
}
if(H2) {
(*H2) = numericalDerivative11<Vector,Vector3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, boost::placeholders::_1), landmark);
(*H2) = numericalDerivative11<Vector, Vector3>(
boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose,
boost::placeholders::_1),
landmark);
}
return inverseDepthError(pose, landmark);
@ -231,13 +237,22 @@ public:
boost::optional<Matrix&> H3=boost::none) const override {
if(H1)
(*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, boost::placeholders::_1, pose2, landmark), pose1);
(*H1) = numericalDerivative11<Vector, Pose3>(
boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this,
boost::placeholders::_1, pose2, landmark),
pose1);
if(H2)
(*H2) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, boost::placeholders::_1, landmark), pose2);
(*H2) = numericalDerivative11<Vector, Pose3>(
boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1,
boost::placeholders::_1, landmark),
pose2);
if(H3)
(*H3) = numericalDerivative11<Vector,Vector3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, boost::placeholders::_1), landmark);
(*H3) = numericalDerivative11<Vector, Vector3>(
boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1,
pose2, boost::placeholders::_1),
landmark);
return inverseDepthError(pose1, pose2, landmark);
}