12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Make a pinhole camera class that subsumes the SimpleCamera, CalibratedCameraT and GeneralCameraT classes. The corresponding files have to be deleted later. Note that CalibratedCamera can be subsumed as well, but it is still kept for potential performance benefit. Fix the corresponding corresponding unit tests .

release/4.3a0
Yong-Dian Jian 2012-01-28 14:59:58 +00:00
parent 3e6054122c
commit 58c4808679
9 changed files with 958 additions and 661 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/geometry/Cal3_S2.h
View File

@ -49,6 +49,9 @@ namespace gtsam {
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {
}
Cal3_S2(const Vector &d): fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)){}
/**
* Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
* @param fov field of view in degrees

500
gtsam/geometry/CalibratedCameraT.h
View File

@ -1,272 +1,272 @@
/**
* @file CalibratedCameraT.h
* @date Mar 5, 2011
* @author Yong-Dian Jian
* @brief calibrated camera template
*/
#pragma once
#include <boost/optional.hpp>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
namespace gtsam {
/**
* A Calibrated camera class [R|-R't], calibration K.
* If calibration is known, it is more computationally efficient
* to calibrate the measurements rather than try to predict in pixels.
* AGC: Is this used or tested anywhere?
* AGC: If this is a "CalibratedCamera," why is there a calibration stored internally?
* @ingroup geometry
* \nosubgrouping
*/
template <typename Calibration>
class CalibratedCameraT {
private:
Pose3 pose_; // 6DOF pose
Calibration k_;
public:
/// @name Standard Constructors
/// @{
///TODO: comment
CalibratedCameraT() {}
///TODO: comment
CalibratedCameraT(const Pose3& pose):pose_(pose){}
///TODO: comment
CalibratedCameraT(const Pose3& pose, const Calibration& k):pose_(pose),k_(k) {}
/// @}
/// @name Advanced Constructors
/// @{
///TODO: comment
CalibratedCameraT(const Vector &v): pose_(Pose3::Expmap(v)) {}
///TODO: comment
CalibratedCameraT(const Vector &v, const Vector &k):pose_(Pose3::Expmap(v)),k_(k){}
/// @}
/// @name Standard Interface
/// @{
virtual ~CalibratedCameraT() {}
///TODO: comment
inline Pose3& pose() { return pose_; }
///TODO: comment
inline const Pose3& pose() const { return pose_; }
///TODO: comment
inline Calibration& calibration() { return k_; }
///TODO: comment
inline const Calibration& calibration() const { return k_; }
///TODO: comment
inline const CalibratedCameraT compose(const CalibratedCameraT &c) const {
return CalibratedCameraT( pose_ * c.pose(), k_ ) ;
}
///TODO: comment
inline const CalibratedCameraT inverse() const {
return CalibratedCameraT( pose_.inverse(), k_ ) ;
}
/// @}
/// @name Testable
/// @{
/// assert equality up to a tolerance
bool equals (const CalibratedCameraT &camera, double tol = 1e-9) const {
return pose_.equals(camera.pose(), tol) && k_.equals(camera.calibration(), tol) ;
}
/// print
void print(const std::string& s = "") const {
pose_.print("pose3");
k_.print("calibration");
}
/// @}
/// @name Manifold
/// @{
///TODO: comment
CalibratedCameraT retract(const Vector& d) const {
return CalibratedCameraT(pose().retract(d), k_) ;
}
///TODO: comment
Vector localCoordinates(const CalibratedCameraT& T2) const {
return pose().localCoordinates(T2.pose()) ;
}
///TODO: comment
inline size_t dim() const { return 6 ; } //TODO: add final dimension variable?
///TODO: comment
inline static size_t Dim() { return 6 ; } //TODO: add final dimension variable?
//TODO: remove comment and method?
/**
* 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 CalibratedCameraT level(const Pose2& pose2, double height);
/* ************************************************************************* */
// 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 CalibratedCameraT
* @param point a 3D point to be projected
* @return the intrinsic coordinates of the projected point
*/
/// @}
/// @name Transformations
/// @{
///TODO: comment
inline Point2 project(const Point3& point,
boost::optional<Matrix&> D_intrinsic_pose = boost::none,
boost::optional<Matrix&> D_intrinsic_point = boost::none) const {
std::pair<Point2,bool> result = projectSafe(point, D_intrinsic_pose, D_intrinsic_point) ;
return result.first ;
}
///TODO: comment
std::pair<Point2,bool> projectSafe(
const Point3& pw,
boost::optional<Matrix&> D_intrinsic_pose = boost::none,
boost::optional<Matrix&> D_intrinsic_point = boost::none) const {
if ( !D_intrinsic_pose && !D_intrinsic_point ) {
Point3 pc = pose_.transform_to(pw) ;
Point2 pn = project_to_camera(pc) ;
return std::make_pair(k_.uncalibrate(pn), pc.z() > 0) ;
}
// world to camera coordinate
Matrix Hc1 /* 3*6 */, Hc2 /* 3*3 */ ;
Point3 pc = pose_.transform_to(pw, Hc1, Hc2) ;
// camera to normalized image coordinate
Matrix Hn; // 2*3
Point2 pn = project_to_camera(pc, Hn) ;
// uncalibration
Matrix Hi; // 2*2
Point2 pi = k_.uncalibrate(pn,boost::none,Hi);
Matrix tmp = Hi*Hn ;
if (D_intrinsic_pose) *D_intrinsic_pose = tmp * Hc1 ;
if (D_intrinsic_point) *D_intrinsic_point = tmp * Hc2 ;
return std::make_pair(pi, pc.z()>0) ;
}
/**
* 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& P,
boost::optional<Matrix&> H1 = boost::none){
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);
}
return Point2(P.x() / P.z(), P.y() / P.z());
}
/**
* backproject a 2-dimensional point to a 3-dimension point
*/
Point3 backproject_from_camera(const Point2& pi, const double scale) const {
Point2 pn = k_.calibrate(pi);
Point3 pc(pn.x()*scale, pn.y()*scale, scale);
return pose_.transform_from(pc);
}
private:
/// @}
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(pose_);
ar & BOOST_SERIALIZATION_NVP(k_);
}
/// @}
};
}
//TODO: remove?
// static CalibratedCameraT Expmap(const Vector& v) {
// return CalibratedCameraT(Pose3::Expmap(v), k_) ;
///**
// * @file CalibratedCameraT.h
// * @date Mar 5, 2011
// * @author Yong-Dian Jian
// * @brief calibrated camera template
// */
//
//#pragma once
//
//#include <boost/optional.hpp>
//#include <gtsam/geometry/Pose2.h>
//#include <gtsam/geometry/Pose3.h>
//
//namespace gtsam {
//
// /**
// * A Calibrated camera class [R|-R't], calibration K.
// * If calibration is known, it is more computationally efficient
// * to calibrate the measurements rather than try to predict in pixels.
// * AGC: Is this used or tested anywhere?
// * AGC: If this is a "CalibratedCamera," why is there a calibration stored internally?
// * @ingroup geometry
// * \nosubgrouping
// */
// template <typename Calibration>
// class CalibratedCameraT {
// private:
// Pose3 pose_; // 6DOF pose
// Calibration k_;
//
// public:
//
// /// @name Standard Constructors
// /// @{
//
// ///TODO: comment
// CalibratedCameraT() {}
//
// ///TODO: comment
// CalibratedCameraT(const Pose3& pose):pose_(pose){}
//
// ///TODO: comment
// CalibratedCameraT(const Pose3& pose, const Calibration& k):pose_(pose),k_(k) {}
//
// /// @}
// /// @name Advanced Constructors
// /// @{
//
// ///TODO: comment
// CalibratedCameraT(const Vector &v): pose_(Pose3::Expmap(v)) {}
//
// ///TODO: comment
// CalibratedCameraT(const Vector &v, const Vector &k):pose_(Pose3::Expmap(v)),k_(k){}
//
// /// @}
// /// @name Standard Interface
// /// @{
//
// virtual ~CalibratedCameraT() {}
//
// ///TODO: comment
// inline Pose3& pose() { return pose_; }
//
// ///TODO: comment
// inline const Pose3& pose() const { return pose_; }
//
// ///TODO: comment
// inline Calibration& calibration() { return k_; }
//
// ///TODO: comment
// inline const Calibration& calibration() const { return k_; }
//
// ///TODO: comment
// inline const CalibratedCameraT compose(const CalibratedCameraT &c) const {
// return CalibratedCameraT( pose_ * c.pose(), k_ ) ;
// }
// static Vector Logmap(const CalibratedCameraT& p) {
// return Pose3::Logmap(p.pose()) ;
//
// ///TODO: comment
// inline const CalibratedCameraT inverse() const {
// return CalibratedCameraT( pose_.inverse(), k_ ) ;
// }
// Point2 project(const Point3& point,
//
// /// @}
// /// @name Testable
// /// @{
//
// /// assert equality up to a tolerance
// bool equals (const CalibratedCameraT &camera, double tol = 1e-9) const {
// return pose_.equals(camera.pose(), tol) && k_.equals(camera.calibration(), tol) ;
// }
//
// /// print
// void print(const std::string& s = "") const {
// pose_.print("pose3");
// k_.print("calibration");
// }
//
// /// @}
// /// @name Manifold
// /// @{
//
// ///TODO: comment
// CalibratedCameraT retract(const Vector& d) const {
// return CalibratedCameraT(pose().retract(d), k_) ;
// }
//
// ///TODO: comment
// Vector localCoordinates(const CalibratedCameraT& T2) const {
// return pose().localCoordinates(T2.pose()) ;
// }
//
// ///TODO: comment
// inline size_t dim() const { return 6 ; } //TODO: add final dimension variable?
//
// ///TODO: comment
// inline static size_t Dim() { return 6 ; } //TODO: add final dimension variable?
//
// //TODO: remove comment and method?
// /**
// * 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 CalibratedCameraT level(const Pose2& pose2, double height);
//
// /* ************************************************************************* */
// // 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 CalibratedCameraT
// * @param point a 3D point to be projected
// * @return the intrinsic coordinates of the projected point
// */
//
// /// @}
// /// @name Transformations
// /// @{
//
// ///TODO: comment
// inline Point2 project(const Point3& point,
// boost::optional<Matrix&> D_intrinsic_pose = boost::none,
// boost::optional<Matrix&> D_intrinsic_point = boost::none) const {
// std::pair<Point2,bool> result = projectSafe(point, D_intrinsic_pose, D_intrinsic_point) ;
// return result.first ;
// }
//
// ///TODO: comment
// std::pair<Point2,bool> projectSafe(
// const Point3& pw,
// boost::optional<Matrix&> D_intrinsic_pose = boost::none,
// boost::optional<Matrix&> D_intrinsic_point = boost::none) const {
//
// // no derivative is necessary
// if ( !D_intrinsic_pose && !D_intrinsic_point ) {
// Point3 pc = pose_.transform_to(point) ;
// Point3 pc = pose_.transform_to(pw) ;
// Point2 pn = project_to_camera(pc) ;
// return k_.uncalibrate(pn) ;
// return std::make_pair(k_.uncalibrate(pn), pc.z() > 0) ;
// }
//
// // world to camera coordinate
// Matrix Hc1 /* 3*6 */, Hc2 /* 3*3 */ ;
// Point3 pc = pose_.transform_to(point, Hc1, Hc2) ;
//
// Point3 pc = pose_.transform_to(pw, Hc1, Hc2) ;
// // camera to normalized image coordinate
// Matrix Hn; // 2*3
// Point2 pn = project_to_camera(pc, Hn) ;
//
// // uncalibration
// Matrix Hi; // 2*2
// Point2 pi = k_.uncalibrate(pn,boost::none,Hi);
//
// Matrix tmp = Hi*Hn ;
// *D_intrinsic_pose = tmp * Hc1 ;
// *D_intrinsic_point = tmp * Hc2 ;
// return pi ;
// if (D_intrinsic_pose) *D_intrinsic_pose = tmp * Hc1 ;
// if (D_intrinsic_point) *D_intrinsic_point = tmp * Hc2 ;
// return std::make_pair(pi, pc.z()>0) ;
// }
//
// std::pair<Point2,bool> projectSafe(
// const Point3& pw,
// boost::optional<Matrix&> H1 = boost::none,
// boost::optional<Matrix&> H2 = boost::none) const {
// Point3 pc = pose_.transform_to(pw);
// return std::pair<Point2, bool>( project(pw,H1,H2), pc.z() > 0);
// /**
// * 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& P,
// boost::optional<Matrix&> H1 = boost::none){
// 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);
// }
// return Point2(P.x() / P.z(), P.y() / P.z());
// }
//
// std::pair<Point2,bool> projectSafe(
// const Point3& pw,
// const Point3& pw_normal,
// boost::optional<Matrix&> H1 = boost::none,
// boost::optional<Matrix&> H2 = boost::none) const {
// Point3 pc = pose_.transform_to(pw);
// Point3 pc_normal = pose_.rotation().unrotate(pw_normal);
// return std::pair<Point2, bool>( project(pw,H1,H2), (pc.z() > 0) && (pc_normal.z() < -0.5) );
// /**
// * backproject a 2-dimensional point to a 3-dimension point
// */
// Point3 backproject_from_camera(const Point2& pi, const double scale) const {
// Point2 pn = k_.calibrate(pi);
// Point3 pc(pn.x()*scale, pn.y()*scale, scale);
// return pose_.transform_from(pc);
// }
//
//private:
//
// /// @}
// /// @name Advanced Interface
// /// @{
//
// /** Serialization function */
// friend class boost::serialization::access;
// template<class Archive>
// void serialize(Archive & ar, const unsigned int version) {
// ar & BOOST_SERIALIZATION_NVP(pose_);
// ar & BOOST_SERIALIZATION_NVP(k_);
// }
//
// /// @}
// };
//}
//
////TODO: remove?
//
//// static CalibratedCameraT Expmap(const Vector& v) {
//// return CalibratedCameraT(Pose3::Expmap(v), k_) ;
//// }
//// static Vector Logmap(const CalibratedCameraT& p) {
//// return Pose3::Logmap(p.pose()) ;
//// }
//
//// Point2 project(const Point3& point,
//// boost::optional<Matrix&> D_intrinsic_pose = boost::none,
//// boost::optional<Matrix&> D_intrinsic_point = boost::none) const {
////
//// // no derivative is necessary
//// if ( !D_intrinsic_pose && !D_intrinsic_point ) {
//// Point3 pc = pose_.transform_to(point) ;
//// Point2 pn = project_to_camera(pc) ;
//// return k_.uncalibrate(pn) ;
//// }
////
//// // world to camera coordinate
//// Matrix Hc1 /* 3*6 */, Hc2 /* 3*3 */ ;
//// Point3 pc = pose_.transform_to(point, Hc1, Hc2) ;
////
//// // camera to normalized image coordinate
//// Matrix Hn; // 2*3
//// Point2 pn = project_to_camera(pc, Hn) ;
////
//// // uncalibration
//// Matrix Hi; // 2*2
//// Point2 pi = k_.uncalibrate(pn,boost::none,Hi);
////
//// Matrix tmp = Hi*Hn ;
//// *D_intrinsic_pose = tmp * Hc1 ;
//// *D_intrinsic_point = tmp * Hc2 ;
//// return pi ;
//// }
////
//// std::pair<Point2,bool> projectSafe(
//// const Point3& pw,
//// boost::optional<Matrix&> H1 = boost::none,
//// boost::optional<Matrix&> H2 = boost::none) const {
//// Point3 pc = pose_.transform_to(pw);
//// return std::pair<Point2, bool>( project(pw,H1,H2), pc.z() > 0);
//// }
////
//// std::pair<Point2,bool> projectSafe(
//// const Point3& pw,
//// const Point3& pw_normal,
//// boost::optional<Matrix&> H1 = boost::none,
//// boost::optional<Matrix&> H2 = boost::none) const {
//// Point3 pc = pose_.transform_to(pw);
//// Point3 pc_normal = pose_.rotation().unrotate(pw_normal);
//// return std::pair<Point2, bool>( project(pw,H1,H2), (pc.z() > 0) && (pc_normal.z() < -0.5) );
//// }
//

