Calibration of a omni-directional camera with mirror + lens radial distortion. More...
#include <Cal3Unified.h>
Public Types | |
enum | { dimension = 10 } |
using | shared_ptr = std::shared_ptr< Cal3Unified > |
Public Types inherited from gtsam::Cal3DS2_Base | |
enum | { dimension = 9 } |
using | shared_ptr = std::shared_ptr< Cal3DS2_Base > |
Public Types inherited from gtsam::Cal3 | |
enum | { dimension = 5 } |
using | shared_ptr = std::shared_ptr< Cal3 > |
Private Types | |
using | Base = Cal3DS2_Base |
using | This = Cal3Unified |
Private Attributes | |
double | xi_ = 0.0f |
mirror parameter More... | |
Standard Constructors | |
Cal3Unified ()=default | |
Default Constructor with only unit focal length. More... | |
Cal3Unified (double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1=0.0, double p2=0.0, double xi=0.0) | |
~Cal3Unified () override | |
Advanced Constructors | |
Cal3Unified (const Vector10 &v) | |
Testable | |
void | print (const std::string &s="") const override |
print with optional string More... | |
bool | equals (const Cal3Unified &K, double tol=10e-9) const |
assert equality up to a tolerance More... | |
GTSAM_EXPORT friend std::ostream & | operator<< (std::ostream &os, const Cal3Unified &cal) |
Output stream operator. More... | |
Standard Interface | |
double | xi () const |
mirror parameter More... | |
Vector10 | vector () const |
Return all parameters as a vector. More... | |
Point2 | uncalibrate (const Point2 &p, OptionalJacobian< 2, 10 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const |
Point2 | calibrate (const Point2 &p, OptionalJacobian< 2, 10 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const |
Conver a pixel coordinate to ideal coordinate. More... | |
Point2 | spaceToNPlane (const Point2 &p) const |
Convert a 3D point to normalized unit plane. More... | |
Point2 | nPlaneToSpace (const Point2 &p) const |
Convert a normalized unit plane point to 3D space. More... | |
Manifold | |
Cal3Unified | retract (const Vector &d) const |
Given delta vector, update calibration. More... | |
Vector | localCoordinates (const Cal3Unified &T2) const |
Given a different calibration, calculate update to obtain it. More... | |
size_t | dim () const override |
Return dimensions of calibration manifold object. More... | |
static size_t | Dim () |
Return dimensions of calibration manifold object. More... | |
Additional Inherited Members | |
Public Member Functions inherited from gtsam::Cal3DS2_Base | |
Cal3DS2_Base ()=default | |
Default Constructor with only unit focal length. More... | |
Cal3DS2_Base (double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1=0.0, double p2=0.0, double tol=1e-5) | |
~Cal3DS2_Base () override | |
Cal3DS2_Base (const Vector9 &v) | |
bool | equals (const Cal3DS2_Base &K, double tol=1e-8) const |
assert equality up to a tolerance More... | |
double | k1 () const |
First distortion coefficient. More... | |
double | k2 () const |
Second distortion coefficient. More... | |
double | p1 () const |
First tangential distortion coefficient. More... | |
double | p2 () const |
Second tangential distortion coefficient. More... | |
Vector4 | k () const |
return distortion parameter vector More... | |
Vector9 | vector () const |
Return all parameters as a vector. More... | |
Point2 | uncalibrate (const Point2 &p, OptionalJacobian< 2, 9 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const |
Point2 | calibrate (const Point2 &p, OptionalJacobian< 2, 9 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const |
Convert (distorted) image coordinates uv to intrinsic coordinates xy. More... | |
Matrix2 | D2d_intrinsic (const Point2 &p) const |
Derivative of uncalibrate wrpt intrinsic coordinates. More... | |
Matrix29 | D2d_calibration (const Point2 &p) const |
Derivative of uncalibrate wrpt the calibration parameters. More... | |
virtual std::shared_ptr< Cal3DS2_Base > | clone () const |
Public Member Functions inherited from gtsam::Cal3 | |
Cal3 ()=default | |
Create a default calibration that leaves coordinates unchanged. More... | |
Cal3 (double fx, double fy, double s, double u0, double v0) | |
constructor from doubles More... | |
Cal3 (const Vector5 &d) | |
constructor from vector More... | |
Cal3 (double fov, int w, int h) | |
virtual | ~Cal3 () |
Virtual destructor. More... | |
Cal3 (const std::string &path) | |
bool | equals (const Cal3 &K, double tol=10e-9) const |
Check if equal up to specified tolerance. More... | |
double | fx () const |
focal length x More... | |
double | fy () const |
focal length y More... | |
double | aspectRatio () const |
aspect ratio More... | |
double | skew () const |
skew More... | |
double | px () const |
image center in x More... | |
double | py () const |
image center in y More... | |
Point2 | principalPoint () const |
return the principal point More... | |
Vector5 | vector () const |
vectorized form (column-wise) More... | |
virtual Matrix3 | K () const |
return calibration matrix K More... | |
Matrix3 | inverse () const |
Return inverted calibration matrix inv(K) More... | |
Static Public Member Functions inherited from gtsam::Cal3DS2_Base | |
static size_t | Dim () |
return DOF, dimensionality of tangent space More... | |
Static Public Member Functions inherited from gtsam::Cal3 | |
static size_t | Dim () |
return DOF, dimensionality of tangent space More... | |
Protected Attributes inherited from gtsam::Cal3DS2_Base | |
double | k1_ = 0.0f |
double | k2_ = 0.0f |
radial 2nd-order and 4th-order More... | |
double | p1_ = 0.0f |
double | p2_ = 0.0f |
tangential distortion More... | |
double | tol_ = 1e-5 |
tolerance value when calibrating More... | |
Protected Attributes inherited from gtsam::Cal3 | |
double | fx_ = 1.0f |
double | fy_ = 1.0f |
focal length More... | |
double | s_ = 0.0f |
skew More... | |
double | u0_ = 0.0f |
double | v0_ = 0.0f |
principal point More... | |
Calibration of a omni-directional camera with mirror + lens radial distortion.
Similar to Cal3DS2, does distortion but has additional mirror parameter xi K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] Pn = [ P.x / (1 + xi * \sqrt{P.x² + P.y² + 1}), P.y / (1 + xi * \sqrt{P.x² + P.y² + 1})] r² = Pn.x² + Pn.y² \hat{pn} = (1 + k1*r² + k2*r⁴ ) pn + [ 2*k3 pn.x pn.y + k4 (r² + 2 Pn.x²) ; k3 (rr + 2 Pn.y²) + 2*k4 pn.x pn.y ] pi = K*pn
Definition at line 45 of file Cal3Unified.h.
|
private |
Definition at line 47 of file Cal3Unified.h.
using gtsam::Cal3Unified::shared_ptr = std::shared_ptr<Cal3Unified> |
Definition at line 56 of file Cal3Unified.h.
|
private |
Definition at line 46 of file Cal3Unified.h.
anonymous enum |
Enumerator | |
---|---|
dimension |
Definition at line 53 of file Cal3Unified.h.
|
default |
Default Constructor with only unit focal length.
|
inline |
Definition at line 64 of file Cal3Unified.h.
|
inlineoverride |
Definition at line 68 of file Cal3Unified.h.
|
inline |
Definition at line 74 of file Cal3Unified.h.
Point2 gtsam::Cal3Unified::calibrate | ( | const Point2 & | p, |
OptionalJacobian< 2, 10 > | Dcal = {} , |
||
OptionalJacobian< 2, 2 > | Dp = {} |
||
) | const |
Conver a pixel coordinate to ideal coordinate.
Definition at line 103 of file Cal3Unified.cpp.
|
inlinestatic |
Return dimensions of calibration manifold object.
Definition at line 136 of file Cal3Unified.h.
|
inlineoverridevirtual |
Return dimensions of calibration manifold object.
Reimplemented from gtsam::Cal3DS2_Base.
Definition at line 133 of file Cal3Unified.h.
bool gtsam::Cal3Unified::equals | ( | const Cal3Unified & | K, |
double | tol = 10e-9 |
||
) | const |
assert equality up to a tolerance
Definition at line 49 of file Cal3Unified.cpp.
Vector gtsam::Cal3Unified::localCoordinates | ( | const Cal3Unified & | T2 | ) | const |
Given a different calibration, calculate update to obtain it.
Definition at line 138 of file Cal3Unified.cpp.
Convert a normalized unit plane point to 3D space.
Definition at line 116 of file Cal3Unified.cpp.
|
overridevirtual |
print with optional string
Reimplemented from gtsam::Cal3DS2_Base.
Definition at line 43 of file Cal3Unified.cpp.
Cal3Unified gtsam::Cal3Unified::retract | ( | const Vector & | d | ) | const |
Given delta vector, update calibration.
Definition at line 133 of file Cal3Unified.cpp.
Convert a 3D point to normalized unit plane.
Definition at line 125 of file Cal3Unified.cpp.
Point2 gtsam::Cal3Unified::uncalibrate | ( | const Point2 & | p, |
OptionalJacobian< 2, 10 > | Dcal = {} , |
||
OptionalJacobian< 2, 2 > | Dp = {} |
||
) | const |
convert intrinsic coordinates xy to image coordinates uv
p | point in intrinsic coordinates |
Dcal | optional 2*10 Jacobian wrpt Cal3Unified parameters |
Dp | optional 2*2 Jacobian wrpt intrinsic coordinates |
Definition at line 56 of file Cal3Unified.cpp.
Vector10 gtsam::Cal3Unified::vector | ( | ) | const |
Return all parameters as a vector.
Definition at line 29 of file Cal3Unified.cpp.
|
inline |
mirror parameter
Definition at line 96 of file Cal3Unified.h.
|
friend |
Output stream operator.
Definition at line 36 of file Cal3Unified.cpp.
|
private |
mirror parameter
Definition at line 50 of file Cal3Unified.h.