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

fixed sized matrices and minor improvements

release/4.3a0
Varun Agrawal 2020-09-29 10:49:44 -04:00
parent 8634d3f42e
commit 7dab718861
1 changed files with 6 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/geometry/Cal3Bundler.cpp
View File

@ -126,20 +126,17 @@ Point2 Cal3Bundler::calibrate(const Point2& pi,
// df/pi = -I (pn and pi are independent args)
// 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
Matrix H_uncal_K, H_uncal_pn;
Matrix23 H_uncal_K;
Matrix22 H_uncal_pn;
if (Dcal || Dp) {
// Compute uncalibrate Jacobians
uncalibrate(pn, H_uncal_K, H_uncal_pn);
}
uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
if (Dcal) {
*Dcal = -H_uncal_pn.inverse() * H_uncal_K;
}
if (Dp) {
*Dp = H_uncal_pn.inverse();
}
if (Dp) *Dp = H_uncal_pn.inverse();
if (Dcal) *Dcal = -H_uncal_pn.inverse() * H_uncal_K;
}
return pn;
}