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

Merge remote-tracking branch 'upstream/develop' into sim3

release/4.3a0
alexma3312 2020-09-27 20:38:11 -04:00
parent 95724be4ae b6f095c033
commit 2f322310cd
7 changed files with 155 additions and 172 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
gtsam/base/numericalDerivative.h
View File

@ -109,7 +109,7 @@ typename Eigen::Matrix<double, N, 1> numericalGradient(
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* Class Y is the output argument * Class Y is the output argument
* Class X is the input 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 * @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 x2 second argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @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, 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) { 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), 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."); "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), 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."); "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 */ /** 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 x2 n-dimensional second argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @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, 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) { 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), // 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."); // "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), 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."); "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 */ /** 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 * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @return m*n Jacobian computed via central differencing
* All classes Y,X1,X2,X3 need dim, expmap, logmap * 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( typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1, boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) { 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."); "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), 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."); "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> 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 * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @return m*n Jacobian computed via central differencing
* All classes Y,X1,X2,X3 need dim, expmap, logmap * 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( typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1, boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) { 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."); "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), 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."); "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> 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 * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @return m*n Jacobian computed via central differencing
* All classes Y,X1,X2,X3 need dim, expmap, logmap * 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( typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1, boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) { 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."); "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), 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."); "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> 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 x4 fourth argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @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( typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1, 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) { 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."); "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), 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."); "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> 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 x4 fourth argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @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( typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1, 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) { 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."); "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), 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."); "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> 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 x4 fourth argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @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( typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative43(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1, 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) { 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."); "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), 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."); "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> 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 x4 n-dimensional fourth argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @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( typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative44(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1, 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) { 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."); "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), 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."); "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> 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 x5 fifth argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @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( typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative51(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1, 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) { 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."); "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), 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."); "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> 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 x5 fifth argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @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( typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative52(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1, 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) { 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."); "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), 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."); "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> 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 x5 fifth argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @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( typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative53(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1, 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) { 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."); "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), 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."); "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> 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 x5 fifth argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @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( typename internal::FixedSizeMatrix<Y,X4>::type numericalDerivative54(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1, 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) { 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."); "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), 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."); "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> 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 x5 fifth argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @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( typename internal::FixedSizeMatrix<Y,X5>::type numericalDerivative55(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&)> h, const X1& x1, 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) { 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."); "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), 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."); "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> 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 x6 sixth argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @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( 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, 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) { 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."); "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), 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."); "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> 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 x6 sixth argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @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( 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, 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) { 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."); "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), 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."); "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> template<class Y, class X1, class X2, class X3, class X4, class X5, class X6>
@ -611,8 +627,9 @@ inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative62(Y (*
* @param x6 sixth argument value * @param x6 sixth argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @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( 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, 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) { 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."); "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), 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."); "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> 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 x6 sixth argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @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( 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, 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) { 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."); "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), 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."); "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> 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 x6 sixth argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @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( 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, 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) { 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."); "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), 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."); "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> 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 x6 sixth argument value
* @param delta increment for numerical derivative * @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing * @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( typename internal::FixedSizeMatrix<Y, X6>::type numericalDerivative66(
boost::function<Y(const X1&, const X2&, const X3&, const X4&, const X5&, const X6&)> h, 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, 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."); "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), 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."); "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> 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; 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) { TEST(testNumericalDerivative, numeriDerivative61) {
@ -153,6 +160,14 @@ TEST(testNumericalDerivative, numeriDerivative61) {
double, double, double, double>(f6, x1, x2, x3, x4, x5, x6); double, double, double, double>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected61, actual61, 1e-5)); 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); double, double, double>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected62, actual62, 1e-5)); 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); double, double, double>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected63, actual63, 1e-5)); 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); double, double, double>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected64, actual64, 1e-5)); 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); double, double, double>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected65, actual65, 1e-5)); 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); double, double, double>(f6, x1, x2, x3, x4, x5, x6);
EXPECT(assert_equal(expected66, actual66, 1e-5)); 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));
} }
/* ************************************************************************* */ /* ************************************************************************* */

102
gtsam/gtsam.i
View File

