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

Merge pull request #545 from borglab/fix/cal3bundler-jacobians 

Minor fixes
release/4.3a0
Frank Dellaert 2020-09-29 17:55:38 -04:00 committed by GitHub
parent 1bbd233f0f bb08c62693
commit 151923cf3b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 8 additions and 6 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) // 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 // Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
Matrix23 H_uncal_K; Matrix23 H_uncal_K;
Matrix22 H_uncal_pn; Matrix22 H_uncal_pn, H_uncal_pn_inv;
if (Dcal || Dp) { if (Dcal || Dp) {
// Compute uncalibrate Jacobians // Compute uncalibrate Jacobians
uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn); uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
if (Dp) *Dp = H_uncal_pn.inverse(); H_uncal_pn_inv = H_uncal_pn.inverse();
if (Dcal) *Dcal = -H_uncal_pn.inverse() * H_uncal_K;
if (Dp) *Dp = H_uncal_pn_inv;
if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
} }
return pn; return pn;
} }

5
gtsam/gtsam.i
View File

@ -1106,7 +1106,7 @@ typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified
//typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2; //typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
//typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified; //typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
//typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler; typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
#include <gtsam/geometry/StereoCamera.h> #include <gtsam/geometry/StereoCamera.h>
class StereoCamera { class StereoCamera {
@ -2769,8 +2769,7 @@ class SfmTrack {
class SfmData { class SfmData {
size_t number_cameras() const; size_t number_cameras() const;
size_t number_tracks() const; size_t number_tracks() const;
//TODO(Varun) Need to fix issue #237 first before this can work gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
// gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
gtsam::SfmTrack track(size_t idx) const; gtsam::SfmTrack track(size_t idx) const;
}; };