Refactor Bundler and Fisheye models
Varun Agrawal
parent
42b0537402
commit
17e9b73fb6
|
|
@ -23,16 +23,6 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3Bundler::Cal3Bundler() :
|
||||
f_(1), k1_(0), k2_(0), u0_(0), v0_(0), tol_(1e-5) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0,
|
||||
double tol)
|
||||
: f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0), tol_(tol) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 Cal3Bundler::K() const {
|
||||
Matrix3 K;
|
||||
|
|
@ -59,11 +49,9 @@ void Cal3Bundler::print(const std::string& s) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
|
||||
if (std::abs(f_ - K.f_) > tol || std::abs(k1_ - K.k1_) > tol
|
||||
|| std::abs(k2_ - K.k2_) > tol || std::abs(u0_ - K.u0_) > tol
|
||||
|| std::abs(v0_ - K.v0_) > tol)
|
||||
return false;
|
||||
return true;
|
||||
return (std::fabs(f_ - K.f_) < tol && std::fabs(k1_ - K.k1_) < tol &&
|
||||
std::fabs(k2_ - K.k2_) < tol && std::fabs(u0_ - K.u0_) < tol &&
|
||||
std::fabs(v0_ - K.v0_) < tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -150,14 +138,4 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
|
|||
return H;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3Bundler Cal3Bundler::retract(const Vector& d) const {
|
||||
return Cal3Bundler(f_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const {
|
||||
return T2.vector() - vector();
|
||||
}
|
||||
|
||||
}
|
||||
} // \ namespace gtsam
|
||||
|
|
|
|||
|
|
@ -28,15 +28,17 @@ namespace gtsam {
|
|||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3Bundler {
|
||||
class GTSAM_EXPORT Cal3Bundler : public Cal3 {
|
||||
|
||||
private:
|
||||
private:
|
||||
double f_; ///< focal length
|
||||
double k1_, k2_; ///< radial distortion
|
||||
double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
|
||||
double tol_; ///< tolerance value when calibrating
|
||||
|
||||
public:
|
||||
// NOTE: image center parameters (u0, v0) are not optimized
|
||||
// but are constants.
|
||||
|
||||
public:
|
||||
|
||||
enum { dimension = 3 };
|
||||
|
||||
|
|
@ -44,7 +46,7 @@ public:
|
|||
/// @{
|
||||
|
||||
/// Default constructor
|
||||
Cal3Bundler();
|
||||
Cal3Bundler() : Cal3(), f_(1), k1_(0), k2_(0), tol_(1e-5) {}
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
|
|
@ -56,7 +58,8 @@ public:
|
|||
* @param tol optional calibration tolerance value
|
||||
*/
|
||||
Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
|
||||
double tol = 1e-5);
|
||||
double tol = 1e-5)
|
||||
: Cal3(f, f, 0, u0, v0), f_(f), k1_(k1), k2_(k2), tol_(tol) {}
|
||||
|
||||
virtual ~Cal3Bundler() {}
|
||||
|
||||
|
|
@ -65,7 +68,7 @@ public:
|
|||
/// @{
|
||||
|
||||
/// print with optional string
|
||||
void print(const std::string& s = "") const;
|
||||
void print(const std::string& s = "") const override;
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const Cal3Bundler& K, double tol = 10e-9) const;
|
||||
|
|
@ -74,11 +77,6 @@ public:
|
|||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
Matrix3 K() const; ///< Standard 3*3 calibration matrix
|
||||
Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
|
||||
|
||||
Vector3 vector() const;
|
||||
|
||||
/// focal length x
|
||||
inline double fx() const {
|
||||
return f_;
|
||||
|
|
@ -109,6 +107,11 @@ public:
|
|||
return v0_;
|
||||
}
|
||||
|
||||
Matrix3 K() const; ///< Standard 3*3 calibration matrix
|
||||
Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
|
||||
|
||||
Vector3 vector() const;
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
|
||||
/// get parameter u0
|
||||
inline double u0() const {
|
||||
|
|
@ -121,12 +124,11 @@ public:
|
|||
}
|
||||
#endif
|
||||
|
||||
|
||||
/**
|
||||
* @brief: convert intrinsic coordinates xy to image coordinates uv
|
||||
* Version of uncalibrate with derivatives
|
||||
* @param p point in intrinsic coordinates
|
||||
* @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
|
||||
* @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in image coordinates
|
||||
*/
|
||||
|
|
@ -158,23 +160,23 @@ public:
|
|||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// return DOF, dimensionality of tangent space
|
||||
virtual size_t dim() const { return dimension; }
|
||||
|
||||
/// return DOF, dimensionality of tangent space
|
||||
static size_t Dim() { return dimension; }
|
||||
|
||||
/// Update calibration with tangent space delta
|
||||
Cal3Bundler retract(const Vector& d) const;
|
||||
inline Cal3Bundler retract(const Vector& d) const {
|
||||
return Cal3Bundler(f_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
|
||||
}
|
||||
|
||||
/// Calculate local coordinates to another calibration
|
||||
Vector3 localCoordinates(const Cal3Bundler& T2) const;
|
||||
|
||||
/// dimensionality
|
||||
virtual size_t dim() const {
|
||||
return 3;
|
||||
Vector3 localCoordinates(const Cal3Bundler& T2) const {
|
||||
return T2.vector() - vector();
|
||||
}
|
||||
|
||||
/// dimensionality
|
||||
static size_t Dim() {
|
||||
return 3;
|
||||
}
|
||||
|
||||
private:
|
||||
private:
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
|
|
@ -184,12 +186,12 @@ private:
|
|||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_NVP(f_);
|
||||
ar & BOOST_SERIALIZATION_NVP(k1_);
|
||||
ar & BOOST_SERIALIZATION_NVP(k2_);
|
||||
ar & BOOST_SERIALIZATION_NVP(u0_);
|
||||
ar & BOOST_SERIALIZATION_NVP(v0_);
|
||||
ar & BOOST_SERIALIZATION_NVP(tol_);
|
||||
ar& boost::serialization::make_nvp(
|
||||
"Cal3Bundler", boost::serialization::base_object<Cal3>(*this));
|
||||
ar& BOOST_SERIALIZATION_NVP(f_);
|
||||
ar& BOOST_SERIALIZATION_NVP(k1_);
|
||||
ar& BOOST_SERIALIZATION_NVP(k2_);
|
||||
ar& BOOST_SERIALIZATION_NVP(tol_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@
|
|||
* @file Cal3Fisheye.cpp
|
||||
* @date Apr 8, 2020
|
||||
* @author ghaggin
|
||||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
|
@ -23,18 +24,6 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3Fisheye::Cal3Fisheye(const Vector9& v)
|
||||
: fx_(v[0]),
|
||||
fy_(v[1]),
|
||||
s_(v[2]),
|
||||
u0_(v[3]),
|
||||
v0_(v[4]),
|
||||
k1_(v[5]),
|
||||
k2_(v[6]),
|
||||
k3_(v[7]),
|
||||
k4_(v[8]) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector9 Cal3Fisheye::vector() const {
|
||||
Vector9 v;
|
||||
|
|
@ -42,13 +31,6 @@ Vector9 Cal3Fisheye::vector() const {
|
|||
return v;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 Cal3Fisheye::K() const {
|
||||
Matrix3 K;
|
||||
K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
|
||||
return K;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Cal3Fisheye::Scaling(double r) {
|
||||
static constexpr double threshold = 1e-8;
|
||||
|
|
@ -165,24 +147,12 @@ void Cal3Fisheye::print(const std::string& s_) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
bool Cal3Fisheye::equals(const Cal3Fisheye& K, double tol) const {
|
||||
if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol ||
|
||||
std::abs(s_ - K.s_) > tol || std::abs(u0_ - K.u0_) > tol ||
|
||||
std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
|
||||
std::abs(k2_ - K.k2_) > tol || std::abs(k3_ - K.k3_) > tol ||
|
||||
std::abs(k4_ - K.k4_) > tol)
|
||||
return false;
|
||||
return true;
|
||||
const Cal3* base = dynamic_cast<const Cal3*>(&K);
|
||||
return Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol &&
|
||||
std::fabs(k2_ - K.k2_) < tol && std::fabs(k3_ - K.k3_) < tol &&
|
||||
std::fabs(k4_ - K.k4_) < tol;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3Fisheye Cal3Fisheye::retract(const Vector& d) const {
|
||||
return Cal3Fisheye(vector() + d);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Cal3Fisheye::localCoordinates(const Cal3Fisheye& T2) const {
|
||||
return T2.vector() - vector();
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
/* ************************************************************************* */
|
||||
} // \ namespace gtsam
|
||||
|
|
|
|||
|
|
@ -46,11 +46,10 @@ namespace gtsam {
|
|||
* K = [fx s u0; 0 fy v0 ;0 0 1]
|
||||
* [u; v; 1] = K*[x_d; y_d; 1]
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3Fisheye {
|
||||
class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
|
||||
private:
|
||||
double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point
|
||||
double k1_, k2_, k3_, k4_; // fisheye distortion coefficients
|
||||
double tol_ = 1e-5; // tolerance value when calibrating
|
||||
double k1_, k2_, k3_, k4_; ///< fisheye distortion coefficients
|
||||
double tol_ = 1e-5; ///< tolerance value when calibrating
|
||||
|
||||
public:
|
||||
enum { dimension = 9 };
|
||||
|
|
@ -61,17 +60,12 @@ class GTSAM_EXPORT Cal3Fisheye {
|
|||
/// @{
|
||||
|
||||
/// Default Constructor with only unit focal length
|
||||
Cal3Fisheye()
|
||||
: fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0), tol_(1e-5) {}
|
||||
Cal3Fisheye() : Cal3(), k1_(0), k2_(0), k3_(0), k4_(0), tol_(1e-5) {}
|
||||
|
||||
Cal3Fisheye(const double fx, const double fy, const double s, const double u0,
|
||||
const double v0, const double k1, const double k2,
|
||||
const double k3, const double k4, double tol = 1e-5)
|
||||
: fx_(fx),
|
||||
fy_(fy),
|
||||
s_(s),
|
||||
u0_(u0),
|
||||
v0_(v0),
|
||||
: Cal3(fx, fy, s, u0, v0),
|
||||
k1_(k1),
|
||||
k2_(k2),
|
||||
k3_(k3),
|
||||
|
|
@ -84,27 +78,17 @@ class GTSAM_EXPORT Cal3Fisheye {
|
|||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
||||
explicit Cal3Fisheye(const Vector9& v);
|
||||
explicit Cal3Fisheye(const Vector9& v)
|
||||
: Cal3(v(0), v(1), v(2), v(3), v(4)),
|
||||
k1_(v(5)),
|
||||
k2_(v(6)),
|
||||
k3_(v(7)),
|
||||
k4_(v(8)) {}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// focal length x
|
||||
inline double fx() const { return fx_; }
|
||||
|
||||
/// focal length x
|
||||
inline double fy() const { return fy_; }
|
||||
|
||||
/// skew
|
||||
inline double skew() const { return s_; }
|
||||
|
||||
/// image center in x
|
||||
inline double px() const { return u0_; }
|
||||
|
||||
/// image center in y
|
||||
inline double py() const { return v0_; }
|
||||
|
||||
/// First distortion coefficient
|
||||
inline double k1() const { return k1_; }
|
||||
|
||||
|
|
@ -117,9 +101,6 @@ class GTSAM_EXPORT Cal3Fisheye {
|
|||
/// Second tangential distortion coefficient
|
||||
inline double k4() const { return k4_; }
|
||||
|
||||
/// return calibration matrix
|
||||
Matrix3 K() const;
|
||||
|
||||
/// return distortion parameter vector
|
||||
Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); }
|
||||
|
||||
|
|
@ -133,16 +114,21 @@ class GTSAM_EXPORT Cal3Fisheye {
|
|||
* @brief convert intrinsic coordinates [x_i; y_i] to (distorted) image
|
||||
* coordinates [u; v]
|
||||
* @param p point in intrinsic coordinates
|
||||
* @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters (fx, fy, ...,
|
||||
* k4)
|
||||
* @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
|
||||
* @return point in (distorted) image coordinates
|
||||
*/
|
||||
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
|
||||
OptionalJacobian<2, 2> Dp = boost::none) const;
|
||||
|
||||
/// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
|
||||
/// y_i]
|
||||
/**
|
||||
* Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
|
||||
* y_i]
|
||||
* @param p point in image coordinates
|
||||
* @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
|
||||
* @return point in intrinsic coordinates
|
||||
*/
|
||||
Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
|
||||
OptionalJacobian<2, 2> Dp = boost::none) const;
|
||||
|
||||
|
|
@ -151,7 +137,7 @@ class GTSAM_EXPORT Cal3Fisheye {
|
|||
/// @{
|
||||
|
||||
/// print with optional string
|
||||
virtual void print(const std::string& s = "") const;
|
||||
virtual void print(const std::string& s = "") const override;
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const Cal3Fisheye& K, double tol = 10e-9) const;
|
||||
|
|
@ -160,17 +146,21 @@ class GTSAM_EXPORT Cal3Fisheye {
|
|||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// Return dimensions of calibration manifold object
|
||||
virtual size_t dim() const { return dimension; }
|
||||
|
||||
/// Return dimensions of calibration manifold object
|
||||
static size_t Dim() { return dimension; }
|
||||
|
||||
/// Given delta vector, update calibration
|
||||
Cal3Fisheye retract(const Vector& d) const;
|
||||
inline Cal3Fisheye retract(const Vector& d) const {
|
||||
return Cal3Fisheye(vector() + d);
|
||||
}
|
||||
|
||||
/// Given a different calibration, calculate update to obtain it
|
||||
Vector localCoordinates(const Cal3Fisheye& T2) const;
|
||||
|
||||
/// Return dimensions of calibration manifold object
|
||||
virtual size_t dim() const { return 9; }
|
||||
|
||||
/// Return dimensions of calibration manifold object
|
||||
static size_t Dim() { return 9; }
|
||||
Vector localCoordinates(const Cal3Fisheye& T2) const {
|
||||
return T2.vector() - vector();
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Clone
|
||||
|
|
@ -191,11 +181,8 @@ class GTSAM_EXPORT Cal3Fisheye {
|
|||
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_);
|
||||
ar& boost::serialization::make_nvp(
|
||||
"Cal3Fisheye", boost::serialization::base_object<Cal3>(*this));
|
||||
ar& BOOST_SERIALIZATION_NVP(k1_);
|
||||
ar& BOOST_SERIALIZATION_NVP(k2_);
|
||||
ar& BOOST_SERIALIZATION_NVP(k3_);
|
||||
|
|
|
|||
Loading…
Reference in New Issue