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

move derivative for calibration to third slot to be consistent with PinholeCamera interface

release/4.3a0
cbeall3 2014-07-10 07:55:52 -04:00
parent 391a29bcaf
commit fc513e584d
2 changed files with 9 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/geometry/StereoCamera.cpp
View File

@ -30,7 +30,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
StereoPoint2 StereoCamera::project(const Point3& point, StereoPoint2 StereoCamera::project(const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const {
#ifdef STEREOCAMERA_CHAIN_RULE #ifdef STEREOCAMERA_CHAIN_RULE
const Point3 q = leftCamPose_.transform_to(point, H1, H2); const Point3 q = leftCamPose_.transform_to(point, H1, H2);

17
gtsam/geometry/StereoCamera.h
View File

@ -114,21 +114,18 @@ public:
/* /*
* project 3D point and compute optional derivatives * project 3D point and compute optional derivatives
* @param H1 derivative with respect to pose
* @param H2 derivative with respect to point
* @param H3 IGNORED (for calibration)
*/ */
StereoPoint2 project(const Point3& point, StereoPoint2 project(const Point3& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const; boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const;
/* /**
* to accomodate tsam's assumption that K is estimated, too *
*/ */
StereoPoint2 project(const Point3& point,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H1_k,
boost::optional<Matrix&> H2) const {
return project(point, H1, H2);
}
Point3 backproject(const StereoPoint2& z) const { Point3 backproject(const StereoPoint2& z) const {
Vector measured = z.vector(); Vector measured = z.vector();
double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]); double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]);