diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index aac0d624d..723ffc67e 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -81,14 +81,23 @@ namespace gtsam { /// return pose inline const Pose3& pose() const { return pose_; } - /// compose the poses - inline const CalibratedCamera compose(const CalibratedCamera &c) const { - return CalibratedCamera( pose_ * c.pose() ) ; + /// compose the two camera poses: TODO Frank says this might not make sense + inline const CalibratedCamera compose(const CalibratedCamera &c, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + return CalibratedCamera( pose_.compose(c.pose(), H1, H2) ); } - /// invert the camera's pose - inline const CalibratedCamera inverse() const { - return CalibratedCamera( pose_.inverse() ) ; + /// between the two camera poses: TODO Frank says this might not make sense + inline const CalibratedCamera between(const CalibratedCamera& c, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + return CalibratedCamera( pose_.between(c.pose(), H1, H2) ); + } + + /// invert the camera pose: TODO Frank says this might not make sense + inline const CalibratedCamera inverse(boost::optional H1=boost::none) const { + return CalibratedCamera( pose_.inverse(H1) ); } /** diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index d20674ea4..39b5b679e 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -154,13 +154,55 @@ namespace gtsam { /// @{ /// compose two cameras: TODO Frank says this might not make sense - inline const PinholeCamera compose(const Pose3 &c) const { - return PinholeCamera( pose_ * c, K_ ) ; + inline const PinholeCamera compose(const PinholeCamera &c, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + PinholeCamera result( pose_.compose(c.pose(), H1, H2), K_ ); + if(H1) { + H1->conservativeResize(Dim(), Dim()); + H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),Calibration::Dim()); + H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); + } + if(H2) { + H2->conservativeResize(Dim(), Dim()); + H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),Calibration::Dim()); + H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); + } + return result; + } + + /// between two cameras: TODO Frank says this might not make sense + inline const PinholeCamera between(const PinholeCamera& c, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + PinholeCamera result( pose_.between(c.pose(), H1, H2), K_ ); + if(H1) { + H1->conservativeResize(Dim(), Dim()); + H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),Calibration::Dim()); + H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); + } + if(H2) { + H2->conservativeResize(Dim(), Dim()); + H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),Calibration::Dim()); + H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); + } + return result; + } + + /// inverse camera: TODO Frank says this might not make sense + inline const PinholeCamera inverse(boost::optional H1=boost::none) const { + PinholeCamera result( pose_.inverse(H1), K_ ); + if(H1) { + H1->conservativeResize(Dim(), Dim()); + H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),Calibration::Dim()); + H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); + } + return result; } /// compose two cameras: TODO Frank says this might not make sense - inline const PinholeCamera inverse() const { - return PinholeCamera( pose_.inverse(), K_ ) ; + inline const PinholeCamera compose(const Pose3 &c) const { + return PinholeCamera( pose_.compose(c), K_ ); } /// @}