522
gtsam/geometry/GeneralCameraT.h
View File

@ -1,261 +1,261 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file GeneralCameraT.h
* @brief General camera template
* @date Mar 1, 2010
* @author Yong-Dian Jian
*/
#pragma once
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3DS2.h>
namespace gtsam {
/**
* General camera template
* @ingroup geometry
* \nosubgrouping
*/
template <typename Camera, typename Calibration>
class GeneralCameraT {
private:
Camera calibrated_; // Calibrated camera
Calibration calibration_; // Calibration
public:
/// @name Standard Constructors
/// @{
///TODO: comment
GeneralCameraT(){}
///TODO: comment
GeneralCameraT(const Camera& calibrated, const Calibration& K) : calibrated_(calibrated), calibration_(K) {}
///TODO: comment
GeneralCameraT(const Camera& calibrated ) : calibrated_(calibrated) {}
///TODO: comment
GeneralCameraT(const Pose3& pose, const Calibration& K) : calibrated_(pose), calibration_(K) {}
///TODO: comment
GeneralCameraT(const Pose3& pose) : calibrated_(pose) {}
/// @}
/// @name Advanced Constructors
/// @{
///TODO: comment
GeneralCameraT(const Pose3& pose, const Vector &v) : calibrated_(pose), calibration_(v) {}
///TODO: comment
GeneralCameraT(const Vector &v) :
calibrated_(sub(v, 0, Camera::Dim())),
calibration_(sub(v, Camera::Dim(), Camera::Dim() + Calibration::Dim() )) {}
/// @}
/// @name Standard Interface
/// @{
///TODO: comment
inline const Pose3& pose() const { return calibrated_.pose(); }
///TODO: comment
inline const Camera& calibrated() const { return calibrated_; }
///TODO: comment
inline const Calibration& calibration() const { return calibration_; }
///TODO: comment
inline GeneralCameraT compose(const Pose3 &p) const {
return GeneralCameraT( pose().compose(p), calibration_ ) ;
}
///TODO: comment
Matrix D2d_camera(const Point3& point) const {
Point2 intrinsic = calibrated_.project(point);
Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point);
Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic);
Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose;
Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic);
const int n1 = calibrated_.dim() ;
const int n2 = calibration_.dim() ;
Matrix D(2,n1+n2) ;
sub(D,0,2,0,n1) = D_2d_pose ;
sub(D,0,2,n1,n1+n2) = D_2d_calibration ;
return D;
}
///TODO: comment
Matrix D2d_3d(const Point3& point) const {
Point2 intrinsic = calibrated_.project(point);
Matrix D_intrinsic_3d = Dproject_point(calibrated_, point);
Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic);
return D_2d_intrinsic * D_intrinsic_3d;
}
///TODO: comment
Matrix D2d_camera_3d(const Point3& point) const {
Point2 intrinsic = calibrated_.project(point);
Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point);
Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic);
Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose;
Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic);
Matrix D_intrinsic_3d = Dproject_point(calibrated_, point);
Matrix D_2d_3d = D_2d_intrinsic * D_intrinsic_3d;
const int n1 = calibrated_.dim() ;
const int n2 = calibration_.dim() ;
Matrix D(2,n1+n2+3) ;
sub(D,0,2,0,n1) = D_2d_pose ;
sub(D,0,2,n1,n1+n2) = D_2d_calibration ;
sub(D,0,2,n1+n2,n1+n2+3) = D_2d_3d ;
return D;
}
/// @}
/// @name Transformations
/// @{
///TODO: comment
std::pair<Point2,bool> projectSafe(
const Point3& P,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
Point3 cameraPoint = calibrated_.pose().transform_to(P);
return std::pair<Point2, bool>(
project(P,H1,H2),
cameraPoint.z() > 0);
}
///TODO: comment
Point3 backproject(const Point2& projection, const double scale) const {
Point2 intrinsic = calibration_.calibrate(projection);
Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale);
return calibrated_.pose().transform_from(cameraPoint);
}
/**
* project function that does not merge the Jacobians of calibration and pose
*/
Point2 project(const Point3& P, Matrix& H1_pose, Matrix& H1_k, Matrix& H2_pt) const {
Matrix tmp;
Point2 intrinsic = calibrated_.project(P, H1_pose, H2_pt);
Point2 projection = calibration_.uncalibrate(intrinsic, H1_k, tmp);
H1_pose = tmp * H1_pose;
H2_pt = tmp * H2_pt;
return projection;
}
/**
* project a 3d point to the camera
* P is point in camera coordinate
* H1 is respect to pose + calibration
* H2 is respect to landmark
*/
Point2 project(const Point3& P,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
if (!H1 && !H2) {
Point2 intrinsic = calibrated_.project(P);
return calibration_.uncalibrate(intrinsic) ;
}
Matrix H1_k, H1_pose, H2_pt;
Point2 projection = project(P, H1_pose, H1_k, H2_pt);
if ( H1 ) *H1 = collect(2, &H1_pose, &H1_k);
if ( H2 ) *H2 = H2_pt;
return projection;
}
/// @}
/// @name Testable
/// @{
/// checks equality up to a tolerance
bool equals (const GeneralCameraT &camera, double tol = 1e-9) const {
return calibrated_.equals(camera.calibrated_, tol) &&
calibration_.equals(camera.calibration_, tol) ;
}
/// print with optional string
void print(const std::string& s = "") const {
calibrated_.pose().print(s + ".camera.") ;
calibration_.print(s + ".calibration.") ;
}
/// @}
/// @name Manifold
/// @{
///TODO: comment
GeneralCameraT retract(const Vector &v) const {
Vector v1 = sub(v,0,Camera::Dim());
Vector v2 = sub(v,Camera::Dim(),Camera::Dim()+Calibration::Dim());
return GeneralCameraT(calibrated_.retract(v1), calibration_.retract(v2));
}
///TODO: comment
Vector localCoordinates(const GeneralCameraT &C) const {
const Vector v1(calibrated().localCoordinates(C.calibrated())),
v2(calibration().localCoordinates(C.calibration()));
return concatVectors(2,&v1,&v2) ;
}
//inline size_t dim() { return Camera::dim() + Calibration::dim() ; }
///TODO: comment
inline size_t dim() const { return calibrated().dim() + calibration().dim() ; }
///TODO: comment
static inline size_t Dim() { return Camera::Dim() + Calibration::Dim() ; }
private:
/// @}
/// @name Advanced Interface
/// @{
friend class boost::serialization::access;
template<class Archive>
/// Serialization function
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(calibrated_);
ar & BOOST_SERIALIZATION_NVP(calibration_);
}
/// @}
};
typedef GeneralCameraT<CalibratedCamera,Cal3Bundler> Cal3BundlerCamera;
typedef GeneralCameraT<CalibratedCamera,Cal3DS2> Cal3DS2Camera; // NOTE: Typedef not referenced in gtsam
typedef GeneralCameraT<CalibratedCamera,Cal3_S2> Cal3_S2Camera; // NOTE: Typedef not referenced in gtsam
}
///* ----------------------------------------------------------------------------
//
// * GTSAM Copyright 2010, Georgia Tech Research Corporation,
// * Atlanta, Georgia 30332-0415
// * All Rights Reserved
// * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
//
// * See LICENSE for the license information
//
// * -------------------------------------------------------------------------- */
//
///**
// * @file GeneralCameraT.h
// * @brief General camera template
// * @date Mar 1, 2010
// * @author Yong-Dian Jian
// */
//
//#pragma once
//
//#include <gtsam/geometry/CalibratedCamera.h>
//#include <gtsam/geometry/Cal3_S2.h>
//#include <gtsam/geometry/Cal3Bundler.h>
//#include <gtsam/geometry/Cal3DS2.h>
//
//namespace gtsam {
//
///**
// * General camera template
// * @ingroup geometry
// * \nosubgrouping
// */
//template <typename Camera, typename Calibration>
//class GeneralCameraT {
//
//private:
// Camera calibrated_; // Calibrated camera
// Calibration calibration_; // Calibration
//
//public:
//
// /// @name Standard Constructors
// /// @{
//
// ///TODO: comment
// GeneralCameraT(){}
//
// ///TODO: comment
// GeneralCameraT(const Camera& calibrated, const Calibration& K) : calibrated_(calibrated), calibration_(K) {}
//
// ///TODO: comment
// GeneralCameraT(const Camera& calibrated ) : calibrated_(calibrated) {}
//
// ///TODO: comment
// GeneralCameraT(const Pose3& pose, const Calibration& K) : calibrated_(pose), calibration_(K) {}
//
// ///TODO: comment
// GeneralCameraT(const Pose3& pose) : calibrated_(pose) {}
//
// /// @}
// /// @name Advanced Constructors
// /// @{
//
// ///TODO: comment
// GeneralCameraT(const Pose3& pose, const Vector &v) : calibrated_(pose), calibration_(v) {}
//
// ///TODO: comment
// GeneralCameraT(const Vector &v) :
// calibrated_(sub(v, 0, Camera::Dim())),
// calibration_(sub(v, Camera::Dim(), Camera::Dim() + Calibration::Dim() )) {}
//
// /// @}
// /// @name Standard Interface
// /// @{
//
// ///TODO: comment
// inline const Pose3& pose() const { return calibrated_.pose(); }
//
// ///TODO: comment
// inline const Camera& calibrated() const { return calibrated_; }
//
// ///TODO: comment
// inline const Calibration& calibration() const { return calibration_; }
//
// ///TODO: comment
// inline GeneralCameraT compose(const Pose3 &p) const {
// return GeneralCameraT( pose().compose(p), calibration_ ) ;
// }
//
// ///TODO: comment
// Matrix D2d_camera(const Point3& point) const {
// Point2 intrinsic = calibrated_.project(point);
// Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point);
// Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic);
// Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose;
// Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic);
//
// const int n1 = calibrated_.dim() ;
// const int n2 = calibration_.dim() ;
// Matrix D(2,n1+n2) ;
//
// sub(D,0,2,0,n1) = D_2d_pose ;
// sub(D,0,2,n1,n1+n2) = D_2d_calibration ;
// return D;
// }
//
// ///TODO: comment
// Matrix D2d_3d(const Point3& point) const {
// Point2 intrinsic = calibrated_.project(point);
// Matrix D_intrinsic_3d = Dproject_point(calibrated_, point);
// Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic);
// return D_2d_intrinsic * D_intrinsic_3d;
// }
//
// ///TODO: comment
// Matrix D2d_camera_3d(const Point3& point) const {
// Point2 intrinsic = calibrated_.project(point);
// Matrix D_intrinsic_pose = Dproject_pose(calibrated_, point);
// Matrix D_2d_intrinsic = calibration_.D2d_intrinsic(intrinsic);
// Matrix D_2d_pose = D_2d_intrinsic * D_intrinsic_pose;
// Matrix D_2d_calibration = calibration_.D2d_calibration(intrinsic);
//
// Matrix D_intrinsic_3d = Dproject_point(calibrated_, point);
// Matrix D_2d_3d = D_2d_intrinsic * D_intrinsic_3d;
//
// const int n1 = calibrated_.dim() ;
// const int n2 = calibration_.dim() ;
//
// Matrix D(2,n1+n2+3) ;
//
// sub(D,0,2,0,n1) = D_2d_pose ;
// sub(D,0,2,n1,n1+n2) = D_2d_calibration ;
// sub(D,0,2,n1+n2,n1+n2+3) = D_2d_3d ;
// return D;
// }
//
// /// @}
// /// @name Transformations
// /// @{
//
// ///TODO: comment
// std::pair<Point2,bool> projectSafe(
// const Point3& P,
// boost::optional<Matrix&> H1 = boost::none,
// boost::optional<Matrix&> H2 = boost::none) const {
//
// Point3 cameraPoint = calibrated_.pose().transform_to(P);
// return std::pair<Point2, bool>(
// project(P,H1,H2),
// cameraPoint.z() > 0);
// }
//
// ///TODO: comment
// Point3 backproject(const Point2& projection, const double scale) const {
// Point2 intrinsic = calibration_.calibrate(projection);
// Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale);
// return calibrated_.pose().transform_from(cameraPoint);
// }
//
// /**
// * project function that does not merge the Jacobians of calibration and pose
// */
// Point2 project(const Point3& P, Matrix& H1_pose, Matrix& H1_k, Matrix& H2_pt) const {
// Matrix tmp;
// Point2 intrinsic = calibrated_.project(P, H1_pose, H2_pt);
// Point2 projection = calibration_.uncalibrate(intrinsic, H1_k, tmp);
// H1_pose = tmp * H1_pose;
// H2_pt = tmp * H2_pt;
// return projection;
// }
//
// /**
// * project a 3d point to the camera
// * P is point in camera coordinate
// * H1 is respect to pose + calibration
// * H2 is respect to landmark
// */
// Point2 project(const Point3& P,
// boost::optional<Matrix&> H1 = boost::none,
// boost::optional<Matrix&> H2 = boost::none) const {
//
// if (!H1 && !H2) {
// Point2 intrinsic = calibrated_.project(P);
// return calibration_.uncalibrate(intrinsic) ;
// }
//
// Matrix H1_k, H1_pose, H2_pt;
// Point2 projection = project(P, H1_pose, H1_k, H2_pt);
// if ( H1 ) *H1 = collect(2, &H1_pose, &H1_k);
// if ( H2 ) *H2 = H2_pt;
//
// return projection;
// }
//
// /// @}
// /// @name Testable
// /// @{
//
// /// checks equality up to a tolerance
// bool equals (const GeneralCameraT &camera, double tol = 1e-9) const {
// return calibrated_.equals(camera.calibrated_, tol) &&
// calibration_.equals(camera.calibration_, tol) ;
// }
//
// /// print with optional string
// void print(const std::string& s = "") const {
// calibrated_.pose().print(s + ".camera.") ;
// calibration_.print(s + ".calibration.") ;
// }
//
// /// @}
// /// @name Manifold
// /// @{
//
// ///TODO: comment
// GeneralCameraT retract(const Vector &v) const {
// Vector v1 = sub(v,0,Camera::Dim());
// Vector v2 = sub(v,Camera::Dim(),Camera::Dim()+Calibration::Dim());
// return GeneralCameraT(calibrated_.retract(v1), calibration_.retract(v2));
// }
//
// ///TODO: comment
// Vector localCoordinates(const GeneralCameraT &C) const {
// const Vector v1(calibrated().localCoordinates(C.calibrated())),
// v2(calibration().localCoordinates(C.calibration()));
// return concatVectors(2,&v1,&v2) ;
// }
//
// //inline size_t dim() { return Camera::dim() + Calibration::dim() ; }
//
// ///TODO: comment
// inline size_t dim() const { return calibrated().dim() + calibration().dim() ; }
//
// ///TODO: comment
// static inline size_t Dim() { return Camera::Dim() + Calibration::Dim() ; }
//
//private:
//
// /// @}
// /// @name Advanced Interface
// /// @{
//
// friend class boost::serialization::access;
// template<class Archive>
//
// /// Serialization function
// void serialize(Archive & ar, const unsigned int version)
// {
// ar & BOOST_SERIALIZATION_NVP(calibrated_);
// ar & BOOST_SERIALIZATION_NVP(calibration_);
// }
//
// /// @}
//
//};
//
//typedef GeneralCameraT<CalibratedCamera,Cal3Bundler> Cal3BundlerCamera;
//typedef GeneralCameraT<CalibratedCamera,Cal3DS2> Cal3DS2Camera; // NOTE: Typedef not referenced in gtsam
//typedef GeneralCameraT<CalibratedCamera,Cal3_S2> Cal3_S2Camera; // NOTE: Typedef not referenced in gtsam
//
//}

