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

Wrapping Cal3Unified as a derived class of Cal3DS2_Base

release/4.3a0
dellaert 2015-01-15 16:17:53 +01:00
parent 26df490c55
commit d0579dff50
4 changed files with 100 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
gtsam.h
View File

@ -629,28 +629,13 @@ class Cal3_S2 {
void serialize() const;
};
#include <gtsam/geometry/Cal3DS2.h>
class Cal3DS2 {
#include <gtsam/geometry/Cal3DS2_Base.h>
virtual class Cal3DS2_Base {
// Standard Constructors
Cal3DS2();
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
Cal3DS2(Vector v);
Cal3DS2_Base();
// Testable
void print(string s) const;
bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Cal3DS2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3DS2& c) const;
// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
// TODO: D2d functions that start with an uppercase letter
// Standard Interface
double fx() const;
@ -658,14 +643,66 @@ class Cal3DS2 {
double skew() const;
double px() const;
double py() const;
Vector vector() const;
Vector k() const;
//Matrix K() const; //FIXME: Uppercase
double k1() const;
double k2() const;
// Action on Point2
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3DS2.h>
virtual class Cal3DS2 : gtsam::Cal3DS2_Base {
// Standard Constructors
Cal3DS2();
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2);
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2);
Cal3DS2(Vector v);
// Testable
bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
// Manifold
size_t dim() const;
static size_t Dim();
gtsam::Cal3DS2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3DS2& c) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3Unified.h>
virtual class Cal3Unified : gtsam::Cal3DS2_Base {
// Standard Constructors
Cal3Unified();
Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2);
Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi);
Cal3Unified(Vector v);
// Testable
bool equals(const gtsam::Cal3Unified& rhs, double tol) const;
// Standard Interface
double xi() const;
gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const;
gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const;
// Manifold
size_t dim() const;
static size_t Dim();
gtsam::Cal3Unified retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3Unified& c) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3_S2Stereo.h>
class Cal3_S2Stereo {
// Standard Constructors
Cal3_S2Stereo();

14
gtsam/geometry/Cal3DS2.h
View File

@ -68,7 +68,7 @@ public:
/// @{
/// print with optional string
void print(const std::string& s = "") const ;
virtual void print(const std::string& s = "") const ;
/// assert equality up to a tolerance
bool equals(const Cal3DS2& K, double tol = 10e-9) const;
@ -89,10 +89,20 @@ public:
/// Return dimensions of calibration manifold object
static size_t Dim() { return 9; } //TODO: make a final dimension variable
/// @}
/// @name Clone
/// @{
/// @return a deep copy of this object
virtual boost::shared_ptr<Base> clone() const {
return boost::shared_ptr<Base>(new Cal3DS2(*this));
}
/// @}
private:
/// @}
/// @name Advanced Interface
/// @{

28
gtsam/geometry/Cal3DS2_Base.h
View File

@ -45,9 +45,6 @@ protected:
double p1_, p2_ ; // tangential distortion
public:
Matrix3 K() const ;
Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
Vector9 vector() const ;
/// @name Standard Constructors
/// @{
@ -59,6 +56,8 @@ public:
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
virtual ~Cal3DS2_Base() {}
/// @}
/// @name Advanced Constructors
/// @{
@ -70,7 +69,7 @@ public:
/// @{
/// print with optional string
void print(const std::string& s = "") const ;
virtual void print(const std::string& s = "") const ;
/// assert equality up to a tolerance
bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
@ -106,6 +105,15 @@ public:
/// Second tangential distortion coefficient
inline double p2() const { return p2_;}
/// return calibration matrix -- not really applicable
Matrix3 K() const;
/// return distortion parameter vector
Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
/// Return all parameters as a vector
Vector9 vector() const;
/**
* convert intrinsic coordinates xy to (distorted) image coordinates uv
* @param p point in intrinsic coordinates
@ -126,9 +134,19 @@ public:
/// Derivative of uncalibrate wrpt the calibration parameters
Matrix29 D2d_calibration(const Point2& p) const ;
private:
/// @}
/// @name Clone
/// @{
/// @return a deep copy of this object
virtual boost::shared_ptr<Cal3DS2_Base> clone() const {
return boost::shared_ptr<Cal3DS2_Base>(new Cal3DS2_Base(*this));
}
/// @}
private:
/// @name Advanced Interface
/// @{

10
gtsam/geometry/Cal3Unified.h
View File

@ -50,9 +50,8 @@ private:
double xi_; // mirror parameter
public:
enum { dimension = 10 };
Vector10 vector() const ;
enum { dimension = 10 };
/// @name Standard Constructors
/// @{
@ -77,7 +76,7 @@ public:
/// @{
/// print with optional string
void print(const std::string& s = "") const ;
virtual void print(const std::string& s = "") const ;
/// assert equality up to a tolerance
bool equals(const Cal3Unified& K, double tol = 10e-9) const;
@ -125,6 +124,11 @@ public:
/// Return dimensions of calibration manifold object
static size_t Dim() { return 10; } //TODO: make a final dimension variable
/// Return all parameters as a vector
Vector10 vector() const ;
/// @}
private:
/** Serialization function */