102 lines
2.9 KiB
C++
102 lines
2.9 KiB
C++
/*
|
|
* CalibratedCamera.h
|
|
*
|
|
* Created on: Aug 17, 2009
|
|
* Author: dellaert
|
|
*/
|
|
|
|
#ifndef CalibratedCAMERA_H_
|
|
#define CalibratedCAMERA_H_
|
|
|
|
#include "Pose2.h"
|
|
#include "Pose3.h"
|
|
|
|
namespace gtsam {
|
|
|
|
class Point2;
|
|
|
|
/**
|
|
* projects a 3-dimensional point in camera coordinates into the
|
|
* camera and returns a 2-dimensional point, no calibration applied
|
|
*/
|
|
Point2 project_to_camera(const Point3& cameraPoint);
|
|
|
|
/**
|
|
* Derivative of project_to_camera
|
|
*/
|
|
Matrix Dproject_to_camera1(const Point3& cameraPoint); /*2by3 <--*/
|
|
|
|
/**
|
|
* backproject a 2-dimensional point to a 3-dimension point
|
|
*/
|
|
Point3 backproject_from_camera(const Point2& p, const double scale);
|
|
|
|
/**
|
|
* A Calibrated camera class [R|-R't], calibration K=I.
|
|
* If calibration is known, it is more computationally efficient
|
|
* to calibrate the measurements rather than try to predict in pixels.
|
|
*/
|
|
class CalibratedCamera {
|
|
private:
|
|
Pose3 pose_; // 6DOF pose
|
|
|
|
public:
|
|
CalibratedCamera(const Pose3& pose);
|
|
CalibratedCamera(const Vector &v) ;
|
|
virtual ~CalibratedCamera();
|
|
|
|
inline const Pose3& pose() const { return pose_; }
|
|
bool equals (const CalibratedCamera &camera, double tol = 1e-9) const {
|
|
return pose_.equals(camera.pose(), tol) ;
|
|
}
|
|
|
|
inline const CalibratedCamera compose(const CalibratedCamera &c) const {
|
|
return CalibratedCamera( pose_ * c.pose() ) ;
|
|
}
|
|
|
|
inline const CalibratedCamera inverse() const {
|
|
return CalibratedCamera( pose_.inverse() ) ;
|
|
}
|
|
|
|
inline static size_t dim() { return 6 ; }
|
|
|
|
/**
|
|
* Create a level camera at the given 2D pose and height
|
|
* @param pose2 specifies the location and viewing direction
|
|
* (theta 0 = looking in direction of positive X axis)
|
|
*/
|
|
static CalibratedCamera level(const Pose2& pose2, double height);
|
|
|
|
Point2 project(const Point3& P) const;
|
|
};
|
|
|
|
/* ************************************************************************* */
|
|
// measurement functions and derivatives
|
|
/* ************************************************************************* */
|
|
|
|
/**
|
|
* This function receives the camera pose and the landmark location and
|
|
* returns the location the point is supposed to appear in the image
|
|
* @param camera the CalibratedCamera
|
|
* @param point a 3D point to be projected
|
|
* @return the intrinsic coordinates of the projected point
|
|
*/
|
|
Point2 project(const CalibratedCamera& camera, const Point3& point);
|
|
|
|
/**
|
|
* Derivatives of project, same paramaters as project
|
|
*/
|
|
Matrix Dproject_pose(const CalibratedCamera& camera, const Point3& point);
|
|
Matrix Dproject_point(const CalibratedCamera& camera, const Point3& point);
|
|
|
|
/**
|
|
* super-duper combined evaluation + derivatives
|
|
* saves a lot of time because a lot of computation is shared
|
|
* @return project(camera,point)
|
|
*/
|
|
Point2 Dproject_pose_point(const CalibratedCamera& camera,
|
|
const Point3& point, Matrix& D_intrinsic_pose, Matrix& D_intrinsic_point);
|
|
}
|
|
|
|
#endif /* CalibratedCAMERA_H_ */
|