updated comment groupings for geometry
Nick Barrash
parent
fede16280c
commit
16bffaba36
|
|
@ -25,6 +25,7 @@ namespace gtsam {
|
|||
/**
|
||||
* @brief Calibration used by Bundler
|
||||
* @ingroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class Cal3Bundler {
|
||||
|
||||
|
|
@ -33,31 +34,77 @@ private:
|
|||
|
||||
public:
|
||||
|
||||
Cal3Bundler() ;
|
||||
Cal3Bundler(const Vector &v) ;
|
||||
Cal3Bundler(const double f, const double k1, const double k2) ;
|
||||
Matrix K() const ;
|
||||
Vector k() const ;
|
||||
|
||||
Vector vector() const;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
///TODO: comment
|
||||
Cal3Bundler() ;
|
||||
|
||||
///TODO: comment
|
||||
Cal3Bundler(const double f, const double k1, const double k2) ;
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
///TODO: comment
|
||||
Cal3Bundler(const Vector &v) ;
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// print with optional string
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const Cal3Bundler& K, double tol = 10e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
///TODO: comment
|
||||
Point2 uncalibrate(const Point2& p,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const ;
|
||||
|
||||
///TODO: comment
|
||||
Matrix D2d_intrinsic(const Point2& p) const ;
|
||||
|
||||
///TODO: comment
|
||||
Matrix D2d_calibration(const Point2& p) const ;
|
||||
|
||||
///TODO: comment
|
||||
Matrix D2d_intrinsic_calibration(const Point2& p) const ;
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
///TODO: comment
|
||||
Cal3Bundler retract(const Vector& d) const ;
|
||||
|
||||
///TODO: comment
|
||||
Vector localCoordinates(const Cal3Bundler& T2) const ;
|
||||
|
||||
int dim() const { return 3 ; }
|
||||
static size_t Dim() { return 3; }
|
||||
///TODO: comment
|
||||
int dim() const { return 3 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
|
||||
|
||||
///TODO: comment
|
||||
static size_t Dim() { return 3; } //TODO: make a final dimension variable
|
||||
|
||||
private:
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
|
|
@ -68,6 +115,9 @@ private:
|
|||
ar & BOOST_SERIALIZATION_NVP(k2_);
|
||||
}
|
||||
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -25,9 +25,12 @@ namespace gtsam {
|
|||
/**
|
||||
* @brief Calibration of a camera with radial distortion
|
||||
* @ingroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class Cal3DS2 {
|
||||
|
||||
private:
|
||||
|
||||
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
|
||||
double k1_, k2_ ; // radial 2nd-order and 4th-order
|
||||
double k3_, k4_ ; // tagential distortion
|
||||
|
|
@ -39,36 +42,72 @@ private:
|
|||
// pi = K*pn
|
||||
|
||||
public:
|
||||
// Default Constructor with only unit focal length
|
||||
Cal3DS2();
|
||||
|
||||
// Construction
|
||||
Cal3DS2(double fx, double fy, double s, double u0, double v0,
|
||||
double k1, double k2, double k3, double k4) ;
|
||||
|
||||
Cal3DS2(const Vector &v) ;
|
||||
|
||||
Matrix K() const ;
|
||||
Vector k() const ;
|
||||
Vector vector() const ;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/// Default Constructor with only unit focal length
|
||||
Cal3DS2();
|
||||
|
||||
Cal3DS2(double fx, double fy, double s, double u0, double v0,
|
||||
double k1, double k2, double k3, double k4) ;
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
Cal3DS2(const Vector &v) ;
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// print with optional string
|
||||
void print(const std::string& s = "") const ;
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const Cal3DS2& K, double tol = 10e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
///TODO: comment
|
||||
Point2 uncalibrate(const Point2& p,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const ;
|
||||
|
||||
///TODO: comment
|
||||
Matrix D2d_intrinsic(const Point2& p) const ;
|
||||
|
||||
///TODO: comment
|
||||
Matrix D2d_calibration(const Point2& p) const ;
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
///TODO: comment
|
||||
Cal3DS2 retract(const Vector& d) const ;
|
||||
|
||||
///TODO: comment
|
||||
Vector localCoordinates(const Cal3DS2& T2) const ;
|
||||
|
||||
int dim() const { return 9 ; }
|
||||
static size_t Dim() { return 9; }
|
||||
///TODO: comment
|
||||
int dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
|
||||
|
||||
///TODO: comment
|
||||
static size_t Dim() { return 9; } //TODO: make a final dimension variable
|
||||
|
||||
private:
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
|
|
@ -85,6 +124,10 @@ private:
|
|||
ar & BOOST_SERIALIZATION_NVP(k4_);
|
||||
}
|
||||
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -28,12 +28,17 @@ namespace gtsam {
|
|||
/**
|
||||
* @brief The most common 5DOF 3D->2D calibration
|
||||
* @ingroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class Cal3_S2 {
|
||||
private:
|
||||
double fx_, fy_, s_, u0_, v0_;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
public:
|
||||
|
||||
/// Create a default calibration that leaves coordinates unchanged
|
||||
Cal3_S2() :
|
||||
fx_(1), fy_(1), s_(0), u0_(0), v0_(0) {
|
||||
|
|
@ -52,6 +57,18 @@ namespace gtsam {
|
|||
*/
|
||||
Cal3_S2(double fov, int w, int h);
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
/// load calibration from location (default name is calibration_info.txt)
|
||||
Cal3_S2(const std::string &path);
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// print with optional string
|
||||
void print(const std::string& s = "") const {
|
||||
gtsam::print(matrix(), s);
|
||||
}
|
||||
|
|
@ -59,8 +76,9 @@ namespace gtsam {
|
|||
/// Check if equal up to specified tolerance
|
||||
bool equals(const Cal3_S2& K, double tol = 10e-9) const;
|
||||
|
||||
/// load calibration from location (default name is calibration_info.txt)
|
||||
Cal3_S2(const std::string &path);
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// focal length x
|
||||
inline double fx() const { return fx_;}
|
||||
|
|
@ -118,6 +136,10 @@ namespace gtsam {
|
|||
* (v - v0_));
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// return DOF, dimensionality of tangent space
|
||||
inline size_t dim() const {
|
||||
return 5;
|
||||
|
|
@ -138,6 +160,10 @@ namespace gtsam {
|
|||
return vector() - T2.vector();
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
private:
|
||||
|
||||
/// Serialization function
|
||||
|
|
@ -150,8 +176,13 @@ namespace gtsam {
|
|||
ar & BOOST_SERIALIZATION_NVP(u0_);
|
||||
ar & BOOST_SERIALIZATION_NVP(v0_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
typedef boost::shared_ptr<Cal3_S2> shared_ptrK; ///< shared pointer to calibration object
|
||||
|
||||
} // gtsam
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@ namespace gtsam {
|
|||
/**
|
||||
* @brief The most common 5DOF 3D->2D calibration, stereo version
|
||||
* @ingroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class Cal3_S2Stereo: public Cal3_S2 {
|
||||
private:
|
||||
|
|
@ -33,32 +34,46 @@ namespace gtsam {
|
|||
|
||||
typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptr; ///< shared pointer to stereo calibration object
|
||||
|
||||
/**
|
||||
* default calibration leaves coordinates unchanged
|
||||
*/
|
||||
/// @name Standard Constructors
|
||||
/// @
|
||||
|
||||
/// default calibration leaves coordinates unchanged
|
||||
Cal3_S2Stereo() :
|
||||
Cal3_S2(1, 1, 0, 0, 0), b_(1.0) {
|
||||
}
|
||||
|
||||
/**
|
||||
* constructor from doubles
|
||||
*/
|
||||
/// constructor from doubles
|
||||
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) :
|
||||
Cal3_S2(fx, fy, s, u0, v0), b_(b) {
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
void print(const std::string& s = "") const {
|
||||
gtsam::print(matrix(), s);
|
||||
std::cout << "Baseline: " << b_ << std::endl;
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
//TODO: remove?
|
||||
// /**
|
||||
// * Check if equal up to specified tolerance
|
||||
// */
|
||||
// bool equals(const Cal3_S2& K, double tol = 10e-9) const;
|
||||
|
||||
|
||||
|
||||
///TODO: comment
|
||||
inline double baseline() const { return b_; }
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
@ -69,6 +84,8 @@ namespace gtsam {
|
|||
ar & BOOST_SERIALIZATION_NVP(b_);
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Cal3_S2);
|
||||
}
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptrKStereo; ///< shared pointer to stereo calibration object
|
||||
|
|
|
|||
|
|
@ -33,25 +33,49 @@ namespace gtsam {
|
|||
* If calibration is known, it is more computationally efficient
|
||||
* to calibrate the measurements rather than try to predict in pixels.
|
||||
* @ingroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class CalibratedCamera {
|
||||
private:
|
||||
Pose3 pose_; // 6DOF pose
|
||||
|
||||
public:
|
||||
CalibratedCamera() {} ///< default constructor
|
||||
CalibratedCamera(const Pose3& pose); ///< construct with pose
|
||||
CalibratedCamera(const Vector &v) ; ///< construct from vector
|
||||
virtual ~CalibratedCamera() {} ///< destructor
|
||||
|
||||
/// return pose
|
||||
inline const Pose3& pose() const { return pose_; }
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/// default constructor
|
||||
CalibratedCamera() {}
|
||||
|
||||
/// construct with pose
|
||||
CalibratedCamera(const Pose3& pose);
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
/// construct from vector
|
||||
CalibratedCamera(const Vector &v) ;
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// 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 poses
|
||||
inline const CalibratedCamera compose(const CalibratedCamera &c) const {
|
||||
return CalibratedCamera( pose_ * c.pose() ) ;
|
||||
|
|
@ -62,6 +86,17 @@ namespace gtsam {
|
|||
return CalibratedCamera( pose_.inverse() ) ;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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);
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// move a cameras pose according to d
|
||||
CalibratedCamera retract(const Vector& d) const;
|
||||
|
||||
|
|
@ -74,17 +109,15 @@ namespace gtsam {
|
|||
/// Lie group dimensionality
|
||||
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);
|
||||
|
||||
/* ************************************************************************* */
|
||||
// measurement functions and derivatives
|
||||
/* ************************************************************************* */
|
||||
|
||||
/// @}
|
||||
/// @name Transformations
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* This function receives the camera pose and the landmark location and
|
||||
* returns the location the point is supposed to appear in the image
|
||||
|
|
@ -111,12 +144,18 @@ namespace gtsam {
|
|||
|
||||
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_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
};
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@ namespace gtsam {
|
|||
* 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 {
|
||||
|
|
@ -28,44 +29,93 @@ namespace gtsam {
|
|||
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_; }
|
||||
inline const Pose3& pose() const { return pose_; }
|
||||
inline Calibration& calibration() { return k_; }
|
||||
inline const Calibration& calibration() const { return k_; }
|
||||
bool equals (const CalibratedCameraT &camera, double tol = 1e-9) const {
|
||||
return pose_.equals(camera.pose(), tol) && k_.equals(camera.calibration(), tol) ;
|
||||
}
|
||||
|
||||
///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_ ) ;
|
||||
}
|
||||
|
||||
CalibratedCameraT retract(const Vector& d) const {
|
||||
return CalibratedCameraT(pose().retract(d), k_) ;
|
||||
}
|
||||
Vector localCoordinates(const CalibratedCameraT& T2) const {
|
||||
return pose().localCoordinates(T2.pose()) ;
|
||||
/// @}
|
||||
/// @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");
|
||||
}
|
||||
|
||||
inline size_t dim() const { return 6 ; }
|
||||
inline static size_t Dim() { return 6 ; }
|
||||
/// @}
|
||||
/// @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
|
||||
|
|
@ -85,6 +135,11 @@ namespace gtsam {
|
|||
* @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 {
|
||||
|
|
@ -92,6 +147,7 @@ namespace gtsam {
|
|||
return result.first ;
|
||||
}
|
||||
|
||||
///TODO: comment
|
||||
std::pair<Point2,bool> projectSafe(
|
||||
const Point3& pw,
|
||||
boost::optional<Matrix&> D_intrinsic_pose = boost::none,
|
||||
|
|
@ -141,6 +197,11 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
private:
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
|
|
@ -148,9 +209,13 @@ private:
|
|||
ar & BOOST_SERIALIZATION_NVP(pose_);
|
||||
ar & BOOST_SERIALIZATION_NVP(k_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
};
|
||||
}
|
||||
|
||||
//TODO: remove?
|
||||
|
||||
// static CalibratedCameraT Expmap(const Vector& v) {
|
||||
// return CalibratedCameraT(Pose3::Expmap(v), k_) ;
|
||||
// }
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@ namespace gtsam {
|
|||
/**
|
||||
* General camera template
|
||||
* @ingroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template <typename Camera, typename Calibration>
|
||||
class GeneralCameraT {
|
||||
|
|
@ -37,22 +38,107 @@ private:
|
|||
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) {}
|
||||
|
||||
// Vector Initialization
|
||||
///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,
|
||||
|
|
@ -64,6 +150,7 @@ public:
|
|||
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);
|
||||
|
|
@ -105,90 +192,66 @@ public:
|
|||
return projection;
|
||||
}
|
||||
|
||||
// dump functions for compilation for now
|
||||
/// @}
|
||||
/// @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 GeneralCameraT compose(const Pose3 &p) const {
|
||||
return GeneralCameraT( pose().compose(p), calibration_ ) ;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
//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;
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@ namespace gtsam {
|
|||
* Complies with the Testable Concept
|
||||
* Functional, so no set functions: once created, a point is constant.
|
||||
* @ingroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class Point2 {
|
||||
public:
|
||||
|
|
@ -37,11 +38,27 @@ private:
|
|||
double x_, y_;
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
///TODO: comment
|
||||
Point2(): x_(0), y_(0) {}
|
||||
|
||||
///TODO: comment
|
||||
Point2(const Point2 &p) : x_(p.x_), y_(p.y_) {}
|
||||
|
||||
///TODO: comment
|
||||
Point2(double x, double y): x_(x), y_(y) {}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
///TODO: comment
|
||||
Point2(const Vector& v) : x_(v(0)), y_(v(1)) { assert(v.size() == 2); }
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
|
|
@ -72,11 +89,19 @@ public:
|
|||
return *this + p2;
|
||||
}
|
||||
|
||||
/** operators */
|
||||
///TODO: comment
|
||||
inline Point2 operator- () const {return Point2(-x_,-y_);}
|
||||
|
||||
///TODO: comment
|
||||
inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}
|
||||
|
||||
///TODO: comment
|
||||
inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);}
|
||||
|
||||
///TODO: comment
|
||||
inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);}
|
||||
|
||||
///TODO: comment
|
||||
inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);}
|
||||
|
||||
/// @}
|
||||
|
|
@ -120,12 +145,18 @@ public:
|
|||
return (p2 - *this).norm();
|
||||
}
|
||||
|
||||
/** operators */
|
||||
///TODO: comment
|
||||
inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;}
|
||||
|
||||
///TODO: comment
|
||||
inline void operator *= (double s) {x_*=s;y_*=s;}
|
||||
|
||||
///TODO: comment
|
||||
inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** "Between", subtracts point coordinates */
|
||||
inline Point2 between(const Point2& p2,
|
||||
|
|
@ -136,14 +167,21 @@ public:
|
|||
return p2 - (*this);
|
||||
}
|
||||
|
||||
/** get functions for x, y */
|
||||
/// get x
|
||||
double x() const {return x_;}
|
||||
|
||||
/// get y
|
||||
double y() const {return y_;}
|
||||
|
||||
/** return vectorized form (column-wise) */
|
||||
Vector vector() const { return Vector_(2, x_, y_); }
|
||||
|
||||
private:
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
|
|
@ -152,9 +190,13 @@ private:
|
|||
ar & BOOST_SERIALIZATION_NVP(x_);
|
||||
ar & BOOST_SERIALIZATION_NVP(y_);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/** multiply with scalar */
|
||||
inline Point2 operator*(double s, const Point2& p) {return p*s;}
|
||||
|
||||
/// @}
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -31,6 +31,7 @@ namespace gtsam {
|
|||
/**
|
||||
* A 3D point
|
||||
* @ingroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class Point3 {
|
||||
public:
|
||||
|
|
@ -41,11 +42,27 @@ namespace gtsam {
|
|||
double x_, y_, z_;
|
||||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
///TODO: comment
|
||||
Point3(): x_(0), y_(0), z_(0) {}
|
||||
|
||||
///TODO: comment
|
||||
Point3(const Point3 &p) : x_(p.x_), y_(p.y_), z_(p.z_) {}
|
||||
|
||||
///TODO: comment
|
||||
Point3(double x, double y, double z): x_(x), y_(y), z_(z) {}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
///TODO: comment
|
||||
Point3(const Vector& v) : x_(v(0)), y_(v(1)), z_(v(2)) {}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
|
|
@ -106,11 +123,22 @@ namespace gtsam {
|
|||
/// @name Vector Operators
|
||||
/// @{
|
||||
|
||||
///TODO: comment
|
||||
Point3 operator - () const { return Point3(-x_,-y_,-z_);}
|
||||
|
||||
///TODO: comment
|
||||
bool operator ==(const Point3& q) const;
|
||||
|
||||
///TODO: comment
|
||||
Point3 operator + (const Point3& q) const;
|
||||
|
||||
///TODO: comment
|
||||
Point3 operator - (const Point3& q) const;
|
||||
|
||||
///TODO: comment
|
||||
Point3 operator * (double s) const;
|
||||
|
||||
///TODO: comment
|
||||
Point3 operator / (double s) const;
|
||||
|
||||
/** distance between two points */
|
||||
|
|
@ -128,6 +156,8 @@ namespace gtsam {
|
|||
double dot(const Point3 &q) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** Between using the default implementation */
|
||||
inline Point3 between(const Point3& p2,
|
||||
|
|
@ -144,9 +174,13 @@ namespace gtsam {
|
|||
return v;
|
||||
}
|
||||
|
||||
/** get functions for x, y, z */
|
||||
/// get x
|
||||
inline double x() const {return x_;}
|
||||
|
||||
/// get y
|
||||
inline double y() const {return y_;}
|
||||
|
||||
/// get z
|
||||
inline double z() const {return z_;}
|
||||
|
||||
/** add two points, add(this,q) is same as this + q */
|
||||
|
|
@ -158,6 +192,11 @@ namespace gtsam {
|
|||
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
|
||||
|
||||
private:
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
|
|
@ -172,4 +211,6 @@ namespace gtsam {
|
|||
/// Syntactic sugar for multiplying coordinates by a scalar s*p
|
||||
inline Point3 operator*(double s, const Point3& p) { return p*s;}
|
||||
|
||||
/// @}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -30,6 +30,7 @@ namespace gtsam {
|
|||
/**
|
||||
* A 2D pose (Point2,Rot2)
|
||||
* @ingroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class Pose2 {
|
||||
|
||||
|
|
@ -46,6 +47,9 @@ private:
|
|||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** default constructor = origin */
|
||||
Pose2() {} // default is origin
|
||||
|
||||
|
|
@ -66,6 +70,8 @@ public:
|
|||
Pose2(double theta, const Point2& t) :
|
||||
r_(Rot2::fromAngle(theta)), t_(t) {
|
||||
}
|
||||
|
||||
///TODO: comment
|
||||
Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {}
|
||||
|
||||
/** Constructor from 3*3 matrix */
|
||||
|
|
@ -74,11 +80,16 @@ public:
|
|||
assert(T.rows() == 3 && T.cols() == 3);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
/** Construct from canonical coordinates (Lie algebra) */
|
||||
Pose2(const Vector& v) {
|
||||
*this = Expmap(v);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
|
|
@ -135,6 +146,8 @@ public:
|
|||
static Vector Logmap(const Pose2& p);
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** return transformation matrix */
|
||||
Matrix matrix() const;
|
||||
|
|
@ -224,8 +237,14 @@ public:
|
|||
}
|
||||
|
||||
/** get functions for x, y, theta */
|
||||
|
||||
/// get x
|
||||
inline double x() const { return t_.x(); }
|
||||
|
||||
/// get y
|
||||
inline double y() const { return t_.y(); }
|
||||
|
||||
/// get theta
|
||||
inline double theta() const { return r_.theta(); }
|
||||
|
||||
/** shorthand access functions */
|
||||
|
|
@ -237,6 +256,11 @@ public:
|
|||
inline const Rot2& rotation() const { return r_; }
|
||||
|
||||
private:
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
// Serialization function
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
|
|
@ -259,5 +283,7 @@ inline Matrix wedge<Pose2>(const Vector& xi) {
|
|||
typedef std::pair<Point2,Point2> Point2Pair;
|
||||
boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
|
||||
|
||||
/// @}
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@ namespace gtsam {
|
|||
* Rotation matrix
|
||||
* NOTE: the angle theta is in radians unless explicitly stated
|
||||
* @ingroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class Rot2 {
|
||||
|
||||
|
|
@ -39,14 +40,18 @@ namespace gtsam {
|
|||
/** we store cos(theta) and sin(theta) */
|
||||
double c_, s_;
|
||||
|
||||
|
||||
/** normalize to make sure cos and sin form unit vector */
|
||||
Rot2& normalize();
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** private constructor from cos/sin */
|
||||
inline Rot2(double c, double s) :
|
||||
c_(c), s_(s) {
|
||||
}
|
||||
|
||||
/** normalize to make sure cos and sin form unit vector */
|
||||
Rot2& normalize();
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor, zero rotation */
|
||||
|
|
@ -59,8 +64,6 @@ namespace gtsam {
|
|||
c_(cos(theta)), s_(sin(theta)) {
|
||||
}
|
||||
|
||||
/** "named constructors" */
|
||||
|
||||
/// Named constructor from angle in radians
|
||||
static Rot2 fromAngle(double theta) {
|
||||
return Rot2(theta);
|
||||
|
|
@ -88,27 +91,7 @@ namespace gtsam {
|
|||
/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
|
||||
static Rot2 atan2(double y, double x);
|
||||
|
||||
/** return angle (RADIANS) */
|
||||
double theta() const {
|
||||
return ::atan2(s_, c_);
|
||||
}
|
||||
|
||||
/** return angle (DEGREES) */
|
||||
double degrees() const {
|
||||
const double degree = M_PI / 180;
|
||||
return theta() / degree;
|
||||
}
|
||||
|
||||
/** return cos */
|
||||
inline double c() const {
|
||||
return c_;
|
||||
}
|
||||
|
||||
/** return sin */
|
||||
inline double s() const {
|
||||
return s_;
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
|
|
@ -197,6 +180,29 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** return angle (RADIANS) */
|
||||
double theta() const {
|
||||
return ::atan2(s_, c_);
|
||||
}
|
||||
|
||||
/** return angle (DEGREES) */
|
||||
double degrees() const {
|
||||
const double degree = M_PI / 180;
|
||||
return theta() / degree;
|
||||
}
|
||||
|
||||
/** return cos */
|
||||
inline double c() const {
|
||||
return c_;
|
||||
}
|
||||
|
||||
/** return sin */
|
||||
inline double s() const {
|
||||
return s_;
|
||||
}
|
||||
|
||||
/** Between using the default implementation */
|
||||
inline Rot2 between(const Rot2& p2, boost::optional<Matrix&> H1 =
|
||||
|
|
@ -226,6 +232,9 @@ namespace gtsam {
|
|||
Point2 unrotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
@ -236,6 +245,8 @@ namespace gtsam {
|
|||
ar & BOOST_SERIALIZATION_NVP(s_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
} // gtsam
|
||||
|
|
|
|||
|
|
@ -25,6 +25,7 @@ namespace gtsam {
|
|||
/**
|
||||
* A 2D stereo point, v will be same for rectified images
|
||||
* @ingroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class StereoPoint2 {
|
||||
public:
|
||||
|
|
@ -34,6 +35,9 @@ namespace gtsam {
|
|||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** default constructor */
|
||||
StereoPoint2() :
|
||||
uL_(0), uR_(0), v_(0) {
|
||||
|
|
@ -44,6 +48,7 @@ namespace gtsam {
|
|||
uL_(uL), uR_(uR), v_(v) {
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
|
|
@ -113,7 +118,9 @@ namespace gtsam {
|
|||
return p.vector();
|
||||
}
|
||||
|
||||
/// @}}
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** convert to vector */
|
||||
Vector vector() const {
|
||||
|
|
@ -130,6 +137,11 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
private:
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
|
|
@ -138,6 +150,9 @@ namespace gtsam {
|
|||
ar & BOOST_SERIALIZATION_NVP(uR_);
|
||||
ar & BOOST_SERIALIZATION_NVP(v_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@ namespace tensors {
|
|||
/**
|
||||
* A rank 1 tensor. Actually stores data.
|
||||
* @ingroup tensors
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<int N>
|
||||
class Tensor1 {
|
||||
|
|
@ -31,6 +32,9 @@ namespace tensors {
|
|||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** default constructor */
|
||||
Tensor1() {
|
||||
}
|
||||
|
|
@ -48,6 +52,10 @@ namespace tensors {
|
|||
T[i] = a(i);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** return data */
|
||||
inline int dim() const {
|
||||
return N;
|
||||
|
|
@ -69,6 +77,8 @@ namespace tensors {
|
|||
return Tensor1Expression<Tensor1, Index<N, I> >(*this);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
// Tensor1
|
||||
|
||||
|
|
|
|||
|
|
@ -30,6 +30,7 @@ namespace tensors {
|
|||
* This class does not store any data but the result of an expression.
|
||||
* It is associated with an index.
|
||||
* @ingroup tensors
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class A, class I> class Tensor1Expression {
|
||||
|
||||
|
|
@ -56,11 +57,18 @@ namespace tensors {
|
|||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** constructor */
|
||||
Tensor1Expression(const A &a) :
|
||||
iter(a) {
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** Print */
|
||||
void print(const std::string s = "") const {
|
||||
std::cout << s << "{";
|
||||
|
|
@ -78,6 +86,10 @@ namespace tensors {
|
|||
return true;
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** norm */
|
||||
double norm() const {
|
||||
double sumsqr = 0.0;
|
||||
|
|
@ -122,6 +134,10 @@ namespace tensors {
|
|||
|
||||
}; // Tensor1Expression
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Print a rank 1 expression */
|
||||
template<class A, class I>
|
||||
void print(const Tensor1Expression<A, I>& T, const std::string s = "") {
|
||||
|
|
@ -160,4 +176,6 @@ namespace tensors {
|
|||
return false;
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
} // namespace tensors
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@ namespace tensors {
|
|||
/**
|
||||
* Rank 2 Tensor
|
||||
* @ingroup tensors
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<int N1, int N2>
|
||||
class Tensor2 {
|
||||
|
|
@ -32,6 +33,9 @@ protected:
|
|||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** default constructor */
|
||||
Tensor2() {
|
||||
}
|
||||
|
|
@ -49,6 +53,10 @@ public:
|
|||
T[j] = a(j);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** dimension - TODO: is this right for anything other than 3x3? */
|
||||
size_t dim() const {return N1 * N2;}
|
||||
|
||||
|
|
@ -67,6 +75,9 @@ public:
|
|||
N2, J> > operator()(Index<N1, I> i, Index<N2, J> j) const {
|
||||
return Tensor2Expression<Tensor2, Index<N1, I> , Index<N2, J> > (*this);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
} // namespace tensors
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@ namespace tensors {
|
|||
/**
|
||||
* Templated class to hold a rank 2 tensor expression.
|
||||
* @ingroup tensors
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class A, class I, class J> class Tensor2Expression {
|
||||
|
||||
|
|
@ -134,11 +135,18 @@ namespace tensors {
|
|||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** constructor */
|
||||
Tensor2Expression(const A &a) :
|
||||
iter(a) {
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/** Print */
|
||||
void print(const std::string& s = "Tensor2:") const {
|
||||
std::cout << s << "{";
|
||||
|
|
@ -159,6 +167,10 @@ namespace tensors {
|
|||
return true;
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** norm */
|
||||
double norm() const {
|
||||
double sumsqr = 0.0;
|
||||
|
|
@ -207,6 +219,10 @@ namespace tensors {
|
|||
return true;
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** c(j) = a(i,j)*b(i) */
|
||||
template<class B>
|
||||
inline Tensor1Expression<ITimesRank1_<B>, J> operator*(
|
||||
|
|
@ -289,4 +305,6 @@ namespace tensors {
|
|||
return false;
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
} // namespace tensors
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@ namespace tensors {
|
|||
/**
|
||||
* Rank 3 Tensor
|
||||
* @ingroup tensors
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<int N1, int N2, int N3>
|
||||
class Tensor3 {
|
||||
|
|
@ -31,6 +32,9 @@ namespace tensors {
|
|||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** default constructor */
|
||||
Tensor3() {
|
||||
}
|
||||
|
|
@ -41,6 +45,10 @@ namespace tensors {
|
|||
T[k] = data[k];
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
/** construct from expression */
|
||||
template<class A, char I, char J, char K>
|
||||
Tensor3(const Tensor3Expression<A, Index<N1, I> , Index<N2, J> , Index<N3,
|
||||
|
|
@ -49,6 +57,10 @@ namespace tensors {
|
|||
T[k] = a(k);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// element access
|
||||
double operator()(int i, int j, int k) const {
|
||||
return T[k](i, j);
|
||||
|
|
@ -89,4 +101,6 @@ namespace tensors {
|
|||
|
||||
}; // Eta
|
||||
|
||||
/// @}
|
||||
|
||||
} // namespace tensors
|
||||
|
|
|
|||
|
|
@ -26,6 +26,7 @@ namespace tensors {
|
|||
/**
|
||||
* templated class to interface to an object A as a rank 3 tensor
|
||||
* @ingroup tensors
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class A, class I, class J, class K> class Tensor3Expression {
|
||||
A iter;
|
||||
|
|
@ -64,11 +65,18 @@ namespace tensors {
|
|||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** constructor */
|
||||
Tensor3Expression(const A &a) :
|
||||
iter(a) {
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** Print */
|
||||
void print(const std::string& s = "Tensor3:") const {
|
||||
std::cout << s << "{";
|
||||
|
|
@ -107,6 +115,10 @@ namespace tensors {
|
|||
|
||||
}; // Tensor3Expression
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Print */
|
||||
template<class A, class I, class J, class K>
|
||||
void print(const Tensor3Expression<A, I, J, K>& T, const std::string& s =
|
||||
|
|
@ -177,4 +189,6 @@ namespace tensors {
|
|||
return false;
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
} // namespace tensors
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@ namespace tensors {
|
|||
/**
|
||||
* Rank 4 Tensor
|
||||
* @ingroup tensors
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<int N1, int N2, int N3, int N4>
|
||||
class Tensor4 {
|
||||
|
|
@ -34,15 +35,24 @@ namespace tensors {
|
|||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** default constructor */
|
||||
Tensor4() {
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// element access
|
||||
double operator()(int i, int j, int k, int l) const {
|
||||
return T[l](i, j, k);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
}; // Tensor4
|
||||
|
||||
} // namespace tensors
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@ namespace tensors {
|
|||
/**
|
||||
* Rank 5 Tensor
|
||||
* @ingroup tensors
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<int N1, int N2, int N3, int N4, int N5>
|
||||
class Tensor5 {
|
||||
|
|
@ -34,10 +35,17 @@ namespace tensors {
|
|||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** default constructor */
|
||||
Tensor5() {
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** construct from expression */
|
||||
template<class A, char I, char J, char K, char L, char M>
|
||||
Tensor5(const Tensor5Expression<A, Index<N1, I> , Index<N2, J> , Index<N3,
|
||||
|
|
@ -59,6 +67,9 @@ namespace tensors {
|
|||
return Tensor5Expression<Tensor5, Index<N1, I> , Index<N2, J> , Index<N3,
|
||||
K> , Index<N4, L> , Index<N5, M> > (*this);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
}; // Tensor5
|
||||
|
||||
} // namespace tensors
|
||||
|
|
|
|||
|
|
@ -26,6 +26,7 @@ namespace tensors {
|
|||
/**
|
||||
* templated class to interface to an object A as a rank 5 tensor
|
||||
* @ingroup tensors
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template<class A, class I, class J, class K, class L, class M> class Tensor5Expression {
|
||||
A iter;
|
||||
|
|
@ -48,11 +49,18 @@ namespace tensors {
|
|||
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** constructor */
|
||||
Tensor5Expression(const A &a) :
|
||||
iter(a) {
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/** Print */
|
||||
void print(const std::string& s = "Tensor5:") const {
|
||||
std::cout << s << std::endl;
|
||||
|
|
@ -85,6 +93,10 @@ namespace tensors {
|
|||
};
|
||||
// Tensor5Expression
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
/** Print */
|
||||
template<class A, class I, class J, class K, class L, class M>
|
||||
void print(const Tensor5Expression<A, I, J, K, L, M>& T,
|
||||
|
|
@ -118,4 +130,6 @@ namespace tensors {
|
|||
return Rank3Rank2_<A, I, J, K, B, L, M>(a, b);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
} // namespace tensors
|
||||
|
|
|
|||
Loading…
Reference in New Issue