@ -334,99 +334,6 @@ virtual class GenericValue : gtsam::Value {
void serializable() const; void serializable() const;
}; };
#include <gtsam/base/deprecated/LieScalar.h>
class LieScalar {
// Standard constructors
LieScalar();
LieScalar(double d);
// Standard interface
double value() const;
// Testable
void print(string s) const;
bool equals(const gtsam::LieScalar& expected, double tol) const;
// Group
static gtsam::LieScalar identity();
gtsam::LieScalar inverse() const;
gtsam::LieScalar compose(const gtsam::LieScalar& p) const;
gtsam::LieScalar between(const gtsam::LieScalar& l2) const;
// Manifold
size_t dim() const;
gtsam::LieScalar retract(Vector v) const;
Vector localCoordinates(const gtsam::LieScalar& t2) const;
// Lie group
static gtsam::LieScalar Expmap(Vector v);
static Vector Logmap(const gtsam::LieScalar& p);
};
#include <gtsam/base/deprecated/LieVector.h>
class LieVector {
// Standard constructors
LieVector();
LieVector(Vector v);
// Standard interface
Vector vector() const;
// Testable
void print(string s) const;
bool equals(const gtsam::LieVector& expected, double tol) const;
// Group
static gtsam::LieVector identity();
gtsam::LieVector inverse() const;
gtsam::LieVector compose(const gtsam::LieVector& p) const;
gtsam::LieVector between(const gtsam::LieVector& l2) const;
// Manifold
size_t dim() const;
gtsam::LieVector retract(Vector v) const;
Vector localCoordinates(const gtsam::LieVector& t2) const;
// Lie group
static gtsam::LieVector Expmap(Vector v);
static Vector Logmap(const gtsam::LieVector& p);
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/base/deprecated/LieMatrix.h>
class LieMatrix {
// Standard constructors
LieMatrix();
LieMatrix(Matrix v);
// Standard interface
Matrix matrix() const;
// Testable
void print(string s) const;
bool equals(const gtsam::LieMatrix& expected, double tol) const;
// Group
static gtsam::LieMatrix identity();
gtsam::LieMatrix inverse() const;
gtsam::LieMatrix compose(const gtsam::LieMatrix& p) const;
gtsam::LieMatrix between(const gtsam::LieMatrix& l2) const;
// Manifold
size_t dim() const;
gtsam::LieMatrix retract(Vector v) const;
Vector localCoordinates(const gtsam::LieMatrix & t2) const;
// Lie group
static gtsam::LieMatrix Expmap(Vector v);
static Vector Logmap(const gtsam::LieMatrix& p);
// enabling serialization functionality
void serialize() const;
};
//************************************************************************* //*************************************************************************
// geometry // geometry
//************************************************************************* //*************************************************************************
@ -2571,9 +2478,9 @@ class ISAM2 {
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys);
// TODO: wrap the full version of update gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys);
//void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys);
//void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys, bool force_relinearize); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, gtsam::KeyGroupMap& constrainedKeys, const gtsam::KeyList& noRelinKeys, const gtsam::KeyList& extraReelimKeys, bool force_relinearize);
gtsam::Values getLinearizationPoint() const; gtsam::Values getLinearizationPoint() const;
gtsam::Values calculateEstimate() const; gtsam::Values calculateEstimate() const;
@ -2705,8 +2612,7 @@ class BearingRange {
BearingRange(const BEARING& b, const RANGE& r); BearingRange(const BEARING& b, const RANGE& r);
BEARING bearing() const; BEARING bearing() const;
RANGE range() const; RANGE range() const;
// TODO(frank): can't class instance itself? static This Measure(const POSE& pose, const POINT& point);
// static gtsam::BearingRange Measure(const POSE& pose, const POINT& point);
static BEARING MeasureBearing(const POSE& pose, const POINT& point); static BEARING MeasureBearing(const POSE& pose, const POINT& point);
static RANGE MeasureRange(const POSE& pose, const POINT& point); static RANGE MeasureRange(const POSE& pose, const POINT& point);
void print(string s) const; void print(string s) const;

14
gtsam_unstable/gtsam_unstable.i
View File

@ -5,8 +5,6 @@
// specify the classes from gtsam we are using // specify the classes from gtsam we are using
virtual class gtsam::Value; virtual class gtsam::Value;
class gtsam::Vector6; class gtsam::Vector6;
class gtsam::LieScalar;
class gtsam::LieVector;
class gtsam::Point2; class gtsam::Point2;
class gtsam::Point2Vector; class gtsam::Point2Vector;
class gtsam::Rot2; class gtsam::Rot2;
@ -476,14 +474,12 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor {
DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model); DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model);
}; };
#include <gtsam/base/deprecated/LieScalar.h>
#include <gtsam_unstable/dynamics/VelocityConstraint3.h> #include <gtsam_unstable/dynamics/VelocityConstraint3.h>
virtual class VelocityConstraint3 : gtsam::NonlinearFactor { virtual class VelocityConstraint3 : gtsam::NonlinearFactor {
/** Standard constructor */ /** Standard constructor */
VelocityConstraint3(size_t key1, size_t key2, size_t velKey, double dt); VelocityConstraint3(size_t key1, size_t key2, size_t velKey, double dt);
Vector evaluateError(const gtsam::LieScalar& x1, const gtsam::LieScalar& x2, const gtsam::LieScalar& v) const; Vector evaluateError(const double& x1, const double& x2, const double& v) const;
}; };
#include <gtsam_unstable/dynamics/Pendulum.h> #include <gtsam_unstable/dynamics/Pendulum.h>
@ -491,7 +487,7 @@ virtual class PendulumFactor1 : gtsam::NonlinearFactor {
/** Standard constructor */ /** Standard constructor */
PendulumFactor1(size_t k1, size_t k, size_t velKey, double dt); PendulumFactor1(size_t k1, size_t k, size_t velKey, double dt);
Vector evaluateError(const gtsam::LieScalar& qk1, const gtsam::LieScalar& qk, const gtsam::LieScalar& v) const; Vector evaluateError(const double& qk1, const double& qk, const double& v) const;
}; };
#include <gtsam_unstable/dynamics/Pendulum.h> #include <gtsam_unstable/dynamics/Pendulum.h>
@ -499,21 +495,21 @@ virtual class PendulumFactor2 : gtsam::NonlinearFactor {
/** Standard constructor */ /** Standard constructor */
PendulumFactor2(size_t vk1, size_t vk, size_t qKey, double dt, double L, double g); PendulumFactor2(size_t vk1, size_t vk, size_t qKey, double dt, double L, double g);
Vector evaluateError(const gtsam::LieScalar& vk1, const gtsam::LieScalar& vk, const gtsam::LieScalar& q) const; Vector evaluateError(const double& vk1, const double& vk, const double& q) const;
}; };
virtual class PendulumFactorPk : gtsam::NonlinearFactor { virtual class PendulumFactorPk : gtsam::NonlinearFactor {
/** Standard constructor */ /** Standard constructor */
PendulumFactorPk(size_t pk, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha); PendulumFactorPk(size_t pk, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha);
Vector evaluateError(const gtsam::LieScalar& pk, const gtsam::LieScalar& qk, const gtsam::LieScalar& qk1) const; Vector evaluateError(const double& pk, const double& qk, const double& qk1) const;
}; };
virtual class PendulumFactorPk1 : gtsam::NonlinearFactor { virtual class PendulumFactorPk1 : gtsam::NonlinearFactor {
/** Standard constructor */ /** Standard constructor */
PendulumFactorPk1(size_t pk1, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha); PendulumFactorPk1(size_t pk1, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha);
Vector evaluateError(const gtsam::LieScalar& pk1, const gtsam::LieScalar& qk, const gtsam::LieScalar& qk1) const; Vector evaluateError(const double& pk1, const double& qk, const double& qk1) const;
}; };
#include <gtsam_unstable/dynamics/SimpleHelicopter.h> #include <gtsam_unstable/dynamics/SimpleHelicopter.h>

