46 template <
typename Cal,
size_t Dim>
51 Eigen::Matrix<double, 2, Dim> H_uncal_K;
52 Matrix22 H_uncal_pn, H_uncal_pn_inv;
55 calibration.uncalibrate(pn, Dcal ? &H_uncal_K :
nullptr, H_uncal_pn);
57 H_uncal_pn_inv = H_uncal_pn.inverse();
59 if (Dp) *Dp = H_uncal_pn_inv;
60 if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
71 double fx_ = 1.0f, fy_ = 1.0f;
73 double u0_ = 0.0f, v0_ = 0.0f;
76 enum { dimension = 5 };
78 using shared_ptr = std::shared_ptr<Cal3>;
87 Cal3(
double fx,
double fy,
double s,
double u0,
double v0)
88 : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {}
92 : fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {}
100 Cal3(
double fov,
int w,
int h);
118 Cal3(
const std::string& path);
125 GTSAM_EXPORT
friend std::ostream& operator<<(std::ostream& os,
129 virtual void print(
const std::string& s =
"")
const;
132 bool equals(
const Cal3& K,
double tol = 10e-9)
const;
139 inline double fx()
const {
return fx_; }
142 inline double fy()
const {
return fy_; }
148 inline double skew()
const {
return s_; }
151 inline double px()
const {
return u0_; }
154 inline double py()
const {
return v0_; }
162 v << fx_, fy_, s_, u0_, v0_;
167 virtual Matrix3
K()
const {
169 K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
174 Matrix3 inverse()
const;
177 inline virtual size_t dim()
const {
return Dim(); }
180 inline static size_t Dim() {
return dimension; }
187 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION 188 friend class boost::serialization::access; 190 template <
class Archive>
191 void serialize(Archive& ar,
const unsigned int ) {
192 ar& BOOST_SERIALIZATION_NVP(fx_);
193 ar& BOOST_SERIALIZATION_NVP(fy_);
194 ar& BOOST_SERIALIZATION_NVP(s_);
195 ar& BOOST_SERIALIZATION_NVP(u0_);
196 ar& BOOST_SERIALIZATION_NVP(v0_);
static size_t Dim()
return DOF, dimensionality of tangent space
Definition: Cal3.h:180
double skew() const
skew
Definition: Cal3.h:148
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
Point2 principalPoint() const
return the principal point
Definition: Cal3.h:157
Vector2 Point2
Definition: Point2.h:32
double py() const
image center in y
Definition: Cal3.h:154
virtual size_t dim() const
return DOF, dimensionality of tangent space
Definition: Cal3.h:177
Common base class for all calibration models.
Definition: Cal3.h:69
void calibrateJacobians(const Cal &calibration, const Point2 &pn, OptionalJacobian< 2, Dim > Dcal={}, OptionalJacobian< 2, 2 > Dp={})
Definition: Cal3.h:47
virtual ~Cal3()
Virtual destructor.
Definition: Cal3.h:103
Definition: Testable.h:112
GTSAM_EXPORT void print(const Matrix &A, const std::string &s, std::ostream &stream)
double fx() const
focal length x
Definition: Cal3.h:139
double aspectRatio() const
aspect ratio
Definition: Cal3.h:145
Cal3(double fx, double fy, double s, double u0, double v0)
constructor from doubles
Definition: Cal3.h:87
virtual Matrix3 K() const
return calibration matrix K
Definition: Cal3.h:167
Definition: chartTesting.h:28
Definition: OptionalJacobian.h:38
Vector5 vector() const
vectorized form (column-wise)
Definition: Cal3.h:160
double fy() const
focal length y
Definition: Cal3.h:142
Cal3(const Vector5 &d)
constructor from vector
Definition: Cal3.h:91
double px() const
image center in x
Definition: Cal3.h:151