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

now using MakeJacobian

release/4.3a0
lcarlone 2021-08-13 19:36:15 -04:00
parent 0b002e6f24
commit cbd6a2a01c
1 changed files with 3 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/base/Lie.h
View File

@ -324,13 +324,12 @@ T expm(const Vector& x, int K=7) {
*/ */
template <typename T> template <typename T>
T interpolate(const T& X, const T& Y, double t, T interpolate(const T& X, const T& Y, double t,
OptionalJacobian< traits<T>::dimension, traits<T>::dimension > Hx = boost::none, typename MakeOptionalJacobian<T, T>::type Hx = boost::none,
OptionalJacobian< traits<T>::dimension, traits<T>::dimension > Hy = boost::none) { typename MakeOptionalJacobian<T, T>::type Hy = boost::none) {
assert(t >= 0.0 && t <= 1.5); assert(t >= 0.0 && t <= 1.5);
if (Hx || Hy) { if (Hx || Hy) {
typedef Eigen::Matrix<double, traits<T>::dimension, traits<T>::dimension> Jacobian;
typename traits<T>::TangentVector log_Xinv_Y; typename traits<T>::TangentVector log_Xinv_Y;
Jacobian Hx_tmp, Hy_tmp, H1, H2; typename MakeJacobian<T, T>::type Hx_tmp, Hy_tmp, H1, H2;
T Xinv_Y = traits<T>::Between(X, Y, Hx_tmp, Hy_tmp); T Xinv_Y = traits<T>::Between(X, Y, Hx_tmp, Hy_tmp);
log_Xinv_Y = traits<T>::Logmap(Xinv_Y, H1); log_Xinv_Y = traits<T>::Logmap(Xinv_Y, H1);