Skip to content

Commit

Permalink
Merge pull request #545 from borglab/fix/cal3bundler-jacobians
Browse files Browse the repository at this point in the history
Minor fixes
  • Loading branch information
dellaert authored Sep 29, 2020
2 parents 1bbd233 + bb08c62 commit 151923c
Show file tree
Hide file tree
Showing 2 changed files with 8 additions and 6 deletions.
9 changes: 6 additions & 3 deletions gtsam/geometry/Cal3Bundler.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down
5 changes: 2 additions & 3 deletions gtsam/gtsam.i
Original file line number Diff line number Diff line change
Expand Up @@ -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
//typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
//typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
//typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;

#include <gtsam/geometry/StereoCamera.h>
class StereoCamera {
Expand Down Expand Up @@ -2769,8 +2769,7 @@ class SfmTrack {
class SfmData {
size_t number_cameras() 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;
};

Expand Down

0 comments on commit 151923c

Please sign in to comment.