Reformatted with new style file, renamed some derivatives to Dcal, Dpose, Dpoint etc.
Frank Dellaert
parent
b161a106c7
commit
1a20272fa2
|
|
@ -23,26 +23,27 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
CalibratedCamera::CalibratedCamera(const Pose3& pose) :
|
||||
pose_(pose) {
|
||||
pose_(pose) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
CalibratedCamera::CalibratedCamera(const Vector &v) : pose_(Pose3::Expmap(v)) {}
|
||||
CalibratedCamera::CalibratedCamera(const Vector &v) :
|
||||
pose_(Pose3::Expmap(v)) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 CalibratedCamera::project_to_camera(const Point3& P,
|
||||
boost::optional<Matrix&> H1) {
|
||||
if (H1) {
|
||||
double d = 1.0 / P.z(), d2 = d * d;
|
||||
*H1 = Matrix_(2, 3,
|
||||
d, 0.0, -P.x() * d2,
|
||||
0.0, d, -P.y() * d2);
|
||||
*H1 = Matrix_(2, 3, 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) {
|
||||
Point3 CalibratedCamera::backproject_from_camera(const Point2& p,
|
||||
const double scale) {
|
||||
return Point3(p.x() * scale, p.y() * scale, scale);
|
||||
}
|
||||
|
||||
|
|
@ -58,41 +59,39 @@ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Point2 CalibratedCamera::project(const Point3& point,
|
||||
boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) const {
|
||||
|
||||
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
|
||||
Point3 q = pose_.transform_to(point, H1, H2);
|
||||
Point3 q = pose_.transform_to(point, Dpose, Dpoint);
|
||||
#else
|
||||
Point3 q = pose_.transform_to(point);
|
||||
#endif
|
||||
Point2 intrinsic = project_to_camera(q);
|
||||
|
||||
// Check if point is in front of camera
|
||||
if(q.z() <= 0)
|
||||
if (q.z() <= 0)
|
||||
throw CheiralityException();
|
||||
|
||||
if (H1 || H2) {
|
||||
if (Dpose || Dpoint) {
|
||||
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
|
||||
// just implement chain rule
|
||||
Matrix H;
|
||||
project_to_camera(q,H);
|
||||
if (H1) *H1 = H * (*H1);
|
||||
if (H2) *H2 = H * (*H2);
|
||||
if (Dpose) *Dpose = H * (*Dpose);
|
||||
if (Dpoint) *Dpoint = H * (*Dpoint);
|
||||
#else
|
||||
// optimized version, see CalibratedCamera.nb
|
||||
const double z = q.z(), d = 1.0/z;
|
||||
const double u = intrinsic.x(), v = intrinsic.y(), uv = u*v;
|
||||
if (H1) *H1 = Matrix_(2,6,
|
||||
uv,-(1.+u*u), v, -d , 0., d*u,
|
||||
(1.+v*v), -uv,-u, 0.,-d , d*v
|
||||
);
|
||||
if (H2) {
|
||||
const double z = q.z(), d = 1.0 / z;
|
||||
const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v;
|
||||
if (Dpose)
|
||||
*Dpose = Matrix_(2, 6, uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
|
||||
-uv, -u, 0., -d, d * v);
|
||||
if (Dpoint) {
|
||||
const Matrix R(pose_.rotation().matrix());
|
||||
*H2 = d * Matrix_(2,3,
|
||||
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) - v*R(2,2)
|
||||
);
|
||||
*Dpoint = d
|
||||
* Matrix_(2, 3, 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) - v * R(2, 2));
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
|
@ -101,12 +100,12 @@ Point2 CalibratedCamera::project(const Point3& point,
|
|||
|
||||
/* ************************************************************************* */
|
||||
CalibratedCamera CalibratedCamera::retract(const Vector& d) const {
|
||||
return CalibratedCamera(pose().retract(d)) ;
|
||||
return CalibratedCamera(pose().retract(d));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector CalibratedCamera::localCoordinates(const CalibratedCamera& T2) const {
|
||||
return pose().localCoordinates(T2.pose()) ;
|
||||
return pose().localCoordinates(T2.pose());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -24,192 +24,202 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
class GTSAM_EXPORT CheiralityException: public ThreadsafeException<CheiralityException> {
|
||||
public:
|
||||
CheiralityException() : ThreadsafeException<CheiralityException>("Cheirality Exception") {}
|
||||
};
|
||||
class GTSAM_EXPORT CheiralityException: public ThreadsafeException<
|
||||
CheiralityException> {
|
||||
public:
|
||||
CheiralityException() :
|
||||
ThreadsafeException<CheiralityException>("Cheirality Exception") {
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* 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.
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT CalibratedCamera: public DerivedValue<CalibratedCamera> {
|
||||
private:
|
||||
Pose3 pose_; // 6DOF pose
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/// default constructor
|
||||
CalibratedCamera() {
|
||||
}
|
||||
|
||||
/// construct with pose
|
||||
explicit CalibratedCamera(const Pose3& pose);
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
/// 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);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// destructor
|
||||
virtual ~CalibratedCamera() {
|
||||
}
|
||||
|
||||
/// return pose
|
||||
inline const Pose3& pose() const {
|
||||
return pose_;
|
||||
}
|
||||
|
||||
/// compose the two camera poses: TODO Frank says this might not make sense
|
||||
inline const CalibratedCamera compose(const CalibratedCamera &c,
|
||||
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
|
||||
boost::none) const {
|
||||
return CalibratedCamera(pose_.compose(c.pose(), H1, H2));
|
||||
}
|
||||
|
||||
/// between the two camera poses: TODO Frank says this might not make sense
|
||||
inline const CalibratedCamera between(const CalibratedCamera& c,
|
||||
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
|
||||
boost::none) const {
|
||||
return CalibratedCamera(pose_.between(c.pose(), H1, H2));
|
||||
}
|
||||
|
||||
/// invert the camera pose: TODO Frank says this might not make sense
|
||||
inline const CalibratedCamera inverse(boost::optional<Matrix&> H1 =
|
||||
boost::none) const {
|
||||
return CalibratedCamera(pose_.inverse(H1));
|
||||
}
|
||||
|
||||
/**
|
||||
* 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.
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
* Create a level camera at the given 2D pose and height
|
||||
* @param pose2 specifies the location and viewing direction
|
||||
* @param height specifies the height of the camera (along the positive Z-axis)
|
||||
* (theta 0 = looking in direction of positive X axis)
|
||||
*/
|
||||
class GTSAM_EXPORT CalibratedCamera : public DerivedValue<CalibratedCamera> {
|
||||
private:
|
||||
Pose3 pose_; // 6DOF pose
|
||||
static CalibratedCamera Level(const Pose2& pose2, double height);
|
||||
|
||||
public:
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
/// move a cameras pose according to d
|
||||
CalibratedCamera retract(const Vector& d) const;
|
||||
|
||||
/// default constructor
|
||||
CalibratedCamera() {}
|
||||
/// Return canonical coordinate
|
||||
Vector localCoordinates(const CalibratedCamera& T2) const;
|
||||
|
||||
/// construct with pose
|
||||
explicit CalibratedCamera(const Pose3& pose);
|
||||
/// Lie group dimensionality
|
||||
inline size_t dim() const {
|
||||
return 6;
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
/// Lie group dimensionality
|
||||
inline static size_t Dim() {
|
||||
return 6;
|
||||
}
|
||||
|
||||
/// construct from vector
|
||||
explicit CalibratedCamera(const Vector &v);
|
||||
/* ************************************************************************* */
|
||||
// measurement functions and derivatives
|
||||
/* ************************************************************************* */
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
/// @}
|
||||
/// @name Transformations and mesaurement functions
|
||||
/// @{
|
||||
/**
|
||||
* This function receives the camera pose and the landmark location and
|
||||
* returns the location the point is supposed to appear in the image
|
||||
* @param point a 3D point to be projected
|
||||
* @param Dpose the optionally computed Jacobian with respect to pose
|
||||
* @param Dpoint the optionally computed Jacobian with respect to the 3D point
|
||||
* @return the intrinsic coordinates of the projected point
|
||||
*/
|
||||
Point2 project(const Point3& point,
|
||||
boost::optional<Matrix&> Dpose = boost::none,
|
||||
boost::optional<Matrix&> Dpoint = boost::none) const;
|
||||
|
||||
virtual void print(const std::string& s = "") const {
|
||||
pose_.print(s);
|
||||
}
|
||||
/**
|
||||
* 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,
|
||||
boost::optional<Matrix&> H1 = boost::none);
|
||||
|
||||
/// check equality to another camera
|
||||
bool equals (const CalibratedCamera &camera, double tol = 1e-9) const {
|
||||
return pose_.equals(camera.pose(), tol) ;
|
||||
}
|
||||
/**
|
||||
* backproject a 2-dimensional point to a 3-dimension point
|
||||
*/
|
||||
static Point3 backproject_from_camera(const Point2& p, const double scale);
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
/**
|
||||
* 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, boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const {
|
||||
return pose_.range(point, H1, H2);
|
||||
}
|
||||
|
||||
/// destructor
|
||||
virtual ~CalibratedCamera() {}
|
||||
/**
|
||||
* 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, boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const {
|
||||
return pose_.range(pose, H1, H2);
|
||||
}
|
||||
|
||||
/// return pose
|
||||
inline const Pose3& pose() const { return pose_; }
|
||||
|
||||
/// compose the two camera poses: TODO Frank says this might not make sense
|
||||
inline const CalibratedCamera compose(const CalibratedCamera &c,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
return CalibratedCamera( pose_.compose(c.pose(), H1, H2) );
|
||||
}
|
||||
|
||||
/// between the two camera poses: TODO Frank says this might not make sense
|
||||
inline const CalibratedCamera between(const CalibratedCamera& c,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
return CalibratedCamera( pose_.between(c.pose(), H1, H2) );
|
||||
}
|
||||
|
||||
/// invert the camera pose: TODO Frank says this might not make sense
|
||||
inline const CalibratedCamera inverse(boost::optional<Matrix&> H1=boost::none) const {
|
||||
return CalibratedCamera( pose_.inverse(H1) );
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a level camera at the given 2D pose and height
|
||||
* @param pose2 specifies the location and viewing direction
|
||||
* @param height specifies the height of the camera (along the positive Z-axis)
|
||||
* (theta 0 = looking in direction of positive X axis)
|
||||
*/
|
||||
static CalibratedCamera Level(const Pose2& pose2, double height);
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// move a cameras pose according to d
|
||||
CalibratedCamera retract(const Vector& d) const;
|
||||
|
||||
/// Return canonical coordinate
|
||||
Vector localCoordinates(const CalibratedCamera& T2) const;
|
||||
|
||||
/// Lie group dimensionality
|
||||
inline size_t dim() const { return 6 ; }
|
||||
|
||||
/// Lie group dimensionality
|
||||
inline static size_t Dim() { return 6 ; }
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
// measurement functions and derivatives
|
||||
/* ************************************************************************* */
|
||||
|
||||
/// @}
|
||||
/// @name Transformations and mesaurement functions
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* This function receives the camera pose and the landmark location and
|
||||
* returns the location the point is supposed to appear in the image
|
||||
* @param point a 3D point to be projected
|
||||
* @param D_intrinsic_pose the optionally computed Jacobian with respect to pose
|
||||
* @param D_intrinsic_point the optionally computed Jacobian with respect to the 3D point
|
||||
* @return the intrinsic coordinates of the projected point
|
||||
*/
|
||||
Point2 project(const Point3& point,
|
||||
boost::optional<Matrix&> D_intrinsic_pose = boost::none,
|
||||
boost::optional<Matrix&> D_intrinsic_point = 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,
|
||||
boost::optional<Matrix&> 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,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> 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,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
return pose_.range(pose, H1, H2); }
|
||||
|
||||
/**
|
||||
* Calculate range to another camera
|
||||
* @param camera Other camera
|
||||
* @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 CalibratedCamera& camera,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
return pose_.range(camera.pose_, H1, H2); }
|
||||
/**
|
||||
* Calculate range to another camera
|
||||
* @param camera Other camera
|
||||
* @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 CalibratedCamera& camera, boost::optional<Matrix&> H1 =
|
||||
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
|
||||
return pose_.range(camera.pose_, H1, H2);
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & boost::serialization::make_nvp("CalibratedCamera",
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar
|
||||
& boost::serialization::make_nvp("CalibratedCamera",
|
||||
boost::serialization::base_object<Value>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(pose_);
|
||||
}
|
||||
ar & BOOST_SERIALIZATION_NVP(pose_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
};
|
||||
}
|
||||
/// @}
|
||||
};}
|
||||
|
||||
|
|
|
|||
|
|
@ -37,43 +37,6 @@ int main()
|
|||
const CalibratedCamera camera(pose1);
|
||||
const Point3 point1(-0.08,-0.08, 0.0);
|
||||
|
||||
/**
|
||||
* NOTE: because we only have combined derivative functions now,
|
||||
* parts of this test are no longer useful.
|
||||
*/
|
||||
|
||||
// Aug 8, iMac 3.06GHz Core i3
|
||||
// 378870 calls/second
|
||||
// 2.63943 musecs/call
|
||||
// AFTER collapse:
|
||||
// 1.57399e+06 calls/second
|
||||
// 0.63533 musecs/call
|
||||
// {
|
||||
// Matrix computed;
|
||||
// long timeLog = clock();
|
||||
// for(int i = 0; i < n; i++)
|
||||
// computed = Dproject_pose(camera, point1);
|
||||
// long timeLog2 = clock();
|
||||
// double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
|
||||
// cout << ((double)n/seconds) << " calls/second" << endl;
|
||||
// cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
|
||||
// }
|
||||
|
||||
// Aug 8, iMac 3.06GHz Core i3
|
||||
// AFTER collapse:
|
||||
// 1.55383e+06 calls/second
|
||||
// 0.64357 musecs/call
|
||||
// {
|
||||
// Matrix computed;
|
||||
// long timeLog = clock();
|
||||
// for(int i = 0; i < n; i++)
|
||||
// computed = Dproject_point(camera, point1);
|
||||
// long timeLog2 = clock();
|
||||
// double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
|
||||
// cout << ((double)n/seconds) << " calls/second" << endl;
|
||||
// cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
|
||||
// }
|
||||
|
||||
// Aug 8, iMac 3.06GHz Core i3
|
||||
// 371153 calls/second
|
||||
// 2.69431 musecs/call
|
||||
|
|
|
|||
Loading…
Reference in New Issue