CalibratedCamera now derived from PinholeBase

release/4.3a0
dellaert 2015-02-21 09:13:12 +01:00
parent 7d37aa4512
commit f5581ec652
2 changed files with 51 additions and 116 deletions

View File

@ -55,33 +55,13 @@ Point2 PinholeBase::project_to_camera(const Point3& P,
} }
/* ************************************************************************* */ /* ************************************************************************* */
CalibratedCamera::CalibratedCamera(const Pose3& pose) : Point3 PinholeBase::backproject_from_camera(const Point2& p,
pose_(pose) {
}
/* ************************************************************************* */
CalibratedCamera::CalibratedCamera(const Vector &v) :
pose_(Pose3::Expmap(v)) {
}
/* ************************************************************************* */
Point2 CalibratedCamera::project_to_camera(const Point3& P,
OptionalJacobian<2, 3> H1) {
if (H1) {
double d = 1.0 / P.z(), d2 = d * d;
*H1 << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2;
}
return Point2(P.x() / P.z(), P.y() / P.z());
}
/* ************************************************************************* */
Point3 CalibratedCamera::backproject_from_camera(const Point2& p,
const double scale) { const double scale) {
return Point3(p.x() * scale, p.y() * scale, scale); return Point3(p.x() * scale, p.y() * scale, scale);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 CalibratedCamera::LevelPose(const Pose2& pose2, double height) { Pose3 PinholeBase::LevelPose(const Pose2& pose2, double height) {
const double st = sin(pose2.theta()), ct = cos(pose2.theta()); const double st = sin(pose2.theta()), ct = cos(pose2.theta());
const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
const Rot3 wRc(x, y, z); const Rot3 wRc(x, y, z);
@ -90,12 +70,7 @@ Pose3 CalibratedCamera::LevelPose(const Pose2& pose2, double height) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) { Pose3 PinholeBase::LookatPose(const Point3& eye, const Point3& target,
return CalibratedCamera(LevelPose(pose2, height));
}
/* ************************************************************************* */
Pose3 CalibratedCamera::LookatPose(const Point3& eye, const Point3& target,
const Point3& upVector) { const Point3& upVector) {
Point3 zc = target - eye; Point3 zc = target - eye;
zc = zc / zc.norm(); zc = zc / zc.norm();
@ -105,6 +80,11 @@ Pose3 CalibratedCamera::LookatPose(const Point3& eye, const Point3& target,
return Pose3(Rot3(xc, yc, zc), eye); return Pose3(Rot3(xc, yc, zc), eye);
} }
/* ************************************************************************* */
CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
return CalibratedCamera(LevelPose(pose2, height));
}
/* ************************************************************************* */ /* ************************************************************************* */
CalibratedCamera CalibratedCamera::Lookat(const Point3& eye, CalibratedCamera CalibratedCamera::Lookat(const Point3& eye,
const Point3& target, const Point3& upVector) { const Point3& target, const Point3& upVector) {
@ -120,7 +100,7 @@ Point2 CalibratedCamera::project(const Point3& point,
Matrix3 Dpoint_; Matrix3 Dpoint_;
Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0); Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0);
#else #else
Point3 q = pose_.transform_to(point); Point3 q = pose().transform_to(point);
#endif #endif
Point2 intrinsic = project_to_camera(q); Point2 intrinsic = project_to_camera(q);
@ -145,7 +125,7 @@ Point2 CalibratedCamera::project(const Point3& point,
*Dpose << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), -uv, -u, 0., -d, d *Dpose << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), -uv, -u, 0., -d, d
* v; * v;
if (Dpoint) { if (Dpoint) {
const Matrix3 R(pose_.rotation().matrix()); const Matrix3 R(pose().rotation().matrix());
Matrix23 Dpoint_; Matrix23 Dpoint_;
Dpoint_ << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) Dpoint_ << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 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) - u * R(2, 2), R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1)

View File

@ -44,6 +44,30 @@ private:
public: public:
/// @name Static functions
/// @{
/**
* Create a level pose at the given 2D pose and height
* @param K the calibration
* @param pose2 specifies the location and viewing direction
* (theta 0 = looking in direction of positive X axis)
* @param height camera height
*/
static Pose3 LevelPose(const Pose2& pose2, double height);
/**
* Create a camera pose at the given eye position looking at a target point in the scene
* with the specified up direction vector.
* @param eye specifies the camera position
* @param target the point to look at
* @param upVector specifies the camera up direction vector,
* doesn't need to be on the image plane nor orthogonal to the viewing axis
*/
static Pose3 LookatPose(const Point3& eye, const Point3& target,
const Point3& upVector);
/// @}
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -102,6 +126,11 @@ public:
static Point2 project_to_camera(const Point3& P, // static Point2 project_to_camera(const Point3& P, //
OptionalJacobian<2, 3> Dpoint = boost::none); OptionalJacobian<2, 3> Dpoint = boost::none);
/**
* backproject a 2-dimensional point to a 3-dimension point
*/
static Point3 backproject_from_camera(const Point2& p, const double scale);
/** /**
* Calculate range to a landmark * Calculate range to a landmark
* @param point 3D location of landmark * @param point 3D location of landmark
@ -195,9 +224,7 @@ private:
* @addtogroup geometry * @addtogroup geometry
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT CalibratedCamera { class GTSAM_EXPORT CalibratedCamera: public PinholeBase {
private:
Pose3 pose_; // 6DOF pose
public: public:
@ -213,21 +240,14 @@ public:
} }
/// construct with pose /// construct with pose
explicit CalibratedCamera(const Pose3& pose); explicit CalibratedCamera(const Pose3& pose) :
PinholeBase(pose) {
}
/// @} /// @}
/// @name Named Constructors /// @name Named Constructors
/// @{ /// @{
/**
* Create a level pose at the given 2D pose and height
* @param K the calibration
* @param pose2 specifies the location and viewing direction
* (theta 0 = looking in direction of positive X axis)
* @param height camera height
*/
static Pose3 LevelPose(const Pose2& pose2, double height);
/** /**
* Create a level camera at the given 2D pose and height * Create a level camera at the given 2D pose and height
* @param pose2 specifies the location and viewing direction * @param pose2 specifies the location and viewing direction
@ -236,17 +256,6 @@ public:
*/ */
static CalibratedCamera Level(const Pose2& pose2, double height); static CalibratedCamera Level(const Pose2& pose2, double height);
/**
* Create a camera pose at the given eye position looking at a target point in the scene
* with the specified up direction vector.
* @param eye specifies the camera position
* @param target the point to look at
* @param upVector specifies the camera up direction vector,
* doesn't need to be on the image plane nor orthogonal to the viewing axis
*/
static Pose3 LookatPose(const Point3& eye, const Point3& target,
const Point3& upVector);
/** /**
* Create a camera at the given eye position looking at a target point in the scene * Create a camera at the given eye position looking at a target point in the scene
* with the specified up direction vector. * with the specified up direction vector.
@ -263,19 +272,8 @@ public:
/// @{ /// @{
/// construct from vector /// construct from vector
explicit CalibratedCamera(const Vector &v); explicit CalibratedCamera(const Vector &v) :
PinholeBase(v) {
/// @}
/// @name Testable
/// @{
virtual void print(const std::string& s = "") const {
pose_.print(s);
}
/// check equality to another camera
bool equals(const CalibratedCamera &camera, double tol = 1e-9) const {
return pose_.equals(camera.pose(), tol);
} }
/// @} /// @}
@ -286,11 +284,6 @@ public:
virtual ~CalibratedCamera() { virtual ~CalibratedCamera() {
} }
/// return pose
inline const Pose3& pose() const {
return pose_;
}
/// @} /// @}
/// @name Manifold /// @name Manifold
/// @{ /// @{
@ -311,10 +304,6 @@ public:
return 6; return 6;
} }
/* ************************************************************************* */
// measurement functions and derivatives
/* ************************************************************************* */
/// @} /// @}
/// @name Transformations and mesaurement functions /// @name Transformations and mesaurement functions
/// @{ /// @{
@ -330,43 +319,6 @@ public:
OptionalJacobian<2, 6> Dpose = boost::none, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const; OptionalJacobian<2, 3> Dpoint = boost::none) const;
/**
* projects a 3-dimensional point in camera coordinates into the
* camera and returns a 2-dimensional point, no calibration applied
* With optional 2by3 derivative
*/
static Point2 project_to_camera(const Point3& cameraPoint,
OptionalJacobian<2, 3> H1 = boost::none);
/**
* backproject a 2-dimensional point to a 3-dimension point
*/
static Point3 backproject_from_camera(const Point2& p, const double scale);
/**
* Calculate range to a landmark
* @param point 3D location of landmark
* @param H1 optionally computed Jacobian with respect to pose
* @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double)
*/
double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) const {
return pose_.range(point, H1, H2);
}
/**
* Calculate range to another pose
* @param pose Other SO(3) pose
* @param H1 optionally computed Jacobian with respect to pose
* @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double)
*/
double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) const {
return pose_.range(pose, H1, H2);
}
/** /**
* Calculate range to another camera * Calculate range to another camera
* @param camera Other camera * @param camera Other camera
@ -376,12 +328,13 @@ public:
*/ */
double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> H1 = double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> H1 =
boost::none, OptionalJacobian<1, 6> H2 = boost::none) const { boost::none, OptionalJacobian<1, 6> H2 = boost::none) const {
return pose_.range(camera.pose_, H1, H2); return pose().range(camera.pose(), H1, H2);
} }
/// @}
private: private:
/// @}
/// @name Advanced Interface /// @name Advanced Interface
/// @{ /// @{
@ -389,7 +342,9 @@ 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_NVP(pose_); ar
& boost::serialization::make_nvp("PinholeBase",
boost::serialization::base_object<PinholeBase>(*this));
} }
/// @} /// @}