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

Make derivative zero, throw exception.

release/4.3a0
dellaert 2015-02-19 18:05:50 +01:00
parent 0a3272dab6
commit b5d9ed4529
2 changed files with 3 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/geometry/StereoCamera.cpp
View File

@ -31,7 +31,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
StereoPoint2 StereoCamera::project(const Point3& point, StereoPoint2 StereoCamera::project(const Point3& point,
OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2, OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2,
OptionalJacobian<3,6> H3) const { OptionalJacobian<3,0> 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);
@ -78,8 +78,7 @@ namespace gtsam {
} }
#endif #endif
if (H3) if (H3)
// TODO, this is set to zero, as Cal3_S2Stereo cannot be optimized yet throw std::runtime_error("StereoCamera::project does not support third derivative yet");
H3->setZero();
} }
// finally translate // finally translate

2
gtsam/geometry/StereoCamera.h
View File

@ -125,7 +125,7 @@ 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, OptionalJacobian<3, 3> H2 = boost::none,
OptionalJacobian<3, 6> H3 = boost::none) const; OptionalJacobian<3, 0> H3 = boost::none) const;
/** /**
* *