Merge remote-tracking branch 'upstream/develop' into sim3
alexma3312
commit
2f322310cd
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
102
gtsam/gtsam.i
102
gtsam/gtsam.i
|
|
@ -334,99 +334,6 @@ virtual class GenericValue : gtsam::Value {
|
|||
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
|
||||
//*************************************************************************
|
||||
|
|
@ -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, const gtsam::FactorIndices& removeFactorIndices);
|
||||
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
|
||||
//void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys);
|
||||
//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);
|
||||
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);
|
||||
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 calculateEstimate() const;
|
||||
|
|
@ -2705,8 +2612,7 @@ class BearingRange {
|
|||
BearingRange(const BEARING& b, const RANGE& r);
|
||||
BEARING bearing() const;
|
||||
RANGE range() const;
|
||||
// TODO(frank): can't class instance itself?
|
||||
// static gtsam::BearingRange Measure(const POSE& pose, const POINT& point);
|
||||
static This Measure(const POSE& pose, const POINT& point);
|
||||
static BEARING MeasureBearing(const POSE& pose, const POINT& point);
|
||||
static RANGE MeasureRange(const POSE& pose, const POINT& point);
|
||||
void print(string s) const;
|
||||
|
|
|
|||
|
|
@ -5,8 +5,6 @@
|
|||
// specify the classes from gtsam we are using
|
||||
virtual class gtsam::Value;
|
||||
class gtsam::Vector6;
|
||||
class gtsam::LieScalar;
|
||||
class gtsam::LieVector;
|
||||
class gtsam::Point2;
|
||||
class gtsam::Point2Vector;
|
||||
class gtsam::Rot2;
|
||||
|
|
@ -476,14 +474,12 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor {
|
|||
DGroundConstraint(size_t key, Vector constraint, const gtsam::noiseModel::Base* model);
|
||||
};
|
||||
|
||||
#include <gtsam/base/deprecated/LieScalar.h>
|
||||
|
||||
#include <gtsam_unstable/dynamics/VelocityConstraint3.h>
|
||||
virtual class VelocityConstraint3 : gtsam::NonlinearFactor {
|
||||
/** Standard constructor */
|
||||
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>
|
||||
|
|
@ -491,7 +487,7 @@ virtual class PendulumFactor1 : gtsam::NonlinearFactor {
|
|||
/** Standard constructor */
|
||||
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>
|
||||
|
|
@ -499,21 +495,21 @@ virtual class PendulumFactor2 : gtsam::NonlinearFactor {
|
|||
/** Standard constructor */
|
||||
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 {
|
||||
/** Standard constructor */
|
||||
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 {
|
||||
/** Standard constructor */
|
||||
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>
|
||||
|
|
|
|||
|
|
@ -4,16 +4,16 @@ 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.
|
||||
|
||||
The interface to the toolbox is generated automatically by the wrap
|
||||
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 interface to the toolbox is generated automatically by the wrap 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.
|
||||
|
||||
## Ubuntu
|
||||
|
||||
If you have a newer Ubuntu system (later than 10.04), you must make a small modification to your MATLAB installation, due to MATLAB being distributed with an old version of the C++ standard library. Delete or rename all files starting with `libstdc++` in your MATLAB installation directory, in paths:
|
||||
|
||||
/usr/local/MATLAB/[version]/sys/os/[system]/
|
||||
/usr/local/MATLAB/[version]/bin/[system]/
|
||||
If you have a newer Ubuntu system (later than 10.04), you must make a small modification to your MATLAB installation, due to MATLAB being distributed with an old version of the C++ standard library. Delete or rename all files starting with `libstdc++` in your MATLAB installation directory, in paths:
|
||||
|
||||
/usr/local/MATLAB/[version]/sys/os/[system]/
|
||||
/usr/local/MATLAB/[version]/bin/[system]/
|
||||
|
||||
## Adding the toolbox to your MATLAB path
|
||||
|
||||
|
|
@ -37,25 +37,28 @@ export LD_LIBRARY_PATH=<install-path>/gtsam:$LD_LIBRARY_PATH
|
|||
|
||||
### 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':
|
||||
Missing symbol 'mexAtExit' 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
|
||||
|
||||
```sh
|
||||
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.
|
||||
|
||||
|
||||
## 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:
|
||||
|
||||
```matlab
|
||||
>> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox
|
||||
|
|
@ -65,7 +68,7 @@ The examples are located in the 'gtsam_examples' subfolder. You may either run
|
|||
|
||||
## Running the unit tests
|
||||
|
||||
The GTSAM MATLAB toolbox also has a small set of unit tests located in the gtsam_tests directory. Example:
|
||||
The GTSAM MATLAB toolbox also has a small set of unit tests located in the gtsam_tests directory. Example:
|
||||
|
||||
```matlab
|
||||
>> cd /Users/yourname/toolbox % Change to wherever you installed the toolbox
|
||||
|
|
@ -86,4 +89,4 @@ Tests complete!
|
|||
|
||||
## Writing your own code
|
||||
|
||||
Coding for the GTSAM MATLAB toolbox is straightforward and very fast once you understand a few basic concepts! Please see the manual to get started.
|
||||
Coding for the GTSAM MATLAB toolbox is straightforward and very fast once you understand a few basic concepts! Please see the manual to get started.
|
||||
|
|
|
|||
|
|
@ -28,10 +28,6 @@ serialized_pose1 = pose1.string_serialize();
|
|||
pose1ds = Pose2.string_deserialize(serialized_pose1);
|
||||
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
|
||||
values = Values;
|
||||
values.insert(i1, pose1);
|
||||
|
|
|
|||
|
|
@ -210,18 +210,26 @@ class MatlabWrapper(object):
|
|||
else:
|
||||
formatted_type_name += name
|
||||
|
||||
if len(type_name.instantiations) == 1:
|
||||
if separator == "::": # C++
|
||||
formatted_type_name += '<{}>'.format(cls._format_type_name(type_name.instantiations[0],
|
||||
include_namespace=include_namespace,
|
||||
constructor=constructor, method=method))
|
||||
else:
|
||||
if separator == "::": # C++
|
||||
templates = []
|
||||
for idx in range(len(type_name.instantiations)):
|
||||
template = '{}'.format(cls._format_type_name(type_name.instantiations[idx],
|
||||
include_namespace=include_namespace,
|
||||
constructor=constructor, method=method))
|
||||
templates.append(template)
|
||||
|
||||
if len(templates) > 0: # If there are no templates
|
||||
formatted_type_name += '<{}>'.format(','.join(templates))
|
||||
|
||||
else:
|
||||
for idx in range(len(type_name.instantiations)):
|
||||
formatted_type_name += '{}'.format(cls._format_type_name(
|
||||
type_name.instantiations[0],
|
||||
type_name.instantiations[idx],
|
||||
separator=separator,
|
||||
include_namespace=False,
|
||||
constructor=constructor, method=method
|
||||
))
|
||||
|
||||
return formatted_type_name
|
||||
|
||||
@classmethod
|
||||
|
|
|
|||
Loading…
Reference in New Issue