13
matlab/README.md
View File

@ -4,8 +4,9 @@ http://borg.cc.gatech.edu/projects/gtsam
This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++ library. To build it, enable `GTSAM_INSTALL_MATLAB_TOOLBOX=ON` in CMake. This is the GTSAM MATLAB toolbox, a MATLAB wrapper around the GTSAM C++ library. To build it, enable `GTSAM_INSTALL_MATLAB_TOOLBOX=ON` in CMake.
The interface to the toolbox is generated automatically by the wrap The interface to the toolbox is generated automatically by the wrap tool which directly parses C++ header files.
tool which directly parses C++ header files. The tool generates matlab proxy objects together with all the native functions for wrapping and unwrapping scalar and non scalar types and objects. The interface generated by the wrap tool also redirects the standard output stream (cout) to matlab's console. The tool generates matlab proxy objects together with all the native functions for wrapping and unwrapping scalar and non scalar types and objects.
The interface generated by the wrap tool also redirects the standard output stream (cout) to matlab's console.
## Ubuntu ## Ubuntu
@ -14,7 +15,6 @@ If you have a newer Ubuntu system (later than 10.04), you must make a small modi
/usr/local/MATLAB/[version]/sys/os/[system]/ /usr/local/MATLAB/[version]/sys/os/[system]/
/usr/local/MATLAB/[version]/bin/[system]/ /usr/local/MATLAB/[version]/bin/[system]/
## Adding the toolbox to your MATLAB path ## Adding the toolbox to your MATLAB path
To get started, first add the `toolbox` (or `gtsam_toolbox`) folder to your MATLAB path - in the MATLAB file browser, right-click on the folder and click 'Add to path -> This folder' (**do not add the subfolders to your path**). To get started, first add the `toolbox` (or `gtsam_toolbox`) folder to your MATLAB path - in the MATLAB file browser, right-click on the folder and click 'Add to path -> This folder' (**do not add the subfolders to your path**).
@ -37,22 +37,25 @@ export LD_LIBRARY_PATH=<install-path>/gtsam:$LD_LIBRARY_PATH
### Linker issues ### Linker issues
If you compile the MATLAB toolbox and everything compiles smoothly, but when you run any MATLAB script, you get following error messages in MATLAB If you compile the MATLAB toolbox and everything compiles smoothly, but when you run any MATLAB script, you get any of the following error messages in MATLAB
``` ```
Invalid MEX-file '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64': Invalid MEX-file '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64':
Missing symbol 'mexAtExit' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64' Missing symbol 'mexAtExit' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64'
Missing symbol 'mexCallMATLABWithObject' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64' Missing symbol 'mexCallMATLABWithObject' required by '/usr/local/gtsam_toolbox/gtsam_wrapper.mexa64'
... ...
``` ```
run following shell line run following shell line
```sh ```sh
export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6 export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6
``` ```
before you run MATLAB from the same shell. before you run MATLAB from the same shell.
This mainly happens if you have GCC >= 5 and newer version MATLAB like R2017a. This mainly happens if you have GCC >= 5 and newer version MATLAB like R2017a.
## Trying out the examples ## Trying out the examples
The examples are located in the 'gtsam_examples' subfolder. You may either run them individually at the MATLAB command line, or open the GTSAM example GUI by running 'gtsamExamples'. Example: The examples are located in the 'gtsam_examples' subfolder. You may either run them individually at the MATLAB command line, or open the GTSAM example GUI by running 'gtsamExamples'. Example:

