CalibratedCamera now derived from PinholeBase
dellaert
parent
7d37aa4512
commit
f5581ec652
|
|
@ -55,33 +55,13 @@ Point2 PinholeBase::project_to_camera(const Point3& P,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
CalibratedCamera::CalibratedCamera(const Pose3& pose) :
|
||||
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,
|
||||
Point3 PinholeBase::backproject_from_camera(const Point2& p,
|
||||
const double 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 Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
|
||||
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) {
|
||||
return CalibratedCamera(LevelPose(pose2, height));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 CalibratedCamera::LookatPose(const Point3& eye, const Point3& target,
|
||||
Pose3 PinholeBase::LookatPose(const Point3& eye, const Point3& target,
|
||||
const Point3& upVector) {
|
||||
Point3 zc = target - eye;
|
||||
zc = zc / zc.norm();
|
||||
|
|
@ -105,6 +80,11 @@ Pose3 CalibratedCamera::LookatPose(const Point3& eye, const Point3& target,
|
|||
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,
|
||||
const Point3& target, const Point3& upVector) {
|
||||
|
|
@ -120,7 +100,7 @@ Point2 CalibratedCamera::project(const Point3& point,
|
|||
Matrix3 Dpoint_;
|
||||
Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0);
|
||||
#else
|
||||
Point3 q = pose_.transform_to(point);
|
||||
Point3 q = pose().transform_to(point);
|
||||
#endif
|
||||
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
|
||||
* v;
|
||||
if (Dpoint) {
|
||||
const Matrix3 R(pose_.rotation().matrix());
|
||||
const Matrix3 R(pose().rotation().matrix());
|
||||
Matrix23 Dpoint_;
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -44,6 +44,30 @@ private:
|
|||
|
||||
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
|
||||
/// @{
|
||||
|
||||
|
|
@ -102,6 +126,11 @@ public:
|
|||
static Point2 project_to_camera(const Point3& P, //
|
||||
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
|
||||
* @param point 3D location of landmark
|
||||
|
|
@ -195,9 +224,7 @@ private:
|
|||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT CalibratedCamera {
|
||||
private:
|
||||
Pose3 pose_; // 6DOF pose
|
||||
class GTSAM_EXPORT CalibratedCamera: public PinholeBase {
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -213,21 +240,14 @@ public:
|
|||
}
|
||||
|
||||
/// construct with pose
|
||||
explicit CalibratedCamera(const Pose3& pose);
|
||||
explicit CalibratedCamera(const Pose3& pose) :
|
||||
PinholeBase(pose) {
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @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
|
||||
* @param pose2 specifies the location and viewing direction
|
||||
|
|
@ -236,17 +256,6 @@ public:
|
|||
*/
|
||||
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
|
||||
* with the specified up direction vector.
|
||||
|
|
@ -263,19 +272,8 @@ public:
|
|||
/// @{
|
||||
|
||||
/// construct from vector
|
||||
explicit CalibratedCamera(const Vector &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);
|
||||
explicit CalibratedCamera(const Vector &v) :
|
||||
PinholeBase(v) {
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
|
@ -286,11 +284,6 @@ public:
|
|||
virtual ~CalibratedCamera() {
|
||||
}
|
||||
|
||||
/// return pose
|
||||
inline const Pose3& pose() const {
|
||||
return pose_;
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
|
@ -311,10 +304,6 @@ public:
|
|||
return 6;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// measurement functions and derivatives
|
||||
/* ************************************************************************* */
|
||||
|
||||
/// @}
|
||||
/// @name Transformations and mesaurement functions
|
||||
/// @{
|
||||
|
|
@ -330,43 +319,6 @@ public:
|
|||
OptionalJacobian<2, 6> Dpose = boost::none,
|
||||
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
|
||||
* @param camera Other camera
|
||||
|
|
@ -376,12 +328,13 @@ public:
|
|||
*/
|
||||
double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> H1 =
|
||||
boost::none, OptionalJacobian<1, 6> H2 = boost::none) const {
|
||||
return pose_.range(camera.pose_, H1, H2);
|
||||
return pose().range(camera.pose(), H1, H2);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
|
|
@ -389,7 +342,9 @@ private:
|
|||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
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));
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
|
|
|||
Loading…
Reference in New Issue