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

Override dim(), cleanup, and add unicode

release/4.3a0
Varun Agrawal 2020-12-02 16:19:05 -05:00
parent ecb0af57fd
commit fc0fd1a125
13 changed files with 78 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/geometry/Cal3.cpp
View File

@ -27,7 +27,6 @@ namespace gtsam {
Cal3::Cal3(double fov, int w, int h)
: s_(0), u0_((double)w / 2.0), v0_((double)h / 2.0) {
double a = fov * M_PI / 360.0; // fov/2 in radians
// old formula: fx_ = (double) w * tan(a);
fx_ = double(w) / (2.0 * tan(a));
fy_ = fx_;
}
@ -63,6 +62,14 @@ bool Cal3::equals(const Cal3& K, double tol) const {
std::fabs(v0_ - K.v0_) < tol);
}
Matrix3 Cal3::inverse() const {
const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
Matrix3 K_inverse;
K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, 1.0 / fy_,
-v0_ / fy_, 0.0, 0.0, 1.0;
return K_inverse;
}
/* ************************************************************************* */
} // \ namespace gtsam

16
gtsam/geometry/Cal3.h
View File

@ -161,7 +161,7 @@ class GTSAM_EXPORT Cal3 {
}
/// return calibration matrix K
Matrix3 K() const {
virtual Matrix3 K() const {
Matrix3 K;
K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
return K;
@ -173,13 +173,13 @@ class GTSAM_EXPORT Cal3 {
#endif
/// Return inverted calibration matrix inv(K)
Matrix3 inverse() const {
const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
Matrix3 K_inverse;
K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, 1.0 / fy_,
-v0_ / fy_, 0.0, 0.0, 1.0;
return K_inverse;
}
Matrix3 inverse() const;
/// return DOF, dimensionality of tangent space
inline virtual size_t dim() const { return Dim(); }
/// return DOF, dimensionality of tangent space
inline static size_t Dim() { return dimension; }
/// @}
/// @name Advanced Interface

22
gtsam/geometry/Cal3Bundler.cpp
View File

@ -25,8 +25,9 @@ namespace gtsam {
/* ************************************************************************* */
Matrix3 Cal3Bundler::K() const {
// This function is needed to ensure skew = 0;
Matrix3 K;
K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1;
K << fx_, 0, u0_, 0, fy_, v0_, 0, 0, 1.0;
return K;
}
@ -39,7 +40,7 @@ Vector4 Cal3Bundler::k() const {
/* ************************************************************************* */
Vector3 Cal3Bundler::vector() const {
return Vector3(f_, k1_, k2_);
return Vector3(fx_, k1_, k2_);
}
/* ************************************************************************* */
@ -51,12 +52,13 @@ std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) {
/* ************************************************************************* */
void Cal3Bundler::print(const std::string& s) const {
gtsam::print((Vector)(Vector(5) << f_, k1_, k2_, u0_, v0_).finished(), s + ".K");
gtsam::print((Vector)(Vector(5) << fx_, k1_, k2_, u0_, v0_).finished(), s + ".K");
}
/* ************************************************************************* */
bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
return (std::fabs(f_ - K.f_) < tol && std::fabs(k1_ - K.k1_) < tol &&
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(u0_ - K.u0_) < tol &&
std::fabs(v0_ - K.v0_) < tol);
}
@ -64,14 +66,16 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
/* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p, //
OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const {
// r = x^2 + y^2;
// g = (1 + k(1)*r + k(2)*r^2);
// r = x² + y²;
// g = (1 + k(1)*r + k(2)*r²);
// pi(:,i) = g * pn(:,i)
const double x = p.x(), y = p.y();
const double r = x * x + y * y;
const double g = 1. + (k1_ + k2_ * r) * r;
const double u = g * x, v = g * y;
const double f_ = fx_;
// Derivatives make use of intermediate variables above
if (Dcal) {
double rx = r * x, ry = r * y;
@ -92,9 +96,9 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
Point2 Cal3Bundler::calibrate(const Point2& pi,
OptionalJacobian<2, 3> Dcal,
OptionalJacobian<2, 2> Dp) const {
// Copied from Cal3DS2 :-(
// but specialized with k1,k2 non-zero only and fx=fy and s=0
double x = (pi.x() - u0_)/f_, y = (pi.y() - v0_)/f_;
// Copied from Cal3DS2
// but specialized with k1, k2 non-zero only and fx=fy and s=0
double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_;
const Point2 invKPi(x, y);
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary

26
gtsam/geometry/Cal3Bundler.h
View File

@ -14,6 +14,7 @@
* @brief Calibration used by Bundler
* @date Sep 25, 2010
* @author Yong Dian Jian
* @author Varun Agrawal
*/
#pragma once
@ -31,11 +32,11 @@ namespace gtsam {
class GTSAM_EXPORT Cal3Bundler : public Cal3 {
private:
double f_ = 1.0f; ///< focal length
double k1_ = 0.0f, k2_ = 0.0f; ///< radial distortion
double tol_ = 1e-5; ///< tolerance value when calibrating
// NOTE: image center parameters (u0, v0) are not optimized
// NOTE: We use the base class fx to represent the common focal length.
// Also, image center parameters (u0, v0) are not optimized
// but are treated as constants.
public:
@ -59,7 +60,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
*/
Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
double tol = 1e-5)
: Cal3(f, f, 0, u0, v0), f_(f), k1_(k1), k2_(k2), tol_(tol) {}
: Cal3(f, f, 0, u0, v0), k1_(k1), k2_(k2), tol_(tol) {}
virtual ~Cal3Bundler() {}
@ -81,16 +82,6 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
/// @name Standard Interface
/// @{
/// focal length x
inline double fx() const {
return f_;
}
/// focal length y
inline double fy() const {
return f_;
}
/// distorsion parameter k1
inline double k1() const {
return k1_;
@ -111,7 +102,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
return v0_;
}
Matrix3 K() const; ///< Standard 3*3 calibration matrix
Matrix3 K() const override; ///< Standard 3*3 calibration matrix
Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
Vector3 vector() const;
@ -165,14 +156,14 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
/// @{
/// return DOF, dimensionality of tangent space
virtual size_t dim() const { return dimension; }
virtual size_t dim() const override { return Dim(); }
/// return DOF, dimensionality of tangent space
static size_t Dim() { return dimension; }
inline static size_t Dim() { return dimension; }
/// Update calibration with tangent space delta
inline Cal3Bundler retract(const Vector& d) const {
return Cal3Bundler(f_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
return Cal3Bundler(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
}
/// Calculate local coordinates to another calibration
@ -192,7 +183,6 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
void serialize(Archive & ar, const unsigned int /*version*/) {
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_);

4
gtsam/geometry/Cal3DS2.h
View File

@ -81,10 +81,10 @@ public:
Vector localCoordinates(const Cal3DS2& T2) const ;
/// Return dimensions of calibration manifold object
virtual size_t dim() const { return dimension ; }
virtual size_t dim() const override { return Dim(); }
/// Return dimensions of calibration manifold object
static size_t Dim() { return dimension; }
inline static size_t Dim() { return dimension; }
/// @}
/// @name Clone

6
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -93,9 +93,9 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
/* ************************************************************************* */
Point2 Cal3DS2_Base::uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal,
OptionalJacobian<2, 2> Dp) const {
// rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2);
// dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
// r² = x² + y²;
// g = (1 + k(1)*r² + k(2)*r⁴);
// dp = [2*k(3)*x*y + k(4)*(r² + 2*x²); 2*k(4)*x*y + k(3)*(r² + 2*y²)];
// pi(:,i) = g * pn(:,i) + dp;
const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy;

15
gtsam/geometry/Cal3DS2_Base.h
View File

@ -33,11 +33,10 @@ namespace gtsam {
* http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
* 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)
* ;
* p1 (rr + 2 Pn.y^2) + 2*p2 Pn.x Pn.y ]
* pi = K*Pn
* r² = P.x² + P.y²
* P̂ = (1 + k1*r² + k2*r⁴) P + [ (2*p1 P.x P.y) + p2 (r² + 2 Pn.x²)
* p1 (r² + 2 Pn.y²) + (2*p2 Pn.x Pn.y) ]
* pi = K*P̂
*/
class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
protected:
@ -132,6 +131,12 @@ class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
/// Derivative of uncalibrate wrpt the calibration parameters
Matrix29 D2d_calibration(const Point2& p) const;
/// return DOF, dimensionality of tangent space
virtual size_t dim() const override { return Dim(); }
/// return DOF, dimensionality of tangent space
inline static size_t Dim() { return dimension; }
/// @}
/// @name Clone
/// @{

8
gtsam/geometry/Cal3Fisheye.h
View File

@ -38,9 +38,9 @@ namespace gtsam {
* Intrinsic coordinates:
* [x_i;y_i] = [x/z; y/z]
* Distorted coordinates:
* r^2 = (x_i)^2 + (y_i)^2
* r² = (x_i)² + (y_i)²
* th = atan(r)
* th_d = th(1 + k1*th^2 + k2*th^4 + k3*th^6 + k4*th^8)
* th_d = th(1 + k1*th² + k2*th⁴ + k3*th⁶ + k4*th⁸)
* [x_d; y_d] = (th_d / r)*[x_i; y_i]
* Pixel coordinates:
* K = [fx s u0; 0 fy v0 ;0 0 1]
@ -152,10 +152,10 @@ class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
/// @{
/// Return dimensions of calibration manifold object
virtual size_t dim() const { return dimension; }
virtual size_t dim() const override { return Dim(); }
/// Return dimensions of calibration manifold object
static size_t Dim() { return dimension; }
inline static size_t Dim() { return dimension; }
/// Given delta vector, update calibration
inline Cal3Fisheye retract(const Vector& d) const {

12
gtsam/geometry/Cal3Unified.h
View File

@ -34,10 +34,10 @@ namespace gtsam {
*
* 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^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})]
* rr = Pn.x^2 + Pn.y^2
* \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* 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
*/
class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
@ -127,10 +127,10 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
Vector localCoordinates(const Cal3Unified& T2) const ;
/// Return dimensions of calibration manifold object
virtual size_t dim() const { return dimension ; }
virtual size_t dim() const override { return Dim(); }
/// Return dimensions of calibration manifold object
static size_t Dim() { return dimension; }
inline static size_t Dim() { return dimension; }
/// @}

5
gtsam/geometry/Cal3_S2.cpp
View File

@ -55,8 +55,9 @@ Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
const double u = p.x(), v = p.y();
double delta_u = u - u0_, delta_v = v - v0_;
double inv_fx = 1 / fx_, inv_fy = 1 / fy_;
double inv_fy_delta_v = inv_fy * delta_v,
inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
double inv_fy_delta_v = inv_fy * delta_v;
double inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v);
if (Dcal)
*Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,

5
gtsam/geometry/Cal3_S2.h
View File

@ -115,10 +115,7 @@ class GTSAM_EXPORT Cal3_S2 : public Cal3 {
/// @{
/// return DOF, dimensionality of tangent space
inline size_t dim() const { return dimension; }
/// return DOF, dimensionality of tangent space
static size_t Dim() { return dimension; }
inline static size_t Dim() { return dimension; }
/// Given 5-dim tangent vector, create new calibration
inline Cal3_S2 retract(const Vector& d) const {

6
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -79,7 +79,7 @@ namespace gtsam {
const Cal3_S2& calibration() const { return *this; }
/// return calibration matrix K, same for left and right
Matrix3 K() const { return K(); }
Matrix3 K() const override { return Cal3_S2::K(); }
/// return baseline
inline double baseline() const { return b_; }
@ -96,10 +96,10 @@ namespace gtsam {
/// @{
/// return DOF, dimensionality of tangent space
inline size_t dim() const { return dimension; }
inline size_t dim() const override { return Dim(); }
/// return DOF, dimensionality of tangent space
static size_t Dim() { return dimension; }
inline static size_t Dim() { return dimension; }
/// Given 6-dim tangent vector, create new calibration
inline Cal3_S2Stereo retract(const Vector& d) const {

23
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -29,8 +29,7 @@ static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
static Point2 p(2,3);
/* ************************************************************************* */
TEST(Cal3Bundler, vector)
{
TEST(Cal3Bundler, vector) {
Cal3Bundler K;
Vector expected(3);
expected << 1, 0, 0;
@ -38,8 +37,7 @@ TEST(Cal3Bundler, vector)
}
/* ************************************************************************* */
TEST(Cal3Bundler, uncalibrate)
{
TEST(Cal3Bundler, uncalibrate) {
Vector v = K.vector() ;
double r = p.x()*p.x() + p.y()*p.y() ;
double g = v[0]*(1+v[1]*r+v[2]*r*r) ;
@ -48,8 +46,7 @@ TEST(Cal3Bundler, uncalibrate)
CHECK(assert_equal(expected,actual));
}
TEST(Cal3Bundler, calibrate )
{
TEST(Cal3Bundler, calibrate ) {
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Point2 pn_hat = K.calibrate(pi);
@ -62,8 +59,7 @@ Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibra
Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); }
/* ************************************************************************* */
TEST(Cal3Bundler, Duncalibrate)
{
TEST(Cal3Bundler, Duncalibrate) {
Matrix Dcal, Dp;
Point2 actual = K.uncalibrate(p, Dcal, Dp);
Point2 expected(2182, 3773);
@ -75,8 +71,7 @@ TEST(Cal3Bundler, Duncalibrate)
}
/* ************************************************************************* */
TEST(Cal3Bundler, Dcalibrate)
{
TEST(Cal3Bundler, Dcalibrate) {
Matrix Dcal, Dp;
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
@ -89,15 +84,15 @@ TEST(Cal3Bundler, Dcalibrate)
}
/* ************************************************************************* */
TEST(Cal3Bundler, assert_equal)
{
TEST(Cal3Bundler, assert_equal) {
CHECK(assert_equal(K,K,1e-7));
}
/* ************************************************************************* */
TEST(Cal3Bundler, retract)
{
TEST(Cal3Bundler, retract) {
Cal3Bundler expected(510, 2e-3, 2e-3, 1000, 2000);
EXPECT_LONGS_EQUAL(3, expected.dim());
Vector3 d;
d << 10, 1e-3, 1e-3;
Cal3Bundler actual = K.retract(d);