286
gtsam/geometry/PinholeCamera.h Normal file
View File

@ -0,0 +1,286 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file PinholeCamera.h
* @brief Base class for all pinhole cameras
* @author Yong-Dian Jian
* @date Jan 27, 2012
*/
#pragma once
#include <cmath>
#include <boost/optional.hpp>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/CalibratedCamera.h>
namespace gtsam {
/**
* A pinhole camera class that has a Pose3 and a Calibration.
* @ingroup geometry
* \nosubgrouping
*/
template <typename Calibration>
class PinholeCamera {
private:
Pose3 pose_;
Calibration k_;
public:
/// @name Standard Constructors
/// @{
/** default constructor */
PinholeCamera() {}
/** constructor with pose */
PinholeCamera(const Pose3& pose):pose_(pose){}
/** constructor with pose and calibration */
PinholeCamera(const Pose3& pose, const Calibration& k):pose_(pose),k_(k) {}
/** alternative constructor with pose and calibration */
PinholeCamera(const Calibration& k, const Pose3& pose):pose_(pose),k_(k) {}
/// @}
/// @name Advanced Constructors
/// @{
PinholeCamera(const Vector &v){
pose_ = Pose3::Expmap(v.head(Pose3::Dim()));
if ( v.size() > Pose3::Dim()) {
k_ = Calibration(v.tail(Calibration::Dim()));
}
}
PinholeCamera(const Vector &v, const Vector &k) : pose_(Pose3::Expmap(v)),k_(k){}
/// @}
/// @name Standard Interface
/// @{
virtual ~PinholeCamera() {}
/// return pose
inline Pose3& pose() { return pose_; }
/// return pose
inline const Pose3& pose() const { return pose_; }
/// return calibration
inline Calibration& calibration() { return k_; }
/// return calibration
inline const Calibration& calibration() const { return k_; }
/// compose two cameras
inline const PinholeCamera compose(const Pose3 &c) const {
return PinholeCamera( pose_ * c, k_ ) ;
}
/// inverse
inline const PinholeCamera inverse() const {
return PinholeCamera( pose_.inverse(), k_ ) ;
}
/// @}
/// @name Testable
/// @{
/// assert equality up to a tolerance
bool equals (const PinholeCamera &camera, double tol = 1e-9) const {
return pose_.equals(camera.pose(), tol) &&
k_.equals(camera.calibration(), tol) ;
}
/// print
void print(const std::string& s = "") const {
pose_.print("pose3");
k_.print("calibration");
}
/// @}
/// @name Manifold
/// @{
/// move a cameras according to d
PinholeCamera retract(const Vector& d) const {
if ( d.size() == pose_.dim() )
return PinholeCamera(pose().retract(d), calibration()) ;
else
return PinholeCamera(pose().retract(d.head(pose().dim())),
calibration().retract(d.tail(calibration().dim()))) ;
}
/// return canonical coordinate
Vector localCoordinates(const PinholeCamera& T2) const {
Vector d(dim());
d.head(pose().dim()) = pose().localCoordinates(T2.pose());
d.tail(calibration().dim()) = calibration().localCoordinates(T2.calibration());
return d;
}
/// Lie group dimensionality
inline size_t dim() const { return pose_.dim() + k_.dim(); }
/// Lie group dimensionality
inline static size_t Dim() { return Pose3::Dim() + Calibration::Dim(); }
/**
* 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 PinholeCamera level(const Pose2& pose2, double height) {
return PinholeCamera::level(Calibration(), pose2, height);
}
static PinholeCamera level(const Calibration &K, const Pose2& pose2, double height) {
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 Rot3 wRc(x, y, z);
const Point3 t(pose2.x(), pose2.y(), height);
const Pose3 pose3(wRc, t);
return PinholeCamera(pose3, K);
}
/* ************************************************************************* */
// measurement functions and derivatives
/* ************************************************************************* */
/**
* projects a 3-dimensional point in camera coordinates into the
* camera and returns a 2-dimensional point, no calibration applied
* With optional 2by3 derivative
*/
inline static Point2 project_to_camera(const Point3& P,
boost::optional<Matrix&> H1 = boost::none){
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);
}
return Point2(P.x() / P.z(), P.y() / P.z());
}
/// Project a point into the image and check depth
inline std::pair<Point2,bool> projectSafe(const Point3& pw) const {
const Point3 pc = pose_.transform_to(pw) ;
const Point2 pn = project_to_camera(pc) ;
return std::make_pair(k_.uncalibrate(pn), pc.z()>0);
}
/// @}
/// @name Transformations
/// @{
/** project a point from world coordinate to the image
* @param pw is a point in the world coordinate
* @param H1 is the jacobian w.r.t. pose3
* @param H2 is the jacobian w.r.t. point3
* @param H3 is the jacobian w.r.t. calibration
*/
inline Point2 project(const Point3& pw,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
if (!H1 && !H2 && !H3) {
const Point3 pc = pose_.transform_to(pw) ;
if ( pc.z() <= 0 ) throw CheiralityException();
const Point2 pn = project_to_camera(pc) ;
return k_.uncalibrate(pn);
}
// world to camera coordinate
Matrix Hc1 /* 3*6 */, Hc2 /* 3*3 */ ;
const Point3 pc = pose_.transform_to(pw, Hc1, Hc2) ;
if( pc.z() <= 0 ) throw CheiralityException();
// camera to normalized image coordinate
Matrix Hn; // 2*3
const Point2 pn = project_to_camera(pc, Hn) ;
// uncalibration
Matrix Hk, Hi; // 2*2
const Point2 pi = k_.uncalibrate(pn, Hk, Hi);
// chain the jacobian matrices
const Matrix tmp = Hi*Hn ;
if (H1) *H1 = tmp * Hc1 ;
if (H2) *H2 = tmp * Hc2 ;
if (H3) *H3 = Hk;
return pi;
}
/** project a point from world coordinate to the image
* @param pw is a point in the world coordinate
* @param H1 is the jacobian w.r.t. [pose3 calibration]
* @param H2 is the jacobian w.r.t. point3
*/
inline Point2 project2(const Point3& pw,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
if (!H1 && !H2) {
const Point3 pc = pose_.transform_to(pw) ;
if ( pc.z() <= 0 ) throw CheiralityException();
const Point2 pn = project_to_camera(pc) ;
return k_.uncalibrate(pn);
}
Matrix Htmp1, Htmp2, Htmp3;
const Point2 pi = this->project(pw, Htmp1, Htmp2, Htmp3);
if (H1) {
*H1 = Matrix(2, this->dim());
H1->leftCols(pose().dim()) = Htmp1 ; // jacobian wrt pose3
H1->rightCols(calibration().dim()) = Htmp3 ; // jacobian wrt calib
}
if (H2) *H2 = Htmp2;
return pi;
}
/**
* backproject a 2-dimensional point to a 3-dimension point
*/
inline Point3 backproject(const Point2& pi, const double scale) const {
const Point2 pn = k_.calibrate(pi);
const Point3 pc(pn.x()*scale, pn.y()*scale, scale);
return pose_.transform_from(pc);
}
inline Point3 backproject_from_camera(const Point2& pi, const double scale) const {
return backproject(pi, scale);
}
private:
/// @}
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(pose_);
ar & BOOST_SERIALIZATION_NVP(k_);
}
/// @}
};
}

