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

Reformatted with new style file, renamed some derivatives to Dcal, Dpose, Dpoint etc.

release/4.3a0
Frank Dellaert 2013-10-12 06:02:16 +00:00
parent b161a106c7
commit 1a20272fa2
3 changed files with 202 additions and 230 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
gtsam/geometry/CalibratedCamera.cpp
View File

@ -27,22 +27,23 @@ CalibratedCamera::CalibratedCamera(const Pose3& pose) :
} }
/* ************************************************************************* */ /* ************************************************************************* */
CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(Pose3::Expmap(v)) {} CalibratedCamera::CalibratedCamera(const Vector &v) :
pose_(Pose3::Expmap(v)) {
}
/* ************************************************************************* */ /* ************************************************************************* */
Point2 CalibratedCamera::project_to_camera(const Point3& P, Point2 CalibratedCamera::project_to_camera(const Point3& P,
boost::optional<Matrix&> H1) { boost::optional<Matrix&> H1) {
if (H1) { if (H1) {
double d = 1.0 / P.z(), d2 = d * d; double d = 1.0 / P.z(), d2 = d * d;
*H1 = Matrix_(2, 3, *H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
d, 0.0, -P.x() * d2,
0.0, d, -P.y() * d2);
} }
return Point2(P.x() / P.z(), P.y() / P.z()); return Point2(P.x() / P.z(), P.y() / P.z());
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 CalibratedCamera::backproject_from_camera(const Point2& p, const double scale) { Point3 CalibratedCamera::backproject_from_camera(const Point2& p,
const double scale) {
return Point3(p.x() * scale, p.y() * scale, scale); return Point3(p.x() * scale, p.y() * scale, scale);
} }
@ -58,11 +59,10 @@ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
/* ************************************************************************* */ /* ************************************************************************* */
Point2 CalibratedCamera::project(const Point3& point, Point2 CalibratedCamera::project(const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) const {
boost::optional<Matrix&> H2) const {
#ifdef CALIBRATEDCAMERA_CHAIN_RULE #ifdef CALIBRATEDCAMERA_CHAIN_RULE
Point3 q = pose_.transform_to(point, H1, H2); Point3 q = pose_.transform_to(point, Dpose, Dpoint);
#else #else
Point3 q = pose_.transform_to(point); Point3 q = pose_.transform_to(point);
#endif #endif
@ -72,27 +72,26 @@ Point2 CalibratedCamera::project(const Point3& point,
if (q.z() <= 0) if (q.z() <= 0)
throw CheiralityException(); throw CheiralityException();
if (H1 || H2) { if (Dpose || Dpoint) {
#ifdef CALIBRATEDCAMERA_CHAIN_RULE #ifdef CALIBRATEDCAMERA_CHAIN_RULE
// just implement chain rule // just implement chain rule
Matrix H; Matrix H;
project_to_camera(q,H); project_to_camera(q,H);
if (H1) *H1 = H * (*H1); if (Dpose) *Dpose = H * (*Dpose);
if (H2) *H2 = H * (*H2); if (Dpoint) *Dpoint = H * (*Dpoint);
#else #else
// optimized version, see CalibratedCamera.nb // optimized version, see CalibratedCamera.nb
const double z = q.z(), d = 1.0 / z; const double z = q.z(), d = 1.0 / z;
const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v; const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v;
if (H1) *H1 = Matrix_(2,6, if (Dpose)
uv,-(1.+u*u), v, -d , 0., d*u, *Dpose = Matrix_(2, 6, uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
(1.+v*v), -uv,-u, 0.,-d , d*v -uv, -u, 0., -d, d * v);
); if (Dpoint) {
if (H2) {
const Matrix R(pose_.rotation().matrix()); const Matrix R(pose_.rotation().matrix());
*H2 = d * Matrix_(2,3, *Dpoint = d
R(0,0) - u*R(0,2), R(1,0) - u*R(1,2), R(2,0) - u*R(2,2), * Matrix_(2, 3, R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
R(0,1) - v*R(0,2), R(1,1) - v*R(1,2), R(2,1) - v*R(2,2) R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2),
); R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2));
} }
#endif #endif
} }

72
gtsam/geometry/CalibratedCamera.h
View File

@ -24,9 +24,12 @@
namespace gtsam { namespace gtsam {
class GTSAM_EXPORT CheiralityException: public ThreadsafeException<CheiralityException> { class GTSAM_EXPORT CheiralityException: public ThreadsafeException<
CheiralityException> {
public: public:
CheiralityException() : ThreadsafeException<CheiralityException>("Cheirality Exception") {} CheiralityException() :
ThreadsafeException<CheiralityException>("Cheirality Exception") {
}
}; };
/** /**
@ -46,7 +49,8 @@ namespace gtsam {
/// @{ /// @{
/// default constructor /// default constructor
CalibratedCamera() {} CalibratedCamera() {
}
/// construct with pose /// construct with pose
explicit CalibratedCamera(const Pose3& pose); explicit CalibratedCamera(const Pose3& pose);
@ -76,27 +80,31 @@ namespace gtsam {
/// @{ /// @{
/// destructor /// destructor
virtual ~CalibratedCamera() {} virtual ~CalibratedCamera() {
}
/// return pose /// return pose
inline const Pose3& pose() const { return pose_; } inline const Pose3& pose() const {
return pose_;
}
/// compose the two camera poses: TODO Frank says this might not make sense /// compose the two camera poses: TODO Frank says this might not make sense
inline const CalibratedCamera compose(const CalibratedCamera &c, inline const CalibratedCamera compose(const CalibratedCamera &c,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::optional<Matrix&> H2=boost::none) const { boost::none) const {
return CalibratedCamera(pose_.compose(c.pose(), H1, H2)); return CalibratedCamera(pose_.compose(c.pose(), H1, H2));
} }
/// between the two camera poses: TODO Frank says this might not make sense /// between the two camera poses: TODO Frank says this might not make sense
inline const CalibratedCamera between(const CalibratedCamera& c, inline const CalibratedCamera between(const CalibratedCamera& c,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::optional<Matrix&> H2=boost::none) const { boost::none) const {
return CalibratedCamera(pose_.between(c.pose(), H1, H2)); return CalibratedCamera(pose_.between(c.pose(), H1, H2));
} }
/// invert the camera pose: TODO Frank says this might not make sense /// invert the camera pose: TODO Frank says this might not make sense
inline const CalibratedCamera inverse(boost::optional<Matrix&> H1=boost::none) const { inline const CalibratedCamera inverse(boost::optional<Matrix&> H1 =
boost::none) const {
return CalibratedCamera(pose_.inverse(H1)); return CalibratedCamera(pose_.inverse(H1));
} }
@ -119,11 +127,14 @@ namespace gtsam {
Vector localCoordinates(const CalibratedCamera& T2) const; Vector localCoordinates(const CalibratedCamera& T2) const;
/// Lie group dimensionality /// Lie group dimensionality
inline size_t dim() const { return 6 ; } inline size_t dim() const {
return 6;
}
/// Lie group dimensionality /// Lie group dimensionality
inline static size_t Dim() { return 6 ; } inline static size_t Dim() {
return 6;
}
/* ************************************************************************* */ /* ************************************************************************* */
// measurement functions and derivatives // measurement functions and derivatives
@ -132,18 +143,17 @@ namespace gtsam {
/// @} /// @}
/// @name Transformations and mesaurement functions /// @name Transformations and mesaurement functions
/// @{ /// @{
/** /**
* This function receives the camera pose and the landmark location and * This function receives the camera pose and the landmark location and
* returns the location the point is supposed to appear in the image * returns the location the point is supposed to appear in the image
* @param point a 3D point to be projected * @param point a 3D point to be projected
* @param D_intrinsic_pose the optionally computed Jacobian with respect to pose * @param Dpose the optionally computed Jacobian with respect to pose
* @param D_intrinsic_point the optionally computed Jacobian with respect to the 3D point * @param Dpoint the optionally computed Jacobian with respect to the 3D point
* @return the intrinsic coordinates of the projected point * @return the intrinsic coordinates of the projected point
*/ */
Point2 project(const Point3& point, Point2 project(const Point3& point,
boost::optional<Matrix&> D_intrinsic_pose = boost::none, boost::optional<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> D_intrinsic_point = boost::none) const; boost::optional<Matrix&> Dpoint = boost::none) const;
/** /**
* projects a 3-dimensional point in camera coordinates into the * projects a 3-dimensional point in camera coordinates into the
@ -165,10 +175,10 @@ namespace gtsam {
* @param H2 optionally computed Jacobian with respect to the 3D point * @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double) * @return range (double)
*/ */
double range(const Point3& point, double range(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) const {
return pose_.range(point, H1, H2); } return pose_.range(point, H1, H2);
}
/** /**
* Calculate range to another pose * Calculate range to another pose
@ -177,10 +187,10 @@ namespace gtsam {
* @param H2 optionally computed Jacobian with respect to the 3D point * @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double) * @return range (double)
*/ */
double range(const Pose3& pose, double range(const Pose3& pose, 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) const {
return pose_.range(pose, H1, H2); } return pose_.range(pose, H1, H2);
}
/** /**
* Calculate range to another camera * Calculate range to another camera
@ -189,10 +199,10 @@ namespace gtsam {
* @param H2 optionally computed Jacobian with respect to the 3D point * @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double) * @return range (double)
*/ */
double range(const CalibratedCamera& camera, double range(const CalibratedCamera& camera, boost::optional<Matrix&> H1 =
boost::optional<Matrix&> H1=boost::none, boost::none, boost::optional<Matrix&> H2 = boost::none) const {
boost::optional<Matrix&> H2=boost::none) const { return pose_.range(camera.pose_, H1, H2);
return pose_.range(camera.pose_, H1, H2); } }
private: private:
@ -204,12 +214,12 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) { void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("CalibratedCamera", ar
& boost::serialization::make_nvp("CalibratedCamera",
boost::serialization::base_object<Value>(*this)); boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(pose_); ar & BOOST_SERIALIZATION_NVP(pose_);
} }
/// @} /// @}
}; };}
}

37
gtsam/geometry/tests/timeCalibratedCamera.cpp
View File

@ -37,43 +37,6 @@ int main()
const CalibratedCamera camera(pose1); const CalibratedCamera camera(pose1);
const Point3 point1(-0.08,-0.08, 0.0); const Point3 point1(-0.08,-0.08, 0.0);
/**
* NOTE: because we only have combined derivative functions now,
* parts of this test are no longer useful.
*/
// Aug 8, iMac 3.06GHz Core i3
// 378870 calls/second
// 2.63943 musecs/call
// AFTER collapse:
// 1.57399e+06 calls/second
// 0.63533 musecs/call
// {
// Matrix computed;
// long timeLog = clock();
// for(int i = 0; i < n; i++)
// computed = Dproject_pose(camera, point1);
// long timeLog2 = clock();
// double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
// cout << ((double)n/seconds) << " calls/second" << endl;
// cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
// }
// Aug 8, iMac 3.06GHz Core i3
// AFTER collapse:
// 1.55383e+06 calls/second
// 0.64357 musecs/call
// {
// Matrix computed;
// long timeLog = clock();
// for(int i = 0; i < n; i++)
// computed = Dproject_point(camera, point1);
// long timeLog2 = clock();
// double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
// cout << ((double)n/seconds) << " calls/second" << endl;
// cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
// }
// Aug 8, iMac 3.06GHz Core i3 // Aug 8, iMac 3.06GHz Core i3
// 371153 calls/second // 371153 calls/second
// 2.69431 musecs/call // 2.69431 musecs/call