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

Fixed smart factors - but @lucacarlone should realize stereo-calibration cannot be optimized, so how come he is calling project with H3 ?

release/4.3a0
dellaert 2014-12-04 13:29:56 +01:00
parent d2e53d4fe9
commit 4b2040fec6
2 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/StereoCamera.cpp
View File

@ -30,7 +30,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
StereoPoint2 StereoCamera::project(const Point3& point, StereoPoint2 StereoCamera::project(const Point3& point,
OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2) const { OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2,
OptionalJacobian<3,6> 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);
@ -76,6 +77,9 @@ namespace gtsam {
*H2 << d * (*H2); *H2 << d * (*H2);
} }
#endif #endif
if (H3)
// TODO, this is set to zero, as Cal3_S2Stereo cannot be optimized yet
H3->setZero();
} }
// finally translate // finally translate

3
gtsam/geometry/StereoCamera.h
View File

@ -120,7 +120,8 @@ public:
*/ */
StereoPoint2 project(const Point3& point, StereoPoint2 project(const Point3& point,
OptionalJacobian<3, 6> H1 = boost::none, OptionalJacobian<3, 6> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const; OptionalJacobian<3, 3> H2 = boost::none,
OptionalJacobian<3, 6> H3 = boost::none) const;
/** /**
* *