144
gtsam/geometry/SimpleCamera.cpp
View File

@ -1,72 +1,72 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SimpleCamera.cpp
* @date Aug 16, 2009
* @author dellaert
*/
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/CalibratedCamera.h>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
SimpleCamera::SimpleCamera(const Cal3_S2& K,
const CalibratedCamera& calibrated) :
calibrated_(calibrated), K_(K) {
}
SimpleCamera::SimpleCamera(const Cal3_S2& K, const Pose3& pose) :
calibrated_(pose), K_(K) {
}
SimpleCamera::~SimpleCamera() {
}
pair<Point2, bool> SimpleCamera::projectSafe(const Point3& P) const {
Point3 cameraPoint = calibrated_.pose().transform_to(P);
Point2 intrinsic = CalibratedCamera::project_to_camera(cameraPoint);
Point2 projection = K_.uncalibrate(intrinsic);
return pair<Point2, bool>(projection, cameraPoint.z() > 0);
}
Point2 SimpleCamera::project(const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point2 intrinsic = calibrated_.project(point, H1, H2);
if (!H1 && !H2)
return K_.uncalibrate(intrinsic);
else {
Matrix D_projection_intrinsic;
Point2 projection = K_.uncalibrate(intrinsic, boost::none, D_projection_intrinsic);
if (H1) *H1 = D_projection_intrinsic * (*H1);
if (H2) *H2 = D_projection_intrinsic * (*H2);
return projection;
}
}
Point3 SimpleCamera::backproject(const Point2& projection, const double scale) const {
Point2 intrinsic = K_.calibrate(projection);
Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale);
return calibrated_.pose().transform_from(cameraPoint);
}
SimpleCamera SimpleCamera::level(const Cal3_S2& K, const Pose2& pose2, double height) {
return SimpleCamera(K, CalibratedCamera::level(pose2, height));
}
/* ************************************************************************* */
}
///* ----------------------------------------------------------------------------
//
// * GTSAM Copyright 2010, Georgia Tech Research Corporation,
// * Atlanta, Georgia 30332-0415
// * All Rights Reserved
// * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
//
// * See LICENSE for the license information
//
// * -------------------------------------------------------------------------- */
//
///**
// * @file SimpleCamera.cpp
// * @date Aug 16, 2009
// * @author dellaert
// */
//
//#include <gtsam/geometry/SimpleCamera.h>
//#include <gtsam/geometry/CalibratedCamera.h>
//
//using namespace std;
//
//namespace gtsam {
//
// /* ************************************************************************* */
//
// SimpleCamera::SimpleCamera(const Cal3_S2& K,
// const CalibratedCamera& calibrated) :
// calibrated_(calibrated), K_(K) {
// }
//
// SimpleCamera::SimpleCamera(const Cal3_S2& K, const Pose3& pose) :
// calibrated_(pose), K_(K) {
// }
//
// SimpleCamera::~SimpleCamera() {
// }
//
// pair<Point2, bool> SimpleCamera::projectSafe(const Point3& P) const {
// Point3 cameraPoint = calibrated_.pose().transform_to(P);
// Point2 intrinsic = CalibratedCamera::project_to_camera(cameraPoint);
// Point2 projection = K_.uncalibrate(intrinsic);
// return pair<Point2, bool>(projection, cameraPoint.z() > 0);
// }
//
// Point2 SimpleCamera::project(const Point3& point,
// boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
//
// Point2 intrinsic = calibrated_.project(point, H1, H2);
// if (!H1 && !H2)
// return K_.uncalibrate(intrinsic);
// else {
// Matrix D_projection_intrinsic;
// Point2 projection = K_.uncalibrate(intrinsic, boost::none, D_projection_intrinsic);
// if (H1) *H1 = D_projection_intrinsic * (*H1);
// if (H2) *H2 = D_projection_intrinsic * (*H2);
// return projection;
// }
// }
//
// Point3 SimpleCamera::backproject(const Point2& projection, const double scale) const {
// Point2 intrinsic = K_.calibrate(projection);
// Point3 cameraPoint = CalibratedCamera::backproject_from_camera(intrinsic, scale);
// return calibrated_.pose().transform_from(cameraPoint);
// }
//
// SimpleCamera SimpleCamera::level(const Cal3_S2& K, const Pose2& pose2, double height) {
// return SimpleCamera(K, CalibratedCamera::level(pose2, height));
// }
//
///* ************************************************************************* */
//}