4
matlab/gtsam_tests/testSerialization.m
View File

@ -28,10 +28,6 @@ serialized_pose1 = pose1.string_serialize();
pose1ds = Pose2.string_deserialize(serialized_pose1); pose1ds = Pose2.string_deserialize(serialized_pose1);
CHECK('pose1ds.equals(pose1, 1e-9)', pose1ds.equals(pose1, 1e-9)); CHECK('pose1ds.equals(pose1, 1e-9)', pose1ds.equals(pose1, 1e-9));
serialized_landmark1 = landmark1.string_serialize();
landmark1ds = Point2.string_deserialize(serialized_landmark1);
CHECK('landmark1ds.equals(landmark1, 1e-9)', landmark1ds.equals(landmark1, 1e-9));
%% Create and serialize Values %% Create and serialize Values
values = Values; values = Values;
values.insert(i1, pose1); values.insert(i1, pose1);

14
wrap/matlab_wrapper.py
View File

@ -210,18 +210,26 @@ class MatlabWrapper(object):
else: else:
formatted_type_name += name formatted_type_name += name
if len(type_name.instantiations) == 1:
if separator == "::": # C++ if separator == "::": # C++
formatted_type_name += '<{}>'.format(cls._format_type_name(type_name.instantiations[0], templates = []
for idx in range(len(type_name.instantiations)):
template = '{}'.format(cls._format_type_name(type_name.instantiations[idx],
include_namespace=include_namespace, include_namespace=include_namespace,
constructor=constructor, method=method)) constructor=constructor, method=method))
templates.append(template)
if len(templates) > 0: # If there are no templates
formatted_type_name += '<{}>'.format(','.join(templates))
else: else:
for idx in range(len(type_name.instantiations)):
formatted_type_name += '{}'.format(cls._format_type_name( formatted_type_name += '{}'.format(cls._format_type_name(
type_name.instantiations[0], type_name.instantiations[idx],
separator=separator, separator=separator,
include_namespace=False, include_namespace=False,
constructor=constructor, method=method constructor=constructor, method=method
)) ))
return formatted_type_name return formatted_type_name
@classmethod @classmethod