CalibratedCamera now derived from PinholeBase
parent
7d37aa4512
commit
f5581ec652
|
@ -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)
|
||||||
|
|
|
@ -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));
|
||||||
}
|
}
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
|
|
Loading…
Reference in New Issue