145
gtsam/geometry/SimpleCamera.h
View File

@ -18,77 +18,84 @@
#pragma once
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/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);
/** constructor for serialization */
SimpleCamera(){}
virtual ~SimpleCamera();
const Pose3& pose() const {
return calibrated_.pose();
typedef PinholeCamera<Cal3_S2> SimpleCamera;
}
const Cal3_S2& calibration() const {
return K_;
}
/**
* project a 3d point to the camera and also check the depth
*/
std::pair<Point2,bool> projectSafe(const Point3& P) const;
/**
* backproject a 2d point from the camera up to a given scale
*/
Point3 backproject(const Point2& projection, const double scale) const;
/**
* 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 SimpleCamera level(const Cal3_S2& K, const Pose2& pose2, double height);
/**
* 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 Point3& point,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
bool equals(const SimpleCamera& X, double tol=1e-9) const {
return calibrated_.equals(X.calibrated_, tol) && K_.equals(X.K_, tol);
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(calibrated_);
ar & BOOST_SERIALIZATION_NVP(K_);
}
};
}
//#include <gtsam/geometry/CalibratedCamera.h>
//#include <gtsam/geometry/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);
//
// /** constructor for serialization */
// SimpleCamera(){}
//
// virtual ~SimpleCamera();
//
// const Pose3& pose() const {
// return calibrated_.pose();
// }
//
// const Cal3_S2& calibration() const {
// return K_;
// }
//
// /**
// * project a 3d point to the camera and also check the depth
// */
// std::pair<Point2,bool> projectSafe(const Point3& P) const;
//
// /**
// * backproject a 2d point from the camera up to a given scale
// */
// Point3 backproject(const Point2& projection, const double scale) const;
//
// /**
// * 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 SimpleCamera level(const Cal3_S2& K, const Pose2& pose2, double height);
//
// /**
// * 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 Point3& point,
// boost::optional<Matrix&> H1 = boost::none,
// boost::optional<Matrix&> H2 = boost::none) const;
//
// bool equals(const SimpleCamera& X, double tol=1e-9) const {
// return calibrated_.equals(X.calibrated_, tol) && K_.equals(X.K_, tol);
// }
//
// private:
// /** Serialization function */
// friend class boost::serialization::access;
// template<class ARCHIVE>
// void serialize(ARCHIVE & ar, const unsigned int version) {
// ar & BOOST_SERIALIZATION_NVP(calibrated_);
// ar & BOOST_SERIALIZATION_NVP(K_);
// }
//
// };
//
//}

