84 lines
2.3 KiB
C++
84 lines
2.3 KiB
C++
/*
|
|
* SimpleCamera.h
|
|
*
|
|
* Created on: Aug 16, 2009
|
|
* Author: dellaert
|
|
*/
|
|
|
|
#ifndef SIMPLECAMERA_H_
|
|
#define SIMPLECAMERA_H_
|
|
|
|
#include "CalibratedCamera.h"
|
|
#include "Cal3_S2.h"
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* A simple camera class with a Cal3_S2 calibration
|
|
* Basically takes a Calibrated camera and adds calibration information
|
|
* to produce measurements in pixels.
|
|
* Not a sublass as a SimpleCamera *is not* a CalibratedCamera.
|
|
*/
|
|
class SimpleCamera {
|
|
private:
|
|
CalibratedCamera calibrated_; // Calibrated camera
|
|
Cal3_S2 K_; // Calibration
|
|
|
|
public:
|
|
SimpleCamera(const Cal3_S2& K, const CalibratedCamera& calibrated);
|
|
SimpleCamera(const Cal3_S2& K, const Pose3& pose);
|
|
virtual ~SimpleCamera();
|
|
|
|
const Pose3& pose() const {
|
|
return calibrated_.pose();
|
|
}
|
|
|
|
const Cal3_S2& calibration() const {
|
|
return K_;
|
|
}
|
|
|
|
Point2 project(const Point3& P) const;
|
|
|
|
/**
|
|
* Create a level camera at the given 2D pose and height
|
|
*/
|
|
static SimpleCamera level(const Cal3_S2& K, const Pose2& pose2, double height);
|
|
|
|
// Friends
|
|
friend Matrix Dproject_pose(const SimpleCamera& camera, const Point3& point);
|
|
friend Matrix Dproject_point(const SimpleCamera& camera, const Point3& point);
|
|
friend void Dproject_pose_point(const SimpleCamera& camera, const Point3& point,
|
|
Point2& projection, Matrix& D_projection_pose,
|
|
Matrix& D_projection_point);
|
|
|
|
};
|
|
|
|
/* ************************************************************************* */
|
|
// 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 SimpleCamera& camera, const Point3& point);
|
|
|
|
/**
|
|
* Derivatives of project.
|
|
*/
|
|
Matrix Dproject_pose(const SimpleCamera& camera, const Point3& point);
|
|
Matrix Dproject_point(const SimpleCamera& 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 SimpleCamera& camera, const Point3& point,
|
|
Point2& projection, Matrix& D_projection_pose,
|
|
Matrix& D_projection_point);
|
|
|
|
}
|
|
|
|
#endif /* SIMPLECAMERA_H_ */
|