diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 1e19bfc0e..48d7bf531 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -329,21 +329,15 @@ T interpolate(const T& X, const T& Y, double t, assert(t >= 0.0 && t <= 1.5); if (Hx || Hy) { typename traits::TangentVector log_Xinv_Y; - typename MakeJacobian::type Hx_tmp, Hy_tmp, H1, H2; + typename MakeJacobian::type between_H_x, log_H, exp_H, compose_H_x; - T Xinv_Y = traits::Between(X, Y, Hx_tmp, Hy_tmp); - log_Xinv_Y = traits::Logmap(Xinv_Y, H1); - Hx_tmp = H1 * Hx_tmp; - Hy_tmp = H1 * Hy_tmp; - Xinv_Y = traits::Expmap(t * log_Xinv_Y, H1); - Hx_tmp = t * H1 * Hx_tmp; - Hy_tmp = t * H1 * Hy_tmp; - Xinv_Y = traits::Compose(X, Xinv_Y, H1, H2); - Hx_tmp = H1 + H2 * Hx_tmp; - Hy_tmp = H2 * Hy_tmp; + T Xinv_Y = traits::Between(X, Y, between_H_x); // between_H_y = identity + log_Xinv_Y = traits::Logmap(Xinv_Y, log_H); + Xinv_Y = traits::Expmap(t * log_Xinv_Y, exp_H); + Xinv_Y = traits::Compose(X, Xinv_Y, compose_H_x); // compose_H_xinv_y = identity - if(Hx) *Hx = Hx_tmp; - if(Hy) *Hy = Hy_tmp; + if(Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x; + if(Hy) *Hy = t * exp_H * log_H; return Xinv_Y; } return traits::Compose(X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index f0bfffb58..3fc77bb2e 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -198,8 +198,9 @@ public: * G = F' * F - F' * E * P * E' * F * g = F' * (b - E * P * E' * b) * In this version, we allow for the case where the keys in the Jacobian are organized - * differents from the keys in the output SymmetricBlockMatrix - * In particular: each block of the Jacobian captures 2 poses (useful for rolling shutter and extrinsic calibration) + * differently from the keys in the output SymmetricBlockMatrix + * In particular: each diagonal block of the Jacobian F captures 2 poses (useful for rolling shutter and extrinsic calibration) + * such that F keeps the block structure that makes the Schur complement trick fast. */ template // N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is the Hessian block dimension static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks(