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

Merge pull request #536 from borglab/feature/numerical-derivative-dim 

Add numericalDerivative Dimension Template Parameter
release/4.3a0
Frank Dellaert 2020-09-26 16:26:04 -04:00 committed by GitHub
parent 23547c76b8 ff57bbc2c4
commit b9733a8396
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 116 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
gtsam/base/numericalDerivative.h
View File

@ -109,7 +109,7 @@ typename Eigen::Matrix<double, N, 1> numericalGradient(
* @param delta increment for numerical derivative
* Class Y is the output argument
* Class X is the input argument
* int N is the dimension of the X input value if variable dimension type but known at test time
* @tparam int N is the dimension of the X input value if variable dimension type but known at test time
* @return m*n Jacobian computed via central differencing
*/
@ -167,15 +167,16 @@ typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const
* @param x2 second argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2>
template<class Y, class X1, class X2, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const boost::function<Y(const X1&, const X2&)>& h,
const X1& x1, const X2& x2, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"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>(boost::bind(h, _1, boost::cref(x2)), x1, delta);
return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2)), x1, delta);
}
/** use a raw C++ function pointer */
@ -192,15 +193,16 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(cons
* @param x2 n-dimensional second argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2>
template<class Y, class X1, class X2, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
const X1& x1, const X2& x2, double delta = 1e-5) {
// 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.");
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>(boost::bind(h, boost::cref(x1), _1), x2, delta);
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1), x2, delta);
}
/** use a raw C++ function pointer */
@ -219,8 +221,9 @@ typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(cons
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* All classes Y,X1,X2,X3 need dim, expmap, logmap
* @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3>
template<class Y, class X1, class X2, class X3, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
@ -228,7 +231,7 @@ 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>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3)), x1, delta);
return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3)), x1, delta);
}
template<class Y, class X1, class X2, class X3>
@ -247,8 +250,9 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(Y (*h)(cons
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* All classes Y,X1,X2,X3 need dim, expmap, logmap
* @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3>
template<class Y, class X1, class X2, class X3, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
@ -256,7 +260,7 @@ 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>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta);
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3)), x2, delta);
}
template<class Y, class X1, class X2, class X3>
@ -275,8 +279,9 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(Y (*
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* All classes Y,X1,X2,X3 need dim, expmap, logmap
* @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3>
template<class Y, class X1, class X2, class X3, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
@ -284,7 +289,7 @@ 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>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta);
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1), x3, delta);
}
template<class Y, class X1, class X2, class X3>
@ -303,8 +308,9 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*
* @param x4 fourth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4>
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
@ -312,7 +318,7 @@ 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>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta);
return numericalDerivative11<Y, X1, N>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4)), x1, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
@ -330,8 +336,9 @@ inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(Y (*
* @param x4 fourth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4>
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
@ -339,7 +346,7 @@ 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>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta);
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4)), x2, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
@ -357,8 +364,9 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(Y (*
* @param x4 fourth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4>
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
@ -366,7 +374,7 @@ 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>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta);
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4)), x3, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
@ -384,8 +392,9 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(Y (*
* @param x4 n-dimensional fourth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X4 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4>
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X4>::dimension>
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
@ -393,7 +402,7 @@ 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>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta);
return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1), x4, delta);
}
template<class Y, class X1, class X2, class X3, class X4>
@ -412,8 +421,9 @@ inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(Y (*
* @param x5 fifth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5>
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
@ -421,7 +431,7 @@ 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>(boost::bind(h, _1, boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5)), x1, delta);
return numericalDerivative11<Y, X1, N>(boost::bind(h, _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>
@ -440,8 +450,9 @@ inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(Y (*
* @param x5 fifth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5>
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
@ -449,7 +460,7 @@ 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>(boost::bind(h, boost::cref(x1), _1, boost::cref(x3), boost::cref(x4), boost::cref(x5)), x2, delta);
return numericalDerivative11<Y, X2, N>(boost::bind(h, boost::cref(x1), _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>
@ -468,8 +479,9 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(Y (*
* @param x5 fifth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5>
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
@ -477,7 +489,7 @@ 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>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta);
return numericalDerivative11<Y, X3, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), _1, boost::cref(x4), boost::cref(x5)), x3, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
@ -496,8 +508,9 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(Y (*
* @param x5 fifth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X4 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5>
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X4>::dimension>
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
@ -505,7 +518,7 @@ 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>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta);
return numericalDerivative11<Y, X4, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _1, boost::cref(x5)), x4, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
@ -524,8 +537,9 @@ inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(Y (*
* @param x5 fifth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X5 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5>
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X5>::dimension>
typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, double delta = 1e-5) {
@ -533,7 +547,7 @@ 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>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta);
return numericalDerivative11<Y, X5, N>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _1), x5, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5>
@ -553,8 +567,9 @@ inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(Y (*
* @param x6 sixth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X1 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X1>::dimension>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
@ -562,7 +577,7 @@ 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>(boost::bind(h, _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, _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>
@ -582,8 +597,9 @@ inline typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative61(Y (*
* @param x6 sixth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X2 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X2>::dimension>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
@ -591,7 +607,7 @@ 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>(boost::bind(h, boost::cref(x1), _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), _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>
@ -608,11 +624,12 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(Y (*
* @param x3 third argument value
* @param x4 fourth argument value
* @param x5 fifth argument value
* @param x6 sixth argument value
* @param x6 sixth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X3 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X3>::dimension>
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
@ -620,7 +637,7 @@ 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>(boost::bind(h, boost::cref(x1), boost::cref(x2), _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), _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>
@ -640,8 +657,9 @@ inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative63(Y (*
* @param x6 sixth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X4 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X4>::dimension>
typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
@ -649,7 +667,7 @@ 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>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), _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), _1, boost::cref(x5), boost::cref(x6)), x4, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
@ -669,8 +687,9 @@ inline typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative64(Y (*
* @param x6 sixth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X5 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X5>::dimension>
typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, double delta = 1e-5) {
@ -678,7 +697,7 @@ 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>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), _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), _1, boost::cref(x6)), x5, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
@ -698,8 +717,9 @@ inline typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative65(Y (*
* @param x6 sixth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
* @tparam int N is the dimension of the X6 input value if variable dimension type but known at test time
*/
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X6>::dimension>
typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h,
const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6,
@ -708,7 +728,7 @@ 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>(boost::bind(h, boost::cref(x1), boost::cref(x2), boost::cref(x3), boost::cref(x4), boost::cref(x5), _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), _1), x6, delta);
}
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>

54
gtsam/base/tests/testNumericalDerivative.cpp
View File

@ -143,6 +143,13 @@ Vector6 f6(const double x1, const double x2, const double x3, const double x4,
return result;
}
Vector g6(const double x1, const double x2, const double x3, const double x4,
const double x5, const double x6) {
Vector result(6);
result << sin(x1), cos(x2), x3 * x3, x4 * x4 * x4, sqrt(x5), sin(x6) - cos(x6);
return result;
}
/* ************************************************************************* */
//
TEST(testNumericalDerivative, numeriDerivative61) {
@ -153,6 +160,14 @@ TEST(testNumericalDerivative, numeriDerivative61) {
double, double, double, double>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected61, actual61, 1e-5));
Matrix expected61Dynamic = Matrix::Zero(6, 1);
expected61Dynamic(0, 0) = cos(x1);
Matrix actual61Dynamic =
numericalDerivative61<Vector, double, double, double, double, double,
double, 1>(g6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected61Dynamic, actual61Dynamic, 1e-5));
}
/* ************************************************************************* */
@ -165,6 +180,13 @@ TEST(testNumericalDerivative, numeriDerivative62) {
double, double, double>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected62, actual62, 1e-5));
Matrix expected62Dynamic = Matrix::Zero(6, 1);
expected62Dynamic(1, 0) = -sin(x2);
Matrix61 actual62Dynamic = numericalDerivative62<Vector, double, double,
double, double, double, double, 1>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected62Dynamic, actual62Dynamic, 1e-5));
}
/* ************************************************************************* */
@ -177,6 +199,14 @@ TEST(testNumericalDerivative, numeriDerivative63) {
double, double, double>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected63, actual63, 1e-5));
Matrix expected63Dynamic = Matrix::Zero(6, 1);
expected63Dynamic(2, 0) = 2 * x3;
Matrix61 actual63Dynamic =
numericalDerivative63<Vector, double, double, double, double, double,
double, 1>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected63Dynamic, actual63Dynamic, 1e-5));
}
/* ************************************************************************* */
@ -189,6 +219,14 @@ TEST(testNumericalDerivative, numeriDerivative64) {
double, double, double>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected64, actual64, 1e-5));
Matrix expected64Dynamic = Matrix::Zero(6, 1);
expected64Dynamic(3, 0) = 3 * x4 * x4;
Matrix61 actual64Dynamic =
numericalDerivative64<Vector, double, double, double, double, double,
double, 1>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected64Dynamic, actual64Dynamic, 1e-5));
}
/* ************************************************************************* */
@ -201,6 +239,14 @@ TEST(testNumericalDerivative, numeriDerivative65) {
double, double, double>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected65, actual65, 1e-5));
Matrix expected65Dynamic = Matrix::Zero(6, 1);
expected65Dynamic(4, 0) = 0.5 / sqrt(x5);
Matrix61 actual65Dynamic =
numericalDerivative65<Vector, double, double, double, double, double,
double, 1>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected65Dynamic, actual65Dynamic, 1e-5));
}
/* ************************************************************************* */
@ -213,6 +259,14 @@ TEST(testNumericalDerivative, numeriDerivative66) {
double, double, double>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected66, actual66, 1e-5));
Matrix expected66Dynamic = Matrix::Zero(6, 1);
expected66Dynamic(5, 0) = cos(x6) + sin(x6);
Matrix61 actual66Dynamic =
numericalDerivative66<Vector, double, double, double, double, double,
double, 1>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected66Dynamic, actual66Dynamic, 1e-5));
}
/* ************************************************************************* */