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

Added optional derivatives to compose, between, and inverse for the two camera types

release/4.3a0
Stephen Williams 2012-10-21 22:32:16 +00:00
parent 5505483111
commit 857b0d0d8c
2 changed files with 61 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/geometry/CalibratedCamera.h
View File

@ -81,14 +81,23 @@ namespace gtsam {
/// return pose /// return pose
inline const Pose3& pose() const { return pose_; } inline const Pose3& pose() const { return pose_; }
/// compose the poses /// compose the two camera poses: TODO Frank says this might not make sense
inline const CalibratedCamera compose(const CalibratedCamera &c) const { inline const CalibratedCamera compose(const CalibratedCamera &c,
return CalibratedCamera( pose_ * c.pose() ) ; boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
return CalibratedCamera( pose_.compose(c.pose(), H1, H2) );
} }
/// invert the camera's pose /// between the two camera poses: TODO Frank says this might not make sense
inline const CalibratedCamera inverse() const { inline const CalibratedCamera between(const CalibratedCamera& c,
return CalibratedCamera( pose_.inverse() ) ; boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none) const {
return CalibratedCamera( pose_.inverse(H1) );
} }
/** /**

50
gtsam/geometry/PinholeCamera.h
View File

@ -154,13 +154,55 @@ namespace gtsam {
/// @{ /// @{
/// compose two cameras: TODO Frank says this might not make sense /// compose two cameras: TODO Frank says this might not make sense
inline const PinholeCamera compose(const Pose3 &c) const { inline const PinholeCamera compose(const PinholeCamera &c,
return PinholeCamera( pose_ * c, K_ ) ; boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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<Matrix&> 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 /// compose two cameras: TODO Frank says this might not make sense
inline const PinholeCamera inverse() const { inline const PinholeCamera compose(const Pose3 &c) const {
return PinholeCamera( pose_.inverse(), K_ ) ; return PinholeCamera( pose_.compose(c), K_ );
} }
/// @} /// @}