73 lines
1.9 KiB
C
73 lines
1.9 KiB
C
|
/*
|
||
|
|
* CalibratedCamera.h
|
||
|
|
*
|
||
|
|
* Created on: Aug 17, 2009
|
||
|
|
* Author: dellaert
|
||
|
|
*/
|
||
|
|
|
||
|
|
#ifndef CalibratedCAMERA_H_
|
||
|
|
#define CalibratedCAMERA_H_
|
||
|
|
|
||
|
|
#include "Point2.h"
|
||
|
|
#include "Pose3.h"
|
||
|
|
|
||
|
|
namespace gtsam {
|
||
|
|
|
||
|
|
/**
|
||
|
|
* 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 <--*/
|
||
|
|
|
||
|
|
/**
|
||
|
|
* 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);
|
||
|
|
virtual ~CalibratedCamera();
|
||
|
|
|
||
|
|
const Pose3& pose() const {
|
||
|
|
return pose_;
|
||
|
|
}
|
||
|
|
|
||
|
|
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
|
||
|
|
*/
|
||
|
|
Point2 project(const CalibratedCamera& camera, const Point3& point);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Derivatives of 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
|
||
|
|
*/
|
||
|
|
void
|
||
|
|
Dproject_pose_point(const CalibratedCamera& camera, const Point3& point,
|
||
|
|
Point2& intrinsic, Matrix& D_intrinsic_pose, Matrix& D_intrinsic_point);
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif /* CalibratedCAMERA_H_ */
|