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

Deprecated project with three derivatives, it's bogus: StereoCamera holds a pointer to a fixed calibration, and hence is similar to the new "PinholePose".

release/4.3a0
dellaert 2015-02-22 23:29:40 +01:00
parent 9d2666e56e
commit 8d5e61a1bf
3 changed files with 48 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
gtsam/geometry/StereoCamera.cpp
View File

@ -29,16 +29,15 @@ namespace gtsam {
}
/* ************************************************************************* */
StereoPoint2 StereoCamera::project(const Point3& point,
OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2,
OptionalJacobian<3,0> H3) const {
StereoPoint2 StereoCamera::project(const Point3& point) const {
return project2(point);
}
/* ************************************************************************* */
StereoPoint2 StereoCamera::project2(const Point3& point,
OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2) const {
#ifdef STEREOCAMERA_CHAIN_RULE
const Point3 q = leftCamPose_.transform_to(point, H1, H2);
#else
// omit derivatives
const Point3 q = leftCamPose_.transform_to(point);
#endif
if ( q.z() <= 0 ) throw StereoCheiralityException();
@ -56,12 +55,6 @@ namespace gtsam {
// check if derivatives need to be computed
if (H1 || H2) {
#ifdef STEREOCAMERA_CHAIN_RULE
// just implement chain rule
Matrix3 D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian
if (H1) *H1 = D_project_point*(*H1);
if (H2) *H2 = D_project_point*(*H2);
#else
// optimized version, see StereoCamera.nb
if (H1) {
const double v1 = v/fy, v2 = fx*v1, dx=d*x;
@ -76,9 +69,6 @@ namespace gtsam {
fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v;
*H2 << d * (*H2);
}
#endif
if (H3)
throw std::runtime_error("StereoCamera::project does not support third derivative yet");
}
// finally translate
@ -86,15 +76,23 @@ namespace gtsam {
}
/* ************************************************************************* */
Matrix3 StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
double d = 1.0 / P.z(), d2 = d*d;
const Cal3_S2Stereo& K = *K_;
double f_x = K.fx(), f_y = K.fy(), b = K.baseline();
Matrix3 m;
m << f_x*d, 0.0, -d2*f_x* P.x(),
f_x*d, 0.0, -d2*f_x*(P.x() - b),
0.0, f_y*d, -d2*f_y* P.y();
return m;
StereoPoint2 StereoCamera::project(const Point3& point,
OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2,
OptionalJacobian<3,0> H3) const {
if (H3)
throw std::runtime_error(
"StereoCamera::project does not support third derivative - BTW use project2");
return project2(point,H1,H2);
}
/* ************************************************************************* */
Point3 StereoCamera::backproject(const StereoPoint2& z) const {
Vector measured = z.vector();
double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]);
double X = Z * (measured[0] - K_->px()) / K_->fx();
double Y = Z * (measured[2] - K_->py()) / K_->fy();
Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
return world_point;
}
}

43
gtsam/geometry/StereoCamera.h
View File

@ -17,9 +17,6 @@
#pragma once
#include <boost/tuple/tuple.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/StereoPoint2.h>
@ -127,33 +124,37 @@ public:
return K_->baseline();
}
/*
* project 3D point and compute optional derivatives
/// Project 3D point to StereoPoint2 (uL,uR,v)
StereoPoint2 project(const Point3& point) const;
/** Project 3D point and compute optional derivatives
* @param H1 derivative with respect to pose
* @param H2 derivative with respect to point
*/
StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 =
boost::none, OptionalJacobian<3, 3> H2 = boost::none) const;
/// back-project a measurement
Point3 backproject(const StereoPoint2& z) const;
/// @}
/// @name Deprecated
/// @{
/** Project 3D point and compute optional derivatives
* @deprecated, use project2 - this class has fixed calibration
* @param H1 derivative with respect to pose
* @param H2 derivative with respect to point
* @param H3 IGNORED (for calibration)
*/
StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1 =
boost::none, OptionalJacobian<3, 3> H2 = boost::none,
OptionalJacobian<3, 0> H3 = boost::none) const;
/// back-project a measurement
Point3 backproject(const StereoPoint2& z) const {
Vector measured = z.vector();
double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]);
double X = Z * (measured[0] - K_->px()) / K_->fx();
double Y = Z * (measured[2] - K_->py()) / K_->fy();
Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
return world_point;
}
StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1,
OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 =
boost::none) const;
/// @}
private:
/// utility function
Matrix3 Dproject_to_stereo_camera1(const Point3& P) const;
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {

8
gtsam/geometry/tests/testStereoCamera.cpp
View File

@ -96,16 +96,12 @@ static StereoPoint2 project3(const Pose3& pose, const Point3& point, const Cal3_
TEST( StereoCamera, Dproject)
{
Matrix expected1 = numericalDerivative31(project3, camPose, landmark, *K);
Matrix actual1; stereoCam.project(landmark, actual1, boost::none, boost::none);
Matrix actual1; stereoCam.project2(landmark, actual1, boost::none);
CHECK(assert_equal(expected1,actual1,1e-7));
Matrix expected2 = numericalDerivative32(project3,camPose, landmark, *K);
Matrix actual2; stereoCam.project(landmark, boost::none, actual2, boost::none);
Matrix actual2; stereoCam.project2(landmark, boost::none, actual2);
CHECK(assert_equal(expected2,actual2,1e-7));
Matrix expected3 = numericalDerivative33(project3,camPose, landmark, *K);
Matrix actual3; stereoCam.project(landmark, boost::none, boost::none, actual3);
// CHECK(assert_equal(expected3,actual3,1e-8));
}
/* ************************************************************************* */