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

simplified jacobian computation, improved comments

release/4.3a0
lcarlone 2021-08-13 19:54:23 -04:00
parent cbd6a2a01c
commit 9f19077217
2 changed files with 10 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/base/Lie.h
View File

@ -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<T>::TangentVector log_Xinv_Y;
typename MakeJacobian<T, T>::type Hx_tmp, Hy_tmp, H1, H2;
typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x;
T Xinv_Y = traits<T>::Between(X, Y, Hx_tmp, Hy_tmp);
log_Xinv_Y = traits<T>::Logmap(Xinv_Y, H1);
Hx_tmp = H1 * Hx_tmp;
Hy_tmp = H1 * Hy_tmp;
Xinv_Y = traits<T>::Expmap(t * log_Xinv_Y, H1);
Hx_tmp = t * H1 * Hx_tmp;
Hy_tmp = t * H1 * Hy_tmp;
Xinv_Y = traits<T>::Compose(X, Xinv_Y, H1, H2);
Hx_tmp = H1 + H2 * Hx_tmp;
Hy_tmp = H2 * Hy_tmp;
T Xinv_Y = traits<T>::Between(X, Y, between_H_x); // between_H_y = identity
log_Xinv_Y = traits<T>::Logmap(Xinv_Y, log_H);
Xinv_Y = traits<T>::Expmap(t * log_Xinv_Y, exp_H);
Xinv_Y = traits<T>::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<T>::Compose(X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));

5
gtsam/geometry/CameraSet.h
View File

@ -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<int N, int ND, int NDD> // N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is the Hessian block dimension
static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks(