Loading...
Searching...
No Matches
Cal3.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/geometry/Point2.h>
namespace gtsam {
template <typename Cal, size_t Dim>
void calibrateJacobians(const Cal& calibration, const Point2& pn,
OptionalJacobian<2, Dim> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) {
if (Dcal || Dp) {
Eigen::Matrix<double, 2, Dim> H_uncal_K;
Matrix22 H_uncal_pn, H_uncal_pn_inv;
// Compute uncalibrate Jacobians
calibration.uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
H_uncal_pn_inv = H_uncal_pn.inverse();
if (Dp) *Dp = H_uncal_pn_inv;
if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
}
}
class GTSAM_EXPORT Cal3 {
protected:
double fx_ = 1.0f, fy_ = 1.0f;
double s_ = 0.0f;
double u0_ = 0.0f, v0_ = 0.0f;
public:
enum { dimension = 5 };
using shared_ptr = boost::shared_ptr<Cal3>;
Cal3() = default;
Cal3(double fx, double fy, double s, double u0, double v0)
: fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {}
Cal3(const Vector5& d)
: fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {}
Cal3(double fov, int w, int h);
virtual ~Cal3() {}
Cal3(const std::string& path);
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Cal3& cal);
virtual void print(const std::string& s = "") const;
bool equals(const Cal3& K, double tol = 10e-9) const;
inline double fx() const { return fx_; }
inline double fy() const { return fy_; }
inline double aspectRatio() const { return fx_ / fy_; }
inline double skew() const { return s_; }
inline double px() const { return u0_; }
inline double py() const { return v0_; }
Point2 principalPoint() const { return Point2(u0_, v0_); }
Vector5 vector() const {
Vector5 v;
v << fx_, fy_, s_, u0_, v0_;
return v;
}
virtual Matrix3 K() const {
Matrix3 K;
K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
return K;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
Matrix3 GTSAM_DEPRECATED matrix() const { return K(); }
#endif
Matrix3 inverse() const;
inline virtual size_t dim() const { return Dim(); }
inline static size_t Dim() { return dimension; }
private:
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_NVP(fx_);
ar& BOOST_SERIALIZATION_NVP(fy_);
ar& BOOST_SERIALIZATION_NVP(s_);
ar& BOOST_SERIALIZATION_NVP(u0_);
ar& BOOST_SERIALIZATION_NVP(v0_);
}
};
} // \ namespace gtsam
Point2.h
2D Point
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
gtsam::calibrateJacobians
void calibrateJacobians(const Cal &calibration, const Point2 &pn, OptionalJacobian< 2, Dim > Dcal=boost::none, OptionalJacobian< 2, 2 > Dp=boost::none)
Function which makes use of the Implicit Function Theorem to compute the Jacobians of calibrate using...
Definition Cal3.h:47
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::Cal3
Common base class for all calibration models.
Definition Cal3.h:69
gtsam::Cal3::Cal3
Cal3(double fx, double fy, double s, double u0, double v0)
constructor from doubles
Definition Cal3.h:87
gtsam::Cal3::K
virtual Matrix3 K() const
return calibration matrix K
Definition Cal3.h:167
gtsam::Cal3::Cal3
Cal3(const Vector5 &d)
constructor from vector
Definition Cal3.h:91
gtsam::Cal3::~Cal3
virtual ~Cal3()
Virtual destructor.
Definition Cal3.h:103
gtsam::Cal3::vector
Vector5 vector() const
vectorized form (column-wise)
Definition Cal3.h:160
gtsam::Cal3::Cal3
Cal3()=default
Create a default calibration that leaves coordinates unchanged.
gtsam::Cal3::fy_
double fy_
focal length
Definition Cal3.h:71
gtsam::Cal3::px
double px() const
image center in x
Definition Cal3.h:151
gtsam::Cal3::s_
double s_
skew
Definition Cal3.h:72
gtsam::Cal3::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition Cal3.h:185
gtsam::Cal3::principalPoint
Point2 principalPoint() const
return the principal point
Definition Cal3.h:157
gtsam::Cal3::fx
double fx() const
focal length x
Definition Cal3.h:139
gtsam::Cal3::py
double py() const
image center in y
Definition Cal3.h:154
gtsam::Cal3::skew
double skew() const
skew
Definition Cal3.h:148
gtsam::Cal3::dim
virtual size_t dim() const
return DOF, dimensionality of tangent space
Definition Cal3.h:182
gtsam::Cal3::aspectRatio
double aspectRatio() const
aspect ratio
Definition Cal3.h:145
gtsam::Cal3::v0_
double v0_
principal point
Definition Cal3.h:73
gtsam::Cal3::fy
double fy() const
focal length y
Definition Cal3.h:142