3
gtsam/slam/GeneralSFMFactor.h
View File

@ -20,6 +20,7 @@
#pragma once
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
@ -82,7 +83,7 @@ namespace gtsam {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
Vector error = z_.localCoordinates(camera.project(point,H1,H2));
Vector error = z_.localCoordinates(camera.project2(point,H1,H2));
// gtsam::print(error, "error");
return error;
}

8
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -16,20 +16,19 @@ using namespace boost;
#define GTSAM_MAGIC_KEY
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/geometry/GeneralCameraT.h>
#include <gtsam/slam/GeneralSFMFactor.h>
using namespace std;
using namespace gtsam;
typedef Cal3_S2Camera GeneralCamera;
//typedef Cal3BundlerCamera GeneralCamera;
typedef PinholeCamera<Cal3_S2> GeneralCamera;
typedef TypedSymbol<GeneralCamera, 'x'> CameraKey;
typedef TypedSymbol<Point3, 'l'> PointKey;
typedef Values<CameraKey> CameraConfig;
@ -341,6 +340,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
Optimizer optimizer(graph, values, ordering, params);
Optimizer optimizer2 = optimizer.levenbergMarquardt();
EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
}

6
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -16,19 +16,19 @@ using namespace boost;
#define GTSAM_MAGIC_KEY
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/geometry/GeneralCameraT.h>
#include <gtsam/slam/GeneralSFMFactor.h>
using namespace std;
using namespace gtsam;
typedef Cal3BundlerCamera GeneralCamera;
typedef PinholeCamera<Cal3Bundler> GeneralCamera;
typedef TypedSymbol<GeneralCamera, 'x'> CameraKey;
typedef TypedSymbol<Point3, 'l'> PointKey;
typedef Values<CameraKey> CameraConfig;