Split off Cal3f from Cal3Bundler
Frank Dellaert
parent
4057e41a80
commit
cd6c0b0a69
|
|
@ -56,10 +56,8 @@ void Cal3Bundler::print(const std::string& s) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
|
||||
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);
|
||||
return Cal3f::equals(static_cast<const Cal3f&>(K), tol) &&
|
||||
std::fabs(k1_ - K.k1_) < tol && std::fabs(k2_ - K.k2_) < tol;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -19,7 +19,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Cal3.h>
|
||||
#include <gtsam/geometry/Cal3f.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -29,22 +29,18 @@ namespace gtsam {
|
|||
* @ingroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3Bundler : public Cal3 {
|
||||
class GTSAM_EXPORT Cal3Bundler : public Cal3f {
|
||||
private:
|
||||
double k1_ = 0.0f, k2_ = 0.0f; ///< radial distortion
|
||||
double tol_ = 1e-5; ///< tolerance value when calibrating
|
||||
double k1_ = 0.0, k2_ = 0.0; ///< radial distortion coefficients
|
||||
double tol_ = 1e-5; ///< tolerance value when calibrating
|
||||
|
||||
// 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.
|
||||
// Note: u0 and v0 are constants and not optimized.
|
||||
|
||||
public:
|
||||
enum { dimension = 3 };
|
||||
|
||||
///< shared pointer to stereo calibration object
|
||||
using shared_ptr = std::shared_ptr<Cal3Bundler>;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/// Default constructor
|
||||
|
|
@ -61,9 +57,9 @@ 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), k1_(k1), k2_(k2), tol_(tol) {}
|
||||
: Cal3f(f, u0, v0), k1_(k1), k2_(k2), tol_(tol) {}
|
||||
|
||||
~Cal3Bundler() override {}
|
||||
~Cal3Bundler() override = default;
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
|
|
@ -77,24 +73,18 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
|
|||
void print(const std::string& s = "") const override;
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const Cal3Bundler& K, double tol = 10e-9) const;
|
||||
bool equals(const Cal3Bundler& K, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// distorsion parameter k1
|
||||
/// distortion parameter k1
|
||||
inline double k1() const { return k1_; }
|
||||
|
||||
/// distorsion parameter k2
|
||||
/// distortion parameter k2
|
||||
inline double k2() const { return k2_; }
|
||||
|
||||
/// image center in x
|
||||
inline double px() const { return u0_; }
|
||||
|
||||
/// image center in y
|
||||
inline double py() const { return v0_; }
|
||||
|
||||
Matrix3 K() const override; ///< Standard 3*3 calibration matrix
|
||||
Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
|
||||
|
||||
|
|
@ -113,8 +103,7 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
|
|||
|
||||
/**
|
||||
* Convert a pixel coordinate to ideal coordinate xy
|
||||
* @param p point in image coordinates
|
||||
* @param tol optional tolerance threshold value for iterative minimization
|
||||
* @param pi point in image coordinates
|
||||
* @param Dcal optional 2*3 Jacobian wrpt Cal3Bundler parameters
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in intrinsic coordinates
|
||||
|
|
@ -135,14 +124,14 @@ class GTSAM_EXPORT Cal3Bundler : public Cal3 {
|
|||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// return DOF, dimensionality of tangent space
|
||||
/// Return DOF, dimensionality of tangent space
|
||||
size_t dim() const override { return Dim(); }
|
||||
|
||||
/// return DOF, dimensionality of tangent space
|
||||
/// Return DOF, dimensionality of tangent space
|
||||
inline static size_t Dim() { return dimension; }
|
||||
|
||||
/// Update calibration with tangent space delta
|
||||
inline Cal3Bundler retract(const Vector& d) const {
|
||||
Cal3Bundler retract(const Vector& d) const {
|
||||
return Cal3Bundler(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,106 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Cal3f.cpp
|
||||
* @brief Implementation file for Cal3f class
|
||||
* @author Frank Dellaert
|
||||
* @date October 2024
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/geometry/Cal3f.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3f::Cal3f(double f, double u0, double v0) : Cal3(f, f, 0.0, u0, v0) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::ostream& operator<<(std::ostream& os, const Cal3f& cal) {
|
||||
os << "f: " << cal.fx_ << ", px: " << cal.u0_ << ", py: " << cal.v0_;
|
||||
return os;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Cal3f::print(const std::string& s) const {
|
||||
if (!s.empty()) std::cout << s << " ";
|
||||
std::cout << *this << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Cal3f::equals(const Cal3f& K, double tol) const {
|
||||
return Cal3::equals(static_cast<const Cal3&>(K), tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector1 Cal3f::vector() const {
|
||||
Vector1 v;
|
||||
v << fx_;
|
||||
return v;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3f::uncalibrate(const Point2& p, OptionalJacobian<2, 1> Dcal,
|
||||
OptionalJacobian<2, 2> Dp) const {
|
||||
assert(fx_ == fy_ && s_ == 0.0);
|
||||
const double x = p.x(), y = p.y();
|
||||
const double u = fx_ * x + u0_;
|
||||
const double v = fx_ * y + v0_;
|
||||
|
||||
if (Dcal) {
|
||||
Dcal->resize(2, 1);
|
||||
(*Dcal) << x, y;
|
||||
}
|
||||
|
||||
if (Dp) {
|
||||
Dp->resize(2, 2);
|
||||
(*Dp) << fx_, 0.0, //
|
||||
0.0, fx_;
|
||||
}
|
||||
|
||||
return Point2(u, v);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3f::calibrate(const Point2& pi, OptionalJacobian<2, 1> Dcal,
|
||||
OptionalJacobian<2, 2> Dp) const {
|
||||
assert(fx_ == fy_ && s_ == 0.0);
|
||||
const double u = pi.x(), v = pi.y();
|
||||
double delta_u = u - u0_, delta_v = v - v0_;
|
||||
double inv_f = 1.0 / fx_;
|
||||
Point2 point(inv_f * delta_u, inv_f * delta_v);
|
||||
|
||||
if (Dcal) {
|
||||
Dcal->resize(2, 1);
|
||||
(*Dcal) << -inv_f * point.x(), -inv_f * point.y();
|
||||
}
|
||||
|
||||
if (Dp) {
|
||||
Dp->resize(2, 2);
|
||||
(*Dp) << inv_f, 0.0, //
|
||||
0.0, inv_f;
|
||||
}
|
||||
|
||||
return point;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,141 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Cal3f.h
|
||||
* @brief Calibration model with a single focal length and zero skew.
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Cal3.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* @brief Calibration model with a single focal length and zero skew.
|
||||
* @ingroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3f : public Cal3 {
|
||||
public:
|
||||
enum { dimension = 1 };
|
||||
using shared_ptr = std::shared_ptr<Cal3f>;
|
||||
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/// Default constructor
|
||||
Cal3f() = default;
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param f focal length
|
||||
* @param u0 image center x-coordinate (considered a constant)
|
||||
* @param v0 image center y-coordinate (considered a constant)
|
||||
*/
|
||||
Cal3f(double f, double u0, double v0);
|
||||
|
||||
~Cal3f() override = default;
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
/// Output stream operator
|
||||
friend std::ostream& operator<<(std::ostream& os, const Cal3f& cal);
|
||||
|
||||
/// print with optional string
|
||||
void print(const std::string& s = "") const override;
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const Cal3f& K, double tol = 1e-9) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
/// focal length
|
||||
inline double f() const { return fx_; }
|
||||
|
||||
/// vectorized form (column-wise)
|
||||
Vector1 vector() const;
|
||||
|
||||
/**
|
||||
* @brief: convert intrinsic coordinates xy to image coordinates uv
|
||||
* Version of uncalibrate with derivatives
|
||||
* @param p point in intrinsic coordinates
|
||||
* @param Dcal optional 2*1 Jacobian wrpt focal length
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in image coordinates
|
||||
*/
|
||||
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 1> Dcal = {},
|
||||
OptionalJacobian<2, 2> Dp = {}) const;
|
||||
|
||||
/**
|
||||
* Convert a pixel coordinate to ideal coordinate xy
|
||||
* @param pi point in image coordinates
|
||||
* @param Dcal optional 2*1 Jacobian wrpt focal length
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in intrinsic coordinates
|
||||
*/
|
||||
Point2 calibrate(const Point2& pi, OptionalJacobian<2, 1> Dcal = {},
|
||||
OptionalJacobian<2, 2> Dp = {}) const;
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
/// Return DOF, dimensionality of tangent space
|
||||
size_t dim() const override { return Dim(); }
|
||||
|
||||
/// Return DOF, dimensionality of tangent space
|
||||
inline static size_t Dim() { return dimension; }
|
||||
|
||||
/// Update calibration with tangent space delta
|
||||
Cal3f retract(const Vector& d) const { return Cal3f(fx_ + d(0), u0_, v0_); }
|
||||
|
||||
/// Calculate local coordinates to another calibration
|
||||
Vector1 localCoordinates(const Cal3f& T2) const {
|
||||
return Vector1(T2.fx_ - fx_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
|
||||
/** 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(u0_);
|
||||
ar& BOOST_SERIALIZATION_NVP(v0_);
|
||||
}
|
||||
#endif
|
||||
|
||||
/// @}
|
||||
};
|
||||
|
||||
template <>
|
||||
struct traits<Cal3f> : public internal::Manifold<Cal3f> {};
|
||||
|
||||
template <>
|
||||
struct traits<const Cal3f> : public internal::Manifold<Cal3f> {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -644,8 +644,36 @@ class EssentialMatrix {
|
|||
double error(gtsam::Vector vA, gtsam::Vector vB);
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/Cal3.h>
|
||||
virtual class Cal3 {
|
||||
// Standard Constructors
|
||||
Cal3();
|
||||
Cal3(double fx, double fy, double s, double u0, double v0);
|
||||
Cal3(gtsam::Vector v);
|
||||
|
||||
// Testable
|
||||
void print(string s = "Cal3") const;
|
||||
bool equals(const gtsam::Cal3& rhs, double tol) const;
|
||||
|
||||
// Standard Interface
|
||||
double fx() const;
|
||||
double fy() const;
|
||||
double aspectRatio() const;
|
||||
double skew() const;
|
||||
double px() const;
|
||||
double py() const;
|
||||
gtsam::Point2 principalPoint() const;
|
||||
gtsam::Vector vector() const;
|
||||
gtsam::Matrix K() const;
|
||||
gtsam::Matrix inverse() const;
|
||||
|
||||
// Manifold
|
||||
static size_t Dim();
|
||||
size_t dim() const;
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
class Cal3_S2 {
|
||||
virtual class Cal3_S2 : gtsam::Cal3 {
|
||||
// Standard Constructors
|
||||
Cal3_S2();
|
||||
Cal3_S2(double fx, double fy, double s, double u0, double v0);
|
||||
|
|
@ -672,23 +700,12 @@ class Cal3_S2 {
|
|||
Eigen::Ref<Eigen::MatrixXd> Dcal,
|
||||
Eigen::Ref<Eigen::MatrixXd> Dp) const;
|
||||
|
||||
// Standard Interface
|
||||
double fx() const;
|
||||
double fy() const;
|
||||
double skew() const;
|
||||
double px() const;
|
||||
double py() const;
|
||||
gtsam::Point2 principalPoint() const;
|
||||
gtsam::Vector vector() const;
|
||||
gtsam::Matrix K() const;
|
||||
gtsam::Matrix inverse() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/Cal3DS2_Base.h>
|
||||
virtual class Cal3DS2_Base {
|
||||
virtual class Cal3DS2_Base : gtsam::Cal3 {
|
||||
// Standard Constructors
|
||||
Cal3DS2_Base();
|
||||
|
||||
|
|
@ -696,16 +713,8 @@ virtual class Cal3DS2_Base {
|
|||
void print(string s = "") const;
|
||||
|
||||
// Standard Interface
|
||||
double fx() const;
|
||||
double fy() const;
|
||||
double skew() const;
|
||||
double px() const;
|
||||
double py() const;
|
||||
double k1() const;
|
||||
double k2() const;
|
||||
gtsam::Matrix K() const;
|
||||
gtsam::Vector k() const;
|
||||
gtsam::Vector vector() const;
|
||||
|
||||
// Action on Point2
|
||||
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
|
||||
|
|
@ -785,7 +794,7 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base {
|
|||
};
|
||||
|
||||
#include <gtsam/geometry/Cal3Fisheye.h>
|
||||
class Cal3Fisheye {
|
||||
virtual class Cal3Fisheye : gtsam::Cal3 {
|
||||
// Standard Constructors
|
||||
Cal3Fisheye();
|
||||
Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1,
|
||||
|
|
@ -797,8 +806,6 @@ class Cal3Fisheye {
|
|||
bool equals(const gtsam::Cal3Fisheye& rhs, double tol) const;
|
||||
|
||||
// Manifold
|
||||
static size_t Dim();
|
||||
size_t dim() const;
|
||||
gtsam::Cal3Fisheye retract(gtsam::Vector v) const;
|
||||
gtsam::Vector localCoordinates(const gtsam::Cal3Fisheye& c) const;
|
||||
|
||||
|
|
@ -813,35 +820,23 @@ class Cal3Fisheye {
|
|||
Eigen::Ref<Eigen::MatrixXd> Dp) const;
|
||||
|
||||
// Standard Interface
|
||||
double fx() const;
|
||||
double fy() const;
|
||||
double skew() const;
|
||||
double k1() const;
|
||||
double k2() const;
|
||||
double k3() const;
|
||||
double k4() const;
|
||||
double px() const;
|
||||
double py() const;
|
||||
gtsam::Point2 principalPoint() const;
|
||||
gtsam::Vector vector() const;
|
||||
gtsam::Vector k() const;
|
||||
gtsam::Matrix K() const;
|
||||
gtsam::Matrix inverse() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/Cal3_S2Stereo.h>
|
||||
class Cal3_S2Stereo {
|
||||
virtual class Cal3_S2Stereo : gtsam::Cal3{
|
||||
// Standard Constructors
|
||||
Cal3_S2Stereo();
|
||||
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b);
|
||||
Cal3_S2Stereo(gtsam::Vector v);
|
||||
|
||||
// Manifold
|
||||
static size_t Dim();
|
||||
size_t dim() const;
|
||||
gtsam::Cal3_S2Stereo retract(gtsam::Vector v) const;
|
||||
gtsam::Vector localCoordinates(const gtsam::Cal3_S2Stereo& c) const;
|
||||
|
||||
|
|
@ -850,35 +845,23 @@ class Cal3_S2Stereo {
|
|||
bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const;
|
||||
|
||||
// Standard Interface
|
||||
double fx() const;
|
||||
double fy() const;
|
||||
double skew() const;
|
||||
double px() const;
|
||||
double py() const;
|
||||
gtsam::Matrix K() const;
|
||||
gtsam::Point2 principalPoint() const;
|
||||
double baseline() const;
|
||||
gtsam::Vector6 vector() const;
|
||||
gtsam::Matrix inverse() const;
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
class Cal3Bundler {
|
||||
virtual class Cal3f : gtsam::Cal3 {
|
||||
// Standard Constructors
|
||||
Cal3Bundler();
|
||||
Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
|
||||
Cal3Bundler(double fx, double k1, double k2, double u0, double v0,
|
||||
double tol);
|
||||
Cal3f();
|
||||
Cal3f(double fx, double u0, double v0);
|
||||
|
||||
// Testable
|
||||
void print(string s = "") const;
|
||||
bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
|
||||
bool equals(const gtsam::Cal3f& rhs, double tol) const;
|
||||
|
||||
// Manifold
|
||||
static size_t Dim();
|
||||
size_t dim() const;
|
||||
gtsam::Cal3Bundler retract(gtsam::Vector v) const;
|
||||
gtsam::Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
|
||||
gtsam::Cal3f retract(gtsam::Vector v) const;
|
||||
gtsam::Vector localCoordinates(const gtsam::Cal3f& c) const;
|
||||
|
||||
// Action on Point2
|
||||
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
|
||||
|
|
@ -891,15 +874,32 @@ class Cal3Bundler {
|
|||
Eigen::Ref<Eigen::MatrixXd> Dp) const;
|
||||
|
||||
// Standard Interface
|
||||
double fx() const;
|
||||
double fy() const;
|
||||
double f() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
virtual class Cal3Bundler : gtsam::Cal3f {
|
||||
// Standard Constructors
|
||||
Cal3Bundler();
|
||||
Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
|
||||
Cal3Bundler(double fx, double k1, double k2, double u0, double v0,
|
||||
double tol);
|
||||
|
||||
// Testable
|
||||
void print(string s = "") const;
|
||||
bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
|
||||
|
||||
// Manifold
|
||||
gtsam::Cal3Bundler retract(gtsam::Vector v) const;
|
||||
gtsam::Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
|
||||
|
||||
// Standard Interface
|
||||
double k1() const;
|
||||
double k2() const;
|
||||
double px() const;
|
||||
double py() const;
|
||||
gtsam::Vector vector() const;
|
||||
gtsam::Vector k() const;
|
||||
gtsam::Matrix K() const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
|
|
|
|||
|
|
@ -29,7 +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;
|
||||
|
|
@ -37,16 +37,19 @@ 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);
|
||||
Point2 expected(1000 + g * p.x(), 2000 + g * p.y());
|
||||
double distortion = 1.0 + v[1] * r + v[2] * r * r;
|
||||
double u = K.px() + v[0] * distortion * p.x();
|
||||
double v_coord = K.py() + v[0] * distortion * p.y();
|
||||
Point2 expected(u, v_coord);
|
||||
Point2 actual = K.uncalibrate(p);
|
||||
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);
|
||||
|
|
@ -63,11 +66,11 @@ Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Bundler, DuncalibrateDefault) {
|
||||
TEST(Cal3Bundler, DUncalibrateDefault) {
|
||||
Cal3Bundler trueK(1, 0, 0);
|
||||
Matrix Dcal, Dp;
|
||||
Point2 actual = trueK.uncalibrate(p, Dcal, Dp);
|
||||
Point2 expected = p;
|
||||
Point2 expected(p); // Since K is identity, uncalibrate should return p
|
||||
CHECK(assert_equal(expected, actual, 1e-7));
|
||||
Matrix numerical1 = numericalDerivative21(uncalibrate_, trueK, p);
|
||||
Matrix numerical2 = numericalDerivative22(uncalibrate_, trueK, p);
|
||||
|
|
@ -76,7 +79,7 @@ TEST(Cal3Bundler, DuncalibrateDefault) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Bundler, DcalibrateDefault) {
|
||||
TEST(Cal3Bundler, DCalibrateDefault) {
|
||||
Cal3Bundler trueK(1, 0, 0);
|
||||
Matrix Dcal, Dp;
|
||||
Point2 pn(0.5, 0.5);
|
||||
|
|
@ -90,11 +93,11 @@ TEST(Cal3Bundler, DcalibrateDefault) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Bundler, DuncalibratePrincipalPoint) {
|
||||
TEST(Cal3Bundler, DUncalibratePrincipalPoint) {
|
||||
Cal3Bundler K(5, 0, 0, 2, 2);
|
||||
Matrix Dcal, Dp;
|
||||
Point2 actual = K.uncalibrate(p, Dcal, Dp);
|
||||
Point2 expected(12, 17);
|
||||
Point2 expected(2.0 + 5.0 * p.x(), 2.0 + 5.0 * p.y());
|
||||
CHECK(assert_equal(expected, actual, 1e-7));
|
||||
Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
|
||||
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
|
||||
|
|
@ -103,7 +106,7 @@ TEST(Cal3Bundler, DuncalibratePrincipalPoint) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Bundler, DcalibratePrincipalPoint) {
|
||||
TEST(Cal3Bundler, DCalibratePrincipalPoint) {
|
||||
Cal3Bundler K(2, 0, 0, 2, 2);
|
||||
Matrix Dcal, Dp;
|
||||
Point2 pn(0.5, 0.5);
|
||||
|
|
@ -117,11 +120,18 @@ TEST(Cal3Bundler, DcalibratePrincipalPoint) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Bundler, Duncalibrate) {
|
||||
TEST(Cal3Bundler, DUncalibrate) {
|
||||
Matrix Dcal, Dp;
|
||||
Point2 actual = K.uncalibrate(p, Dcal, Dp);
|
||||
Point2 expected(2182, 3773);
|
||||
// Compute expected value manually
|
||||
Vector v = K.vector();
|
||||
double r2 = p.x() * p.x() + p.y() * p.y();
|
||||
double distortion = 1.0 + v[1] * r2 + v[2] * r2 * r2;
|
||||
Point2 expected(
|
||||
K.px() + v[0] * distortion * p.x(),
|
||||
K.py() + v[0] * distortion * p.y());
|
||||
CHECK(assert_equal(expected, actual, 1e-7));
|
||||
|
||||
Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
|
||||
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
|
||||
CHECK(assert_equal(numerical1, Dcal, 1e-7));
|
||||
|
|
@ -129,7 +139,7 @@ TEST(Cal3Bundler, Duncalibrate) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Bundler, Dcalibrate) {
|
||||
TEST(Cal3Bundler, DCalibrate) {
|
||||
Matrix Dcal, Dp;
|
||||
Point2 pn(0.5, 0.5);
|
||||
Point2 pi = K.uncalibrate(pn);
|
||||
|
|
@ -145,7 +155,7 @@ TEST(Cal3Bundler, Dcalibrate) {
|
|||
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());
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,132 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
/**
|
||||
* @file testCal3F.cpp
|
||||
* @brief Unit tests for the Cal3f calibration model.
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/geometry/Cal3f.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
GTSAM_CONCEPT_TESTABLE_INST(Cal3f)
|
||||
GTSAM_CONCEPT_MANIFOLD_INST(Cal3f)
|
||||
|
||||
static Cal3f K(500.0, 1000.0, 2000.0); // f = 500, u0 = 1000, v0 = 2000
|
||||
static Point2 p(2.0, 3.0);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3f, Vector) {
|
||||
Cal3f K(1.0, 0.0, 0.0);
|
||||
Vector expected(1);
|
||||
expected << 1.0;
|
||||
CHECK(assert_equal(expected, K.vector()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3f, Uncalibrate) {
|
||||
// Expected output: apply the intrinsic calibration matrix to point p
|
||||
Matrix3 K_matrix = K.K();
|
||||
Vector3 p_homogeneous(p.x(), p.y(), 1.0);
|
||||
Vector3 expected_homogeneous = K_matrix * p_homogeneous;
|
||||
Point2 expected(expected_homogeneous.x() / expected_homogeneous.z(),
|
||||
expected_homogeneous.y() / expected_homogeneous.z());
|
||||
|
||||
Point2 actual = K.uncalibrate(p);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3f, Calibrate) {
|
||||
Point2 pi = K.uncalibrate(p);
|
||||
Point2 pn = K.calibrate(pi);
|
||||
CHECK(traits<Point2>::Equals(p, pn, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 uncalibrate_(const Cal3f& k, const Point2& pt) {
|
||||
return k.uncalibrate(pt);
|
||||
}
|
||||
|
||||
Point2 calibrate_(const Cal3f& k, const Point2& pt) { return k.calibrate(pt); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3f, DUncalibrate) {
|
||||
Cal3f K(500.0, 1000.0, 2000.0);
|
||||
Matrix Dcal, Dp;
|
||||
Point2 actual = K.uncalibrate(p, Dcal, Dp);
|
||||
|
||||
// Expected value computed manually
|
||||
Point2 expected = Point2(K.px() + K.fx() * p.x(), K.py() + K.fx() * p.y());
|
||||
CHECK(assert_equal(expected, actual, 1e-9));
|
||||
|
||||
// Compute numerical derivatives
|
||||
Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
|
||||
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
|
||||
CHECK(assert_equal(numerical1, Dcal, 1e-6));
|
||||
CHECK(assert_equal(numerical2, Dp, 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3f, DCalibrate) {
|
||||
Point2 pi = K.uncalibrate(p);
|
||||
Matrix Dcal, Dp;
|
||||
Point2 actual = K.calibrate(pi, Dcal, Dp);
|
||||
CHECK(assert_equal(p, actual, 1e-9));
|
||||
|
||||
// Compute numerical derivatives
|
||||
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
|
||||
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
|
||||
CHECK(assert_equal(numerical1, Dcal, 1e-6));
|
||||
CHECK(assert_equal(numerical2, Dp, 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3f, Manifold) {
|
||||
Cal3f K1(500.0, 1000.0, 2000.0);
|
||||
Vector1 delta;
|
||||
delta << 10.0; // Increment focal length by 10
|
||||
|
||||
Cal3f K2 = K1.retract(delta);
|
||||
CHECK(assert_equal(510.0, K2.fx(), 1e-9));
|
||||
CHECK(assert_equal(K1.px(), K2.px(), 1e-9));
|
||||
CHECK(assert_equal(K1.py(), K2.py(), 1e-9));
|
||||
|
||||
Vector1 delta_computed = K1.localCoordinates(K2);
|
||||
CHECK(assert_equal(delta, delta_computed, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3f, Assert_equal) {
|
||||
CHECK(assert_equal(K, K, 1e-9));
|
||||
Cal3f K2(500.0, 1000.0, 2000.0);
|
||||
CHECK(assert_equal(K, K2, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3f, Print) {
|
||||
Cal3f cal(500.0, 1000.0, 2000.0);
|
||||
std::stringstream os;
|
||||
os << "f: " << cal.fx() << ", px: " << cal.px() << ", py: " << cal.py();
|
||||
|
||||
EXPECT(assert_stdout_equal(os.str(), cal));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue