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

Refactor all Cal3D based models

release/4.3a0
Varun Agrawal 2020-12-01 17:23:10 -05:00
parent ad66a5927d
commit 42b0537402
5 changed files with 54 additions and 117 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/geometry/Cal3DS2.cpp
View File

@ -13,6 +13,7 @@
* @file Cal3DS2.cpp
* @date Feb 28, 2010
* @author ydjian
* @author Varun Agrawal
*/
#include <gtsam/base/Vector.h>
@ -30,11 +31,8 @@ void Cal3DS2::print(const std::string& s_) const {
/* ************************************************************************* */
bool Cal3DS2::equals(const Cal3DS2& 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(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol)
return false;
return true;
const Cal3DS2_Base* base = dynamic_cast<const Cal3DS2_Base*>(&K);
return Cal3DS2_Base::equals(*base, tol);
}
/* ************************************************************************* */

1
gtsam/geometry/Cal3DS2.h
View File

@ -14,6 +14,7 @@
* @brief Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base
* @date Feb 28, 2010
* @author ydjian
* @autho Varun Agrawal
*/
#pragma once

28
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -24,25 +24,6 @@
namespace gtsam {
/* ************************************************************************* */
Cal3DS2_Base::Cal3DS2_Base(const Vector& v)
: fx_(v(0)),
fy_(v(1)),
s_(v(2)),
u0_(v(3)),
v0_(v(4)),
k1_(v(5)),
k2_(v(6)),
p1_(v(7)),
p2_(v(8)) {}
/* ************************************************************************* */
Matrix3 Cal3DS2_Base::K() const {
Matrix3 K;
K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
return K;
}
/* ************************************************************************* */
Vector9 Cal3DS2_Base::vector() const {
Vector9 v;
@ -58,11 +39,10 @@ void Cal3DS2_Base::print(const std::string& s_) const {
/* ************************************************************************* */
bool Cal3DS2_Base::equals(const Cal3DS2_Base& 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(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > 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(p1_ - K.p1_) < tol &&
std::fabs(p2_ - K.p2_) < tol;
}
/* ************************************************************************* */

67
gtsam/geometry/Cal3DS2_Base.h
View File

@ -34,45 +34,29 @@ namespace gtsam {
* but using only k1,k2,p1, and p2 coefficients.
* K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
* rr = Pn.x^2 + Pn.y^2
* \hat{Pn} = (1 + k1*rr + k2*rr^2 ) Pn + [ 2*p1 Pn.x Pn.y + p2 (rr + 2 Pn.x^2) ;
* \hat{Pn} = (1 + k1*rr + k2*rr^2 ) Pn + [ 2*p1 Pn.x Pn.y + p2 (rr + 2 Pn.x^2)
* ;
* p1 (rr + 2 Pn.y^2) + 2*p2 Pn.x Pn.y ]
* pi = K*Pn
*/
class GTSAM_EXPORT Cal3DS2_Base {
class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
protected:
double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point
double k1_, k2_; // radial 2nd-order and 4th-order
double p1_, p2_; // tangential distortion
double tol_ = 1e-5; // tolerance value when calibrating
public:
enum { dimension = 9 };
/// @name Standard Constructors
/// @{
/// Default Constructor with only unit focal length
Cal3DS2_Base()
: fx_(1),
fy_(1),
s_(0),
u0_(0),
v0_(0),
k1_(0),
k2_(0),
p1_(0),
p2_(0),
tol_(1e-5) {}
Cal3DS2_Base() : Cal3(), k1_(0), k2_(0), p1_(0), p2_(0), tol_(1e-5) {}
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)
: fx_(fx),
fy_(fy),
s_(s),
u0_(u0),
v0_(v0),
: Cal3(fx, fy, s, u0, v0),
k1_(k1),
k2_(k2),
p1_(p1),
@ -85,14 +69,19 @@ public:
/// @name Advanced Constructors
/// @{
Cal3DS2_Base(const Vector &v) ;
Cal3DS2_Base(const Vector& v)
: Cal3(v(0), v(1), v(2), v(3), v(4)),
k1_(v(5)),
k2_(v(6)),
p1_(v(7)),
p2_(v(8)) {}
/// @}
/// @name Testable
/// @{
/// print with optional string
virtual void print(const std::string& s = "") const;
void print(const std::string& s = "") const override;
/// assert equality up to a tolerance
bool equals(const Cal3DS2_Base& K, double tol = 1e-8) const;
@ -101,21 +90,6 @@ public:
/// @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_; }
@ -128,9 +102,6 @@ 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_); }
@ -169,20 +140,15 @@ public:
/// @}
private:
/// @name Advanced Interface
/// @{
/** Serialization function */
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_);
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Cal3DS2_Base", boost::serialization::base_object<Cal3>(*this));
ar& BOOST_SERIALIZATION_NVP(k1_);
ar& BOOST_SERIALIZATION_NVP(k2_);
ar& BOOST_SERIALIZATION_NVP(p1_);
@ -191,8 +157,5 @@ private:
}
/// @}
};
}

15
gtsam/geometry/Cal3Unified.cpp
View File

@ -26,8 +26,8 @@
namespace gtsam {
/* ************************************************************************* */
Cal3Unified::Cal3Unified(const Vector &v):
Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {}
Cal3Unified::Cal3Unified(const Vector& v)
: Base(v(0), v(1), v(2), v(3), v(4), v(5), v(6), v(7), v(8)), xi_(v(9)) {}
/* ************************************************************************* */
Vector10 Cal3Unified::vector() const {
@ -44,12 +44,8 @@ void Cal3Unified::print(const std::string& s) const {
/* ************************************************************************* */
bool Cal3Unified::equals(const Cal3Unified& 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(p1_ - K.p1_) > tol || std::abs(p2_ - K.p2_) > tol ||
std::abs(xi_ - K.xi_) > tol)
return false;
return true;
const Cal3DS2_Base* base = dynamic_cast<const Cal3DS2_Base*>(&K);
return Cal3DS2_Base::equals(*base, tol) && std::fabs(xi_ - K.xi_) < tol;
}
/* ************************************************************************* */
@ -144,7 +140,6 @@ Vector10 Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
return T2.vector() - vector();
}
}
/* ************************************************************************* */
} // \ namespace gtsam