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

compute inverse only once

release/4.3a0
Varun Agrawal 2020-09-29 12:57:55 -04:00
parent 7dab718861
commit 0ae5a92d42
1 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/geometry/Cal3Bundler.cpp
View File

@ -127,16 +127,19 @@ Point2 Cal3Bundler::calibrate(const Point2& pi,
// Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
// Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
Matrix23 H_uncal_K;
Matrix22 H_uncal_pn;
Matrix22 H_uncal_pn, H_uncal_pn_inv;
if (Dcal || Dp) {
// Compute uncalibrate Jacobians
uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
if (Dp) *Dp = H_uncal_pn.inverse();
if (Dcal) *Dcal = -H_uncal_pn.inverse() * H_uncal_K;
H_uncal_pn_inv = H_uncal_pn.inverse();
if (Dp) *Dp = H_uncal_pn_inv;
if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
}
return pn;
}