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

Merge branch 'develop' of https://github.com/borglab/gtsam into feature/wrap_camVector

release/4.3a0
Sushmita 2020-12-05 12:37:39 -05:00
parent 44d1d69274 0527a83674
commit 6a4a40de44
28 changed files with 1438 additions and 1021 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
gtsam/geometry/Cal3.cpp Normal file
View File

@ -0,0 +1,75 @@
/* ----------------------------------------------------------------------------
* 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 Cal3.cpp
* @brief Common code for all calibration models.
* @author Frank Dellaert
*/
#include <gtsam/geometry/Cal3.h>
#include <cmath>
#include <fstream>
#include <iostream>
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
fx_ = double(w) / (2.0 * tan(a));
fy_ = fx_;
}
/* ************************************************************************* */
Cal3::Cal3(const std::string& path) {
const auto buffer = path + std::string("/calibration_info.txt");
std::ifstream infile(buffer, std::ios::in);
if (infile && !infile.eof()) {
infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
} else {
throw std::runtime_error("Cal3: Unable to load the calibration");
}
infile.close();
}
/* ************************************************************************* */
std::ostream& operator<<(std::ostream& os, const Cal3& cal) {
os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py();
return os;
}
/* ************************************************************************* */
void Cal3::print(const std::string& s) const { gtsam::print((Matrix)K(), s); }
/* ************************************************************************* */
bool Cal3::equals(const Cal3& K, double tol) const {
return (std::fabs(fx_ - K.fx_) < tol && std::fabs(fy_ - K.fy_) < tol &&
std::fabs(s_ - K.s_) < tol && std::fabs(u0_ - K.u0_) < tol &&
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

141
gtsam/geometry/Cal3.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
* Jacobians of `calibrate` using `uncalibrate`.
* This is useful when there are iterative operations in the `calibrate`
* function which make computing jacobians difficult.
*
*
* Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can
* easily compute the Jacobians:
* df/pi = -I (pn and pi are independent args)
@ -61,6 +61,143 @@ void calibrateJacobians(const Cal& calibration, const Point2& pn,
}
}
//TODO(Varun) Make common base class for all calibration models.
/**
* @brief Common base class for all calibration models.
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3 {
protected:
double fx_ = 1.0f, fy_ = 1.0f; ///< focal length
double s_ = 0.0f; ///< skew
double u0_ = 0.0f, v0_ = 0.0f; ///< principal point
public:
enum { dimension = 5 };
///< shared pointer to calibration object
using shared_ptr = boost::shared_ptr<Cal3>;
/// @name Standard Constructors
/// @{
/// Create a default calibration that leaves coordinates unchanged
Cal3() = default;
/// constructor from doubles
Cal3(double fx, double fy, double s, double u0, double v0)
: fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {}
/// constructor from vector
Cal3(const Vector5& d)
: fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {}
/**
* Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
* @param fov field of view in degrees
* @param w image width
* @param h image height
*/
Cal3(double fov, int w, int h);
/// @}
/// @name Advanced Constructors
/// @{
/**
* Load calibration parameters from `calibration_info.txt` file located in
* `path` directory.
*
* The contents of calibration file should be the 5 parameters in order:
* `fx, fy, s, u0, v0`
*
* @param path path to directory containing `calibration_info.txt`.
*/
Cal3(const std::string& path);
/// @}
/// @name Testable
/// @{
/// Output stream operator
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Cal3& cal);
/// print with optional string
virtual void print(const std::string& s = "") const;
/// Check if equal up to specified tolerance
bool equals(const Cal3& K, double tol = 10e-9) const;
/// @}
/// @name Standard Interface
/// @{
/// focal length x
inline double fx() const { return fx_; }
/// focal length y
inline double fy() const { return fy_; }
/// aspect ratio
inline double aspectRatio() const { return fx_ / 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_; }
/// return the principal point
Point2 principalPoint() const { return Point2(u0_, v0_); }
/// vectorized form (column-wise)
Vector5 vector() const {
Vector5 v;
v << fx_, fy_, s_, u0_, v0_;
return v;
}
/// return calibration matrix K
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_V41
/** @deprecated The following function has been deprecated, use K above */
Matrix3 matrix() const { return K(); }
#endif
/// Return inverted calibration matrix inv(K)
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
/// @{
private:
/// 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_);
}
/// @}
};
} // \ namespace gtsam

78
gtsam/geometry/Cal3Bundler.cpp
View File

@ -15,28 +15,19 @@
* @author ydjian
*/
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3Bundler.h>
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 {
// 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;
}
@ -48,35 +39,42 @@ Vector4 Cal3Bundler::k() const {
}
/* ************************************************************************* */
Vector3 Cal3Bundler::vector() const {
return Vector3(f_, k1_, k2_);
Vector3 Cal3Bundler::vector() const { return Vector3(fx_, k1_, k2_); }
/* ************************************************************************* */
std::ostream& operator<<(std::ostream& os, const Cal3Bundler& cal) {
os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2()
<< ", px: " << cal.px() << ", py: " << cal.py();
return os;
}
/* ************************************************************************* */
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 {
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;
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);
}
/* ************************************************************************* */
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);
Point2 Cal3Bundler::uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal,
OptionalJacobian<2, 2> Dp) const {
// 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;
@ -94,23 +92,22 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
}
/* ************************************************************************* */
Point2 Cal3Bundler::calibrate(const Point2& pi,
OptionalJacobian<2, 3> Dcal,
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
// initialize by ignoring the distortion at all, might be problematic for
// pixels around boundary
Point2 pn(x, y);
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (distance2(uncalibrate(pn), pi) <= tol_)
break;
if (distance2(uncalibrate(pn), pi) <= tol_) break;
const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
const double rr = xx + yy;
const double g = (1 + k1_ * rr + k2_ * rr * rr);
@ -119,7 +116,8 @@ Point2 Cal3Bundler::calibrate(const Point2& pi,
if (iteration >= maxIterations)
throw std::runtime_error(
"Cal3Bundler::calibrate fails to converge. need a better initialization");
"Cal3Bundler::calibrate fails to converge. need a better "
"initialization");
calibrateJacobians<Cal3Bundler, dimension>(*this, pn, Dcal, Dp);
@ -150,14 +148,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

137
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
@ -28,23 +29,23 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3Bundler {
class GTSAM_EXPORT Cal3Bundler : public Cal3 {
private:
double k1_ = 0.0f, k2_ = 0.0f; ///< radial distortion
double tol_ = 1e-5; ///< tolerance value when calibrating
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: 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:
enum { dimension = 3 };
/// @name Standard Constructors
/// @{
/// Default constructor
Cal3Bundler();
Cal3Bundler() = default;
/**
* Constructor
@ -56,7 +57,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), k1_(k1), k2_(k2), tol_(tol) {}
virtual ~Cal3Bundler() {}
@ -64,8 +66,12 @@ public:
/// @name Testable
/// @{
/// Output stream operator
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Cal3Bundler& cal);
/// 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,64 +80,41 @@ public:
/// @name Standard Interface
/// @{
Matrix3 K() const; ///< Standard 3*3 calibration matrix
Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
/// distorsion parameter k1
inline double k1() const { return k1_; }
/// distorsion 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)
Vector3 vector() const;
/// 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_;
}
/// distorsion 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_;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/// get parameter u0
inline double u0() const {
return u0_;
}
inline double u0() const { return u0_; }
/// get parameter v0
inline double v0() const {
return v0_;
}
inline double v0() const { return v0_; }
#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
*/
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
OptionalJacobian<2, 2> Dp = boost::none) const;
/**
* Convert a pixel coordinate to ideal coordinate xy
@ -141,8 +124,7 @@ public:
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& pi,
OptionalJacobian<2, 3> Dcal = boost::none,
Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// @deprecated might be removed in next release, use uncalibrate
@ -158,48 +140,45 @@ public:
/// @name Manifold
/// @{
/// 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; }
/// Update calibration with tangent space delta
Cal3Bundler retract(const Vector& d) const;
inline Cal3Bundler retract(const Vector& d) const {
return Cal3Bundler(fx_ + 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
/// @{
/** Serialization function */
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_);
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Cal3Bundler", boost::serialization::base_object<Cal3>(*this));
ar& BOOST_SERIALIZATION_NVP(k1_);
ar& BOOST_SERIALIZATION_NVP(k2_);
ar& BOOST_SERIALIZATION_NVP(tol_);
}
/// @}
};
template<>
template <>
struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
template<>
template <>
struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
} // namespace gtsam
} // namespace gtsam

23
gtsam/geometry/Cal3DS2.cpp
View File

@ -13,28 +13,30 @@
* @file Cal3DS2.cpp
* @date Feb 28, 2010
* @author ydjian
* @author Varun Agrawal
*/
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3DS2.h>
namespace gtsam {
/* ************************************************************************* */
void Cal3DS2::print(const std::string& s_) const {
Base::print(s_);
std::ostream& operator<<(std::ostream& os, const Cal3DS2& cal) {
os << (Cal3DS2_Base&)cal;
return os;
}
/* ************************************************************************* */
void Cal3DS2::print(const std::string& s_) const { Base::print(s_); }
/* ************************************************************************* */
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);
}
/* ************************************************************************* */
@ -46,8 +48,5 @@ Cal3DS2 Cal3DS2::retract(const Vector& d) const {
Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const {
return T2.vector() - vector();
}
}
/* ************************************************************************* */

52
gtsam/geometry/Cal3DS2.h
View File

@ -11,9 +11,11 @@
/**
* @file Cal3DS2.h
* @brief Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base
* @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
@ -30,22 +32,20 @@ namespace gtsam {
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
using Base = Cal3DS2_Base;
typedef Cal3DS2_Base Base;
public:
public:
enum { dimension = 9 };
/// @name Standard Constructors
/// @{
/// Default Constructor with only unit focal length
Cal3DS2() : Base() {}
Cal3DS2() = default;
Cal3DS2(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) :
Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {}
Cal3DS2(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)
: Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {}
virtual ~Cal3DS2() {}
@ -53,12 +53,16 @@ public:
/// @name Advanced Constructors
/// @{
Cal3DS2(const Vector &v) : Base(v) {}
Cal3DS2(const Vector9& v) : Base(v) {}
/// @}
/// @name Testable
/// @{
/// Output stream operator
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Cal3DS2& cal);
/// print with optional string
void print(const std::string& s = "") const override;
@ -70,16 +74,16 @@ public:
/// @{
/// Given delta vector, update calibration
Cal3DS2 retract(const Vector& d) const ;
Cal3DS2 retract(const Vector& d) const;
/// Given a different calibration, calculate update to obtain it
Vector localCoordinates(const Cal3DS2& T2) const ;
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
@ -92,30 +96,24 @@ public:
/// @}
private:
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::make_nvp("Cal3DS2",
boost::serialization::base_object<Cal3DS2_Base>(*this));
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Cal3DS2", boost::serialization::base_object<Cal3DS2_Base>(*this));
}
/// @}
};
template<>
template <>
struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
template<>
template <>
struct traits<const Cal3DS2> : public internal::Manifold<Cal3DS2> {};
}

71
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -16,33 +16,14 @@
* @author Varun Agrawal
*/
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Cal3DS2_Base.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3DS2_Base.h>
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;
@ -50,6 +31,14 @@ Vector9 Cal3DS2_Base::vector() const {
return v;
}
/* ************************************************************************* */
std::ostream& operator<<(std::ostream& os, const Cal3DS2_Base& cal) {
os << (Cal3&)cal;
os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", p1: " << cal.p1()
<< ", p2: " << cal.p2();
return os;
}
/* ************************************************************************* */
void Cal3DS2_Base::print(const std::string& s_) const {
gtsam::print((Matrix)K(), s_ + ".K");
@ -58,31 +47,30 @@ 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;
}
/* ************************************************************************* */
static Matrix29 D2dcalibration(double x, double y, double xx,
double yy, double xy, double rr, double r4, double pnx, double pny,
const Matrix2& DK) {
static Matrix29 D2dcalibration(double x, double y, double xx, double yy,
double xy, double rr, double r4, double pnx,
double pny, const Matrix2& DK) {
Matrix25 DR1;
DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
Matrix24 DR2;
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
y * rr, y * r4, rr + 2 * yy, 2 * xy;
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
y * rr, y * r4, rr + 2 * yy, 2 * xy;
Matrix29 D;
D << DR1, DK * DR2;
return D;
}
/* ************************************************************************* */
static Matrix2 D2dintrinsic(double x, double y, double rr,
double g, double k1, double k2, double p1, double p2,
const Matrix2& DK) {
static Matrix2 D2dintrinsic(double x, double y, double rr, double g, double k1,
double k2, double p1, double p2,
const Matrix2& DK) {
const double drdx = 2. * x;
const double drdy = 2. * y;
const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
@ -96,8 +84,8 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
Matrix2 DR;
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy;
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy;
return DK * DR;
}
@ -105,14 +93,14 @@ 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;
const double r4 = rr * rr;
const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
// tangential component
const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
@ -202,8 +190,5 @@ Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const {
DK << fx_, s_, 0.0, fy_;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
}
}
/* ************************************************************************* */

134
gtsam/geometry/Cal3DS2_Base.h
View File

@ -33,51 +33,34 @@ 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 {
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:
class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
protected:
double k1_ = 0.0f, k2_ = 0.0f; ///< radial 2nd-order and 4th-order
double p1_ = 0.0f, p2_ = 0.0f; ///< 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) {}
/// Default Constructor with only unit focal length
Cal3DS2_Base() = default;
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),
k1_(k1),
k2_(k2),
p1_(p1),
p2_(p2),
tol_(tol) {}
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)
: Cal3(fx, fy, s, u0, v0),
k1_(k1),
k2_(k2),
p1_(p1),
p2_(p2),
tol_(tol) {}
virtual ~Cal3DS2_Base() {}
@ -85,14 +68,23 @@ public:
/// @name Advanced Constructors
/// @{
Cal3DS2_Base(const Vector &v) ;
Cal3DS2_Base(const Vector9& 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
/// @{
/// Output stream operator
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Cal3DS2_Base& cal);
/// 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,35 +93,17 @@ 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_;}
inline double k1() const { return k1_; }
/// Second distortion coefficient
inline double k2() const { return k2_;}
inline double k2() const { return k2_; }
/// First tangential distortion coefficient
inline double p1() const { return p1_;}
inline double p1() const { return p1_; }
/// Second tangential distortion coefficient
inline double p2() const { return p2_;}
/// return calibration matrix -- not really applicable
Matrix3 K() const;
inline double p2() const { return p2_; }
/// return distortion parameter vector
Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
@ -152,10 +126,16 @@ public:
OptionalJacobian<2, 2> Dp = boost::none) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates
Matrix2 D2d_intrinsic(const Point2& p) const ;
Matrix2 D2d_intrinsic(const Point2& p) const;
/// Derivative of uncalibrate wrpt the calibration parameters
Matrix29 D2d_calibration(const Point2& p) const ;
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
@ -168,31 +148,23 @@ public:
/// @}
private:
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_);
ar & BOOST_SERIALIZATION_NVP(k1_);
ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(p1_);
ar & BOOST_SERIALIZATION_NVP(p2_);
ar & BOOST_SERIALIZATION_NVP(tol_);
template <class Archive>
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_);
ar& BOOST_SERIALIZATION_NVP(p2_);
ar& BOOST_SERIALIZATION_NVP(tol_);
}
/// @}
};
}

50
gtsam/geometry/Cal3Fisheye.cpp
View File

@ -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;
@ -157,6 +139,14 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
return pi;
}
/* ************************************************************************* */
std::ostream& operator<<(std::ostream& os, const Cal3Fisheye& cal) {
os << (Cal3&)cal;
os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", k3: " << cal.k3()
<< ", k4: " << cal.k4();
return os;
}
/* ************************************************************************* */
void Cal3Fisheye::print(const std::string& s_) const {
gtsam::print((Matrix)K(), s_ + ".K");
@ -165,24 +155,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

96
gtsam/geometry/Cal3Fisheye.h
View File

@ -38,40 +38,35 @@ 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]
* [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_ = 0.0f, k2_ = 0.0f; ///< fisheye distortion coefficients
double k3_ = 0.0f, k4_ = 0.0f; ///< fisheye distortion coefficients
double tol_ = 1e-5; ///< tolerance value when calibrating
public:
enum { dimension = 9 };
typedef boost::shared_ptr<Cal3Fisheye>
shared_ptr; ///< shared pointer to fisheye calibration object
///< shared pointer to fisheye calibration object
using shared_ptr = boost::shared_ptr<Cal3Fisheye>;
/// @name Standard Constructors
/// @{
/// 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() = default;
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 +79,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 +102,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 +115,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;
@ -150,8 +137,12 @@ class GTSAM_EXPORT Cal3Fisheye {
/// @name Testable
/// @{
/// Output stream operator
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Cal3Fisheye& cal);
/// 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 +151,21 @@ class GTSAM_EXPORT Cal3Fisheye {
/// @name Manifold
/// @{
/// Return dimensions of calibration manifold object
virtual size_t dim() const override { return Dim(); }
/// Return dimensions of calibration manifold object
inline 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 +186,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_);

50
gtsam/geometry/Cal3Unified.cpp
View File

@ -16,19 +16,15 @@
* @author Varun Agrawal
*/
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Point2.h>
#include <cmath>
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]) {}
/* ************************************************************************* */
Vector10 Cal3Unified::vector() const {
Vector10 v;
@ -36,6 +32,13 @@ Vector10 Cal3Unified::vector() const {
return v;
}
/* ************************************************************************* */
std::ostream& operator<<(std::ostream& os, const Cal3Unified& cal) {
os << (Cal3DS2_Base&)cal;
os << ", xi: " << cal.xi();
return os;
}
/* ************************************************************************* */
void Cal3Unified::print(const std::string& s) const {
Base::print(s);
@ -44,20 +47,14 @@ 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;
}
/* ************************************************************************* */
// todo: make a fixed sized jacobian version of this
Point2 Cal3Unified::uncalibrate(const Point2& p,
OptionalJacobian<2,10> Dcal,
OptionalJacobian<2,2> Dp) const {
Point2 Cal3Unified::uncalibrate(const Point2& p, OptionalJacobian<2, 10> Dcal,
OptionalJacobian<2, 2> Dp) const {
// this part of code is modified from Cal3DS2,
// since the second part of this model (after project to normalized plane)
// is same as Cal3DS2
@ -70,19 +67,19 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
const double sqrt_nx = sqrt(xs * xs + ys * ys + 1.0);
const double xi_sqrt_nx = 1.0 / (1 + xi * sqrt_nx);
const double xi_sqrt_nx2 = xi_sqrt_nx * xi_sqrt_nx;
const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx; // points on NPlane
const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx; // points on NPlane
// Part2: project NPlane point to pixel plane: use Cal3DS2
Point2 m(x,y);
Point2 m(x, y);
Matrix29 H1base;
Matrix2 H2base; // jacobians from Base class
Matrix2 H2base; // jacobians from Base class
Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
// Inlined derivative for calibration
if (Dcal) {
// part1
Vector2 DU;
DU << -xs * sqrt_nx * xi_sqrt_nx2, //
DU << -xs * sqrt_nx * xi_sqrt_nx2, //
-ys * sqrt_nx * xi_sqrt_nx2;
*Dcal << H1base, H2base * DU;
}
@ -91,10 +88,10 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
if (Dp) {
// part1
const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
const double mid = -(xi * xs*ys) * denom;
const double mid = -(xi * xs * ys) * denom;
Matrix2 DU;
DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
DU << (sqrt_nx + xi * (ys * ys + 1)) * denom, mid, //
mid, (sqrt_nx + xi * (xs * xs + 1)) * denom;
*Dp << H2base * DU;
}
@ -117,7 +114,6 @@ Point2 Cal3Unified::calibrate(const Point2& pi, OptionalJacobian<2, 10> Dcal,
}
/* ************************************************************************* */
Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {
const double x = p.x(), y = p.y();
const double xy2 = x * x + y * y;
const double sq_xy = (xi_ + sqrt(1 + (1 - xi_ * xi_) * xy2)) / (xy2 + 1);
@ -127,7 +123,6 @@ Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {
/* ************************************************************************* */
Point2 Cal3Unified::spaceToNPlane(const Point2& p) const {
const double x = p.x(), y = p.y();
const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1);
@ -140,11 +135,10 @@ Cal3Unified Cal3Unified::retract(const Vector& d) const {
}
/* ************************************************************************* */
Vector10 Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
return T2.vector() - vector();
}
}
/* ************************************************************************* */
} // \ namespace gtsam

81
gtsam/geometry/Cal3Unified.h
View File

@ -28,40 +28,39 @@
namespace gtsam {
/**
* @brief Calibration of a omni-directional camera with mirror + lens radial distortion
* @brief Calibration of a omni-directional camera with mirror + lens radial
* distortion
* @addtogroup geometry
* \nosubgrouping
*
* 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 {
using This = Cal3Unified;
using Base = Cal3DS2_Base;
typedef Cal3Unified This;
typedef Cal3DS2_Base Base;
private:
double xi_; // mirror parameter
public:
private:
double xi_ = 0.0f; ///< mirror parameter
public:
enum { dimension = 10 };
/// @name Standard Constructors
/// @{
/// Default Constructor with only unit focal length
Cal3Unified() : Base(), xi_(0) {}
Cal3Unified() = default;
Cal3Unified(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) :
Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1,
double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0)
: Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
virtual ~Cal3Unified() {}
@ -69,12 +68,17 @@ public:
/// @name Advanced Constructors
/// @{
Cal3Unified(const Vector &v) ;
Cal3Unified(const Vector10& v)
: Base(v(0), v(1), v(2), v(3), v(4), v(5), v(6), v(7), v(8)), xi_(v(9)) {}
/// @}
/// @name Testable
/// @{
/// Output stream operator
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Cal3Unified& cal);
/// print with optional string
void print(const std::string& s = "") const override;
@ -86,7 +90,10 @@ public:
/// @{
/// mirror parameter
inline double xi() const { return xi_;}
inline double xi() const { return xi_; }
/// Return all parameters as a vector
Vector10 vector() const;
/**
* convert intrinsic coordinates xy to image coordinates uv
@ -96,8 +103,8 @@ public:
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p,
OptionalJacobian<2,10> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const ;
OptionalJacobian<2, 10> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// Conver a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none,
@ -114,41 +121,33 @@ public:
/// @{
/// Given delta vector, update calibration
Cal3Unified retract(const Vector& d) const ;
Cal3Unified retract(const Vector& d) const;
/// Given a different calibration, calculate update to obtain it
Vector10 localCoordinates(const Cal3Unified& T2) const ;
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; }
/// Return all parameters as a vector
Vector10 vector() const ;
inline static size_t Dim() { return dimension; }
/// @}
private:
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/)
{
ar & boost::serialization::make_nvp("Cal3Unified",
boost::serialization::base_object<Cal3DS2_Base>(*this));
ar & BOOST_SERIALIZATION_NVP(xi_);
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Cal3Unified", boost::serialization::base_object<Cal3DS2_Base>(*this));
ar& BOOST_SERIALIZATION_NVP(xi_);
}
};
template<>
template <>
struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
template<>
template <>
struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {};
}

92
gtsam/geometry/Cal3_S2.cpp
View File

@ -22,95 +22,55 @@
#include <iostream>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
Cal3_S2::Cal3_S2(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
fx_ = (double) w / (2.0 * tan(a)); // old formula: fx_ = (double) w * tan(a);
fy_ = fx_;
}
/* ************************************************************************* */
Cal3_S2::Cal3_S2(const std::string &path) :
fx_(320), fy_(320), s_(0), u0_(320), v0_(140) {
char buffer[200];
buffer[0] = 0;
sprintf(buffer, "%s/calibration_info.txt", path.c_str());
std::ifstream infile(buffer, std::ios::in);
if (infile)
infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
else {
throw std::runtime_error("Cal3_S2: Unable to load the calibration");
}
infile.close();
}
/* ************************************************************************* */
ostream& operator<<(ostream& os, const Cal3_S2& cal) {
os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px()
<< ", py:" << cal.py() << "}";
std::ostream& operator<<(std::ostream& os, const Cal3_S2& cal) {
// Use the base class version since it is identical.
os << (Cal3&)cal;
return os;
}
/* ************************************************************************* */
void Cal3_S2::print(const std::string& s) const {
gtsam::print((Matrix)matrix(), s);
gtsam::print((Matrix)K(), s);
}
/* ************************************************************************* */
bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
if (std::abs(fx_ - K.fx_) > tol)
return false;
if (std::abs(fy_ - K.fy_) > tol)
return false;
if (std::abs(s_ - K.s_) > tol)
return false;
if (std::abs(u0_ - K.u0_) > tol)
return false;
if (std::abs(v0_ - K.v0_) > tol)
return false;
return true;
return Cal3::equals(K, tol);
}
/* ************************************************************************* */
Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
OptionalJacobian<2, 2> Dp) const {
OptionalJacobian<2, 2> Dp) const {
const double x = p.x(), y = p.y();
if (Dcal)
*Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
if (Dp)
*Dp << fx_, s_, 0.0, fy_;
if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
if (Dp) *Dp << fx_, s_, 0.0, fy_;
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}
/* ************************************************************************* */
Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal,
OptionalJacobian<2,2> Dp) const {
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;
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, -inv_fx * point.y(),
-inv_fx, inv_fx_s_inv_fy,
0, -inv_fy * point.y(), 0, 0, -inv_fy;
if(Dp)
*Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
return point;
Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
OptionalJacobian<2, 2> Dp) const {
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;
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,
-inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, -inv_fy * point.y(),
0, 0, -inv_fy;
}
if (Dp) *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
return point;
}
/* ************************************************************************* */
Vector3 Cal3_S2::calibrate(const Vector3& p) const {
return matrix_inverse() * p;
}
Vector3 Cal3_S2::calibrate(const Vector3& p) const { return inverse() * p; }
/* ************************************************************************* */
} // namespace gtsam
} // namespace gtsam

194
gtsam/geometry/Cal3_S2.h
View File

@ -21,6 +21,7 @@
#pragma once
#include <gtsam/geometry/Cal3.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -30,31 +31,25 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3_S2 {
private:
double fx_, fy_, s_, u0_, v0_;
public:
class GTSAM_EXPORT Cal3_S2 : public Cal3 {
public:
enum { dimension = 5 };
typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
///< shared pointer to calibration object
using shared_ptr = boost::shared_ptr<Cal3_S2>;
/// @name Standard Constructors
/// @{
/// Create a default calibration that leaves coordinates unchanged
Cal3_S2() :
fx_(1), fy_(1), s_(0), u0_(0), v0_(0) {
}
Cal3_S2() = default;
/// constructor from doubles
Cal3_S2(double fx, double fy, double s, double u0, double v0) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {
}
Cal3_S2(double fx, double fy, double s, double u0, double v0)
: Cal3(fx, fy, s, u0, v0) {}
/// constructor from vector
Cal3_S2(const Vector &d) :
fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {
}
Cal3_S2(const Vector5& d) : Cal3(d) {}
/**
* Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect
@ -62,141 +57,65 @@ public:
* @param w image width
* @param h image height
*/
Cal3_S2(double fov, int w, int h);
Cal3_S2(double fov, int w, int h) : Cal3(fov, w, h) {}
/// @}
/// @name Advanced Constructors
/// @{
/**
* Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
* @param p point in intrinsic coordinates
* @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// load calibration from location (default name is calibration_info.txt)
Cal3_S2(const std::string &path);
/**
* Convert image coordinates uv to intrinsic coordinates xy
* @param p point in image coordinates
* @param Dcal optional 2*5 Jacobian wrpt Cal3 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/**
* Convert homogeneous image coordinates to intrinsic coordinates
* @param p point in image coordinates
* @return point in intrinsic coordinates
*/
Vector3 calibrate(const Vector3& p) const;
/// @}
/// @name Testable
/// @{
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Cal3_S2& cal);
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Cal3_S2& cal);
/// print with optional string
void print(const std::string& s = "Cal3_S2") const;
void print(const std::string& s = "Cal3_S2") const override;
/// Check if equal up to specified tolerance
bool equals(const Cal3_S2& K, double tol = 10e-9) const;
/// @}
/// @name Standard Interface
/// @{
/// focal length x
inline double fx() const {
return fx_;
}
/// focal length y
inline double fy() const {
return fy_;
}
/// aspect ratio
inline double aspectRatio() const {
return fx_/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_;
}
/// return the principal point
Point2 principalPoint() const {
return Point2(u0_, v0_);
}
/// vectorized form (column-wise)
Vector5 vector() const {
Vector5 v;
v << fx_, fy_, s_, u0_, v0_;
return v;
}
/// return calibration matrix K
Matrix3 K() const {
Matrix3 K;
K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
return K;
}
/** @deprecated The following function has been deprecated, use K above */
Matrix3 matrix() const {
return K();
}
/// return inverted calibration matrix inv(K)
Matrix3 matrix_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;
}
/**
* convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
* @param p point in intrinsic coordinates
* @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const;
/**
* convert image coordinates uv to intrinsic coordinates xy
* @param p point in image coordinates
* @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const;
/**
* convert homogeneous image coordinates to intrinsic coordinates
* @param p point in image coordinates
* @return point in intrinsic coordinates
*/
Vector3 calibrate(const Vector3& p) const;
/// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
inline Cal3_S2 between(const Cal3_S2& q,
OptionalJacobian<5,5> H1=boost::none,
OptionalJacobian<5,5> H2=boost::none) const {
if(H1) *H1 = -I_5x5;
if(H2) *H2 = I_5x5;
return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
OptionalJacobian<5, 5> H1 = boost::none,
OptionalJacobian<5, 5> H2 = boost::none) const {
if (H1) *H1 = -I_5x5;
if (H2) *H2 = I_5x5;
return Cal3_S2(q.fx_ - fx_, q.fy_ - fy_, q.s_ - s_, q.u0_ - u0_,
q.v0_ - v0_);
}
/// @}
/// @name Manifold
/// @{
/// 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 {
@ -212,27 +131,22 @@ public:
/// @name Advanced Interface
/// @{
private:
private:
/// 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_);
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Cal3_S2", boost::serialization::base_object<Cal3>(*this));
}
/// @}
};
template<>
template <>
struct traits<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
template<>
template <>
struct traits<const Cal3_S2> : public internal::Manifold<Cal3_S2> {};
} // \ namespace gtsam
} // \ namespace gtsam

50
gtsam/geometry/Cal3_S2Stereo.cpp
View File

@ -20,20 +20,56 @@
#include <iostream>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
std::ostream& operator<<(std::ostream& os, const Cal3_S2Stereo& cal) {
os << (Cal3_S2&)cal;
os << ", b: " << cal.baseline();
return os;
}
/* ************************************************************************* */
void Cal3_S2Stereo::print(const std::string& s) const {
K_.print(s+"K: ");
std::cout << s << "Baseline: " << b_ << std::endl;
}
std::cout << s << (s != "" ? " " : "");
std::cout << "K: " << (Matrix)K() << std::endl;
std::cout << "Baseline: " << b_ << std::endl;
}
/* ************************************************************************* */
bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const {
if (std::abs(b_ - other.b_) > tol) return false;
return K_.equals(other.K_,tol);
const Cal3_S2* base = dynamic_cast<const Cal3_S2*>(&other);
return (Cal3_S2::equals(*base, tol) &&
std::fabs(b_ - other.baseline()) < tol);
}
/* ************************************************************************* */
Point2 Cal3_S2Stereo::uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal,
OptionalJacobian<2, 2> Dp) const {
const double x = p.x(), y = p.y();
if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, 0.0, y, 0.0, 0.0, 1.0, 0.0;
if (Dp) *Dp << fx_, s_, 0.0, fy_;
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}
/* ************************************************************************* */
Point2 Cal3_S2Stereo::calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal,
OptionalJacobian<2, 2> Dp) const {
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;
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,
-inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, 0,
-inv_fy * point.y(), 0, 0, -inv_fy, 0;
}
if (Dp) *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
return point;
}
/* ************************************************************************* */
} // namespace gtsam
} // namespace gtsam

226
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -22,135 +22,143 @@
namespace gtsam {
/**
* @brief The most common 5DOF 3D->2D calibration, stereo version
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
private:
double b_ = 1.0f; ///< Stereo baseline.
public:
enum { dimension = 6 };
///< shared pointer to stereo calibration object
using shared_ptr = boost::shared_ptr<Cal3_S2Stereo>;
/// @name Standard Constructors
/// @
/// default calibration leaves coordinates unchanged
Cal3_S2Stereo() = default;
/// constructor from doubles
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
: Cal3_S2(fx, fy, s, u0, v0), b_(b) {}
/// constructor from vector
Cal3_S2Stereo(const Vector6& d)
: Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {}
/// easy constructor; field-of-view in degrees, assumes zero skew
Cal3_S2Stereo(double fov, int w, int h, double b)
: Cal3_S2(fov, w, h), b_(b) {}
/**
* @brief The most common 5DOF 3D->2D calibration, stereo version
* @addtogroup geometry
* \nosubgrouping
* Convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
* @param p point in intrinsic coordinates
* @param Dcal optional 2*6 Jacobian wrpt Cal3_S2Stereo parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
class GTSAM_EXPORT Cal3_S2Stereo {
private:
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
Cal3_S2 K_;
double b_;
/**
* Convert image coordinates uv to intrinsic coordinates xy
* @param p point in image coordinates
* @param Dcal optional 2*6 Jacobian wrpt Cal3_S2Stereo parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates
*/
Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
public:
/**
* Convert homogeneous image coordinates to intrinsic coordinates
* @param p point in image coordinates
* @return point in intrinsic coordinates
*/
Vector3 calibrate(const Vector3& p) const { return Cal3_S2::calibrate(p); }
enum { dimension = 6 };
typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptr; ///< shared pointer to stereo calibration object
/// @}
/// @name Testable
/// @{
/// @name Standard Constructors
/// @
/// Output stream operator
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Cal3_S2Stereo& cal);
/// default calibration leaves coordinates unchanged
Cal3_S2Stereo() :
K_(1, 1, 0, 0, 0), b_(1.0) {
}
/// print with optional string
void print(const std::string& s = "") const override;
/// constructor from doubles
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) :
K_(fx, fy, s, u0, v0), b_(b) {
}
/// Check if equal up to specified tolerance
bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const;
/// constructor from vector
Cal3_S2Stereo(const Vector &d): K_(d(0), d(1), d(2), d(3), d(4)), b_(d(5)){}
/// @}
/// @name Standard Interface
/// @{
/// easy constructor; field-of-view in degrees, assumes zero skew
Cal3_S2Stereo(double fov, int w, int h, double b) :
K_(fov, w, h), b_(b) {
}
/// return calibration, same for left and right
const Cal3_S2& calibration() const { return *this; }
/// @}
/// @name Testable
/// @{
/// return calibration matrix K, same for left and right
Matrix3 K() const override { return Cal3_S2::K(); }
void print(const std::string& s = "") const;
/// return baseline
inline double baseline() const { return b_; }
/// Check if equal up to specified tolerance
bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const;
/// vectorized form (column-wise)
Vector6 vector() const {
Vector6 v;
v << Cal3_S2::vector(), b_;
return v;
}
/// @}
/// @name Standard Interface
/// @{
/// @}
/// @name Manifold
/// @{
/// return calibration, same for left and right
const Cal3_S2& calibration() const { return K_;}
/// return DOF, dimensionality of tangent space
inline size_t dim() const override { return Dim(); }
/// return calibration matrix K, same for left and right
Matrix matrix() const { return K_.matrix();}
/// return DOF, dimensionality of tangent space
inline static size_t Dim() { return dimension; }
/// focal length x
inline double fx() const { return K_.fx();}
/// Given 6-dim tangent vector, create new calibration
inline Cal3_S2Stereo retract(const Vector& d) const {
return Cal3_S2Stereo(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3),
py() + d(4), b_ + d(5));
}
/// focal length x
inline double fy() const { return K_.fy();}
/// Unretraction for the calibration
Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
return T2.vector() - vector();
}
/// skew
inline double skew() const { return K_.skew();}
/// @}
/// @name Advanced Interface
/// @{
/// image center in x
inline double px() const { return K_.px();}
private:
/** Serialization function */
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
ar& BOOST_SERIALIZATION_NVP(b_);
}
/// @}
};
/// image center in y
inline double py() const { return K_.py();}
// Define GTSAM traits
template <>
struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {};
/// return the principal point
Point2 principalPoint() const { return K_.principalPoint();}
template <>
struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
};
/// return baseline
inline double baseline() const { return b_; }
/// vectorized form (column-wise)
Vector6 vector() const {
Vector6 v;
v << K_.vector(), b_;
return v;
}
/// @}
/// @name Manifold
/// @{
/// 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; }
/// Given 6-dim tangent vector, create new calibration
inline Cal3_S2Stereo retract(const Vector& d) const {
return Cal3_S2Stereo(K_.fx() + d(0), K_.fy() + d(1), K_.skew() + d(2), K_.px() + d(3), K_.py() + d(4), b_ + d(5));
}
/// Unretraction for the calibration
Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
return T2.vector() - vector();
}
/// @}
/// @name Advanced Interface
/// @{
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/)
{
ar & BOOST_SERIALIZATION_NVP(K_);
ar & BOOST_SERIALIZATION_NVP(b_);
}
/// @}
};
// Define GTSAM traits
template<>
struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
};
template<>
struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
};
} // \ namespace gtsam
} // \ namespace gtsam

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

@ -11,11 +11,12 @@
/**
* @file testCal3Bundler.cpp
* @brief Unit tests for transform derivatives
* @brief Unit tests for Bundler calibration model.
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Bundler.h>
@ -25,30 +26,27 @@ GTSAM_CONCEPT_TESTABLE_INST(Cal3Bundler)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3Bundler)
static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
static Point2 p(2,3);
static Point2 p(2, 3);
/* ************************************************************************* */
TEST( Cal3Bundler, vector)
{
TEST(Cal3Bundler, vector) {
Cal3Bundler K;
Vector expected(3);
expected << 1, 0, 0;
CHECK(assert_equal(expected,K.vector()));
CHECK(assert_equal(expected, K.vector()));
}
/* ************************************************************************* */
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()) ;
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());
Point2 actual = K.uncalibrate(p);
CHECK(assert_equal(expected,actual));
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);
@ -56,26 +54,28 @@ TEST( Cal3Bundler, calibrate )
}
/* ************************************************************************* */
Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) { return k.uncalibrate(pt); }
Point2 uncalibrate_(const Cal3Bundler& k, const Point2& pt) {
return k.uncalibrate(pt);
}
Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) { return k.calibrate(pt); }
/* ************************************************************************* */
TEST( Cal3Bundler, Duncalibrate)
{
Matrix Dcal, Dp;
Point2 actual = K.uncalibrate(p, Dcal, Dp);
Point2 expected(2182, 3773);
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));
CHECK(assert_equal(numerical2,Dp,1e-7));
Point2 calibrate_(const Cal3Bundler& k, const Point2& pt) {
return k.calibrate(pt);
}
/* ************************************************************************* */
TEST( Cal3Bundler, Dcalibrate)
{
TEST(Cal3Bundler, Duncalibrate) {
Matrix Dcal, Dp;
Point2 actual = K.uncalibrate(p, Dcal, Dp);
Point2 expected(2182, 3773);
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));
CHECK(assert_equal(numerical2, Dp, 1e-7));
}
/* ************************************************************************* */
TEST(Cal3Bundler, Dcalibrate) {
Matrix Dcal, Dp;
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
@ -83,27 +83,41 @@ TEST( Cal3Bundler, Dcalibrate)
CHECK(assert_equal(pn, actual, 1e-7));
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
CHECK(assert_equal(numerical1,Dcal,1e-5));
CHECK(assert_equal(numerical2,Dp,1e-5));
CHECK(assert_equal(numerical1, Dcal, 1e-5));
CHECK(assert_equal(numerical2, Dp, 1e-5));
}
/* ************************************************************************* */
TEST( Cal3Bundler, assert_equal)
{
CHECK(assert_equal(K,K,1e-7));
}
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);
Vector d(3);
EXPECT_LONGS_EQUAL(3, expected.dim());
EXPECT_LONGS_EQUAL(Cal3Bundler::Dim(), 3);
EXPECT_LONGS_EQUAL(expected.dim(), 3);
Vector3 d;
d << 10, 1e-3, 1e-3;
Cal3Bundler actual = K.retract(d);
CHECK(assert_equal(expected,actual,1e-7));
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
CHECK(assert_equal(expected, actual, 1e-7));
CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
TEST(Cal3_S2, Print) {
Cal3Bundler cal(1, 2, 3, 4, 5);
std::stringstream os;
os << "f: " << cal.fx() << ", k1: " << cal.k1() << ", k2: " << cal.k2()
<< ", px: " << cal.px() << ", py: " << cal.py();
EXPECT(assert_stdout_equal(os.str(), cal));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

27
gtsam/geometry/tests/testCal3DFisheye.cpp
View File

@ -10,12 +10,13 @@
* -------------------------------------------------------------------------- */
/**
* @file testCal3DFisheye.cpp
* @file testCal3Fisheye.cpp
* @brief Unit tests for fisheye calibration class
* @author ghaggin
*/
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Point3.h>
@ -41,7 +42,11 @@ TEST(Cal3Fisheye, retract) {
Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4,
K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8,
K.k4() + 9);
Vector d(9);
EXPECT_LONGS_EQUAL(Cal3Fisheye::Dim(), 9);
EXPECT_LONGS_EQUAL(expected.dim(), 9);
Vector9 d;
d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
Cal3Fisheye actual = K.retract(d);
CHECK(assert_equal(expected, actual, 1e-7));
@ -186,16 +191,26 @@ Point2 calibrate_(const Cal3Fisheye& k, const Point2& pt) {
}
/* ************************************************************************* */
TEST(Cal3Fisheye, Dcalibrate)
{
TEST(Cal3Fisheye, Dcalibrate) {
Point2 p(0.5, 0.5);
Point2 pi = K.uncalibrate(p);
Matrix Dcal, Dp;
K.calibrate(pi, Dcal, Dp);
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
CHECK(assert_equal(numerical1,Dcal,1e-5));
CHECK(assert_equal(numerical1, Dcal, 1e-5));
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
CHECK(assert_equal(numerical2,Dp,1e-5));
CHECK(assert_equal(numerical2, Dp, 1e-5));
}
/* ************************************************************************* */
TEST(Cal3Fisheye, Print) {
Cal3Fisheye cal(1, 2, 3, 4, 5, 6, 7, 8, 9);
std::stringstream os;
os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
<< ", k2: " << cal.k2() << ", k3: " << cal.k3() << ", k4: " << cal.k4();
EXPECT(assert_stdout_equal(os.str(), cal));
}
/* ************************************************************************* */

94
gtsam/geometry/tests/testCal3DS2.cpp
View File

@ -11,12 +11,12 @@
/**
* @file testCal3DS2.cpp
* @brief Unit tests for transform derivatives
* @brief Unit tests for Cal3DS2 calibration model.
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3DS2.h>
@ -25,53 +25,53 @@ using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Cal3DS2)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3DS2)
static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
static Point2 p(2,3);
static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0 * 1e-3, 3.0 * 1e-3,
4.0 * 1e-3);
static Point2 p(2, 3);
/* ************************************************************************* */
TEST( Cal3DS2, uncalibrate)
{
Vector k = K.k() ;
double r = p.x()*p.x() + p.y()*p.y() ;
double g = 1+k[0]*r+k[1]*r*r ;
double tx = 2*k[2]*p.x()*p.y() + k[3]*(r+2*p.x()*p.x()) ;
double ty = k[2]*(r+2*p.y()*p.y()) + 2*k[3]*p.x()*p.y() ;
Vector v_hat = (Vector(3) << g*p.x() + tx, g*p.y() + ty, 1.0).finished();
Vector v_i = K.K() * v_hat ;
Point2 p_i(v_i(0)/v_i(2), v_i(1)/v_i(2)) ;
TEST(Cal3DS2, Uncalibrate) {
Vector k = K.k();
double r = p.x() * p.x() + p.y() * p.y();
double g = 1 + k[0] * r + k[1] * r * r;
double tx = 2 * k[2] * p.x() * p.y() + k[3] * (r + 2 * p.x() * p.x());
double ty = k[2] * (r + 2 * p.y() * p.y()) + 2 * k[3] * p.x() * p.y();
Vector v_hat = (Vector(3) << g * p.x() + tx, g * p.y() + ty, 1.0).finished();
Vector v_i = K.K() * v_hat;
Point2 p_i(v_i(0) / v_i(2), v_i(1) / v_i(2));
Point2 q = K.uncalibrate(p);
CHECK(assert_equal(q,p_i));
CHECK(assert_equal(q, p_i));
}
TEST( Cal3DS2, calibrate )
{
TEST(Cal3DS2, Calibrate) {
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Point2 pn_hat = K.calibrate(pi);
CHECK( traits<Point2>::Equals(pn, pn_hat, 1e-5));
CHECK(traits<Point2>::Equals(pn, pn_hat, 1e-5));
}
Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); }
Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) {
return k.uncalibrate(pt);
}
/* ************************************************************************* */
TEST( Cal3DS2, Duncalibrate1)
{
TEST(Cal3DS2, Duncalibrate1) {
Matrix computed;
K.uncalibrate(p, computed, boost::none);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical,computed,1e-5));
CHECK(assert_equal(numerical, computed, 1e-5));
Matrix separate = K.D2d_calibration(p);
CHECK(assert_equal(numerical,separate,1e-5));
CHECK(assert_equal(numerical, separate, 1e-5));
}
/* ************************************************************************* */
TEST( Cal3DS2, Duncalibrate2)
{
Matrix computed; K.uncalibrate(p, boost::none, computed);
TEST(Cal3DS2, Duncalibrate2) {
Matrix computed;
K.uncalibrate(p, boost::none, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical,computed,1e-5));
CHECK(assert_equal(numerical, computed, 1e-5));
Matrix separate = K.D2d_intrinsic(p);
CHECK(assert_equal(numerical,separate,1e-5));
CHECK(assert_equal(numerical, separate, 1e-5));
}
Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
@ -79,8 +79,7 @@ Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
}
/* ************************************************************************* */
TEST( Cal3DS2, Dcalibrate)
{
TEST(Cal3DS2, Dcalibrate) {
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Matrix Dcal, Dp;
@ -92,20 +91,37 @@ TEST( Cal3DS2, Dcalibrate)
}
/* ************************************************************************* */
TEST(Cal3DS2, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
TEST(Cal3DS2, Equal) { CHECK(assert_equal(K, K, 1e-5)); }
/* ************************************************************************* */
TEST( Cal3DS2, retract)
{
TEST(Cal3DS2, Retract) {
Cal3DS2 expected(500 + 1, 100 + 2, 0.1 + 3, 320 + 4, 240 + 5, 1e-3 + 6,
2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9);
Vector d(9);
d << 1,2,3,4,5,6,7,8,9;
2.0 * 1e-3 + 7, 3.0 * 1e-3 + 8, 4.0 * 1e-3 + 9);
EXPECT_LONGS_EQUAL(Cal3DS2::Dim(), 9);
EXPECT_LONGS_EQUAL(expected.dim(), 9);
Vector9 d;
d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
Cal3DS2 actual = K.retract(d);
CHECK(assert_equal(expected,actual,1e-7));
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
CHECK(assert_equal(expected, actual, 1e-7));
CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
TEST(Cal3DS2, Print) {
Cal3DS2 cal(1, 2, 3, 4, 5, 6, 7, 8, 9);
std::stringstream os;
os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
<< ", k2: " << cal.k2() << ", p1: " << cal.p1() << ", p2: " << cal.p2();
EXPECT(assert_stdout_equal(os.str(), cal));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

91
gtsam/geometry/tests/testCal3Unified.cpp
View File

@ -10,17 +10,18 @@
* -------------------------------------------------------------------------- */
/**
* @file testCal3Unify.cpp
* @brief Unit tests for transform derivatives
* @file testCal3Unified.cpp
* @brief Unit tests for Cal3Unified calibration model.
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Key.h>
#include <gtsam/nonlinear/Values.h>
using namespace gtsam;
@ -35,51 +36,49 @@ V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240];
matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox
*/
static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1);
static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0 * 1e-3, 3.0 * 1e-3,
4.0 * 1e-3, 0.1);
static Point2 p(0.5, 0.7);
/* ************************************************************************* */
TEST( Cal3Unified, uncalibrate)
{
Point2 p_i(364.7791831734982, 305.6677211952602) ;
TEST(Cal3Unified, Uncalibrate) {
Point2 p_i(364.7791831734982, 305.6677211952602);
Point2 q = K.uncalibrate(p);
CHECK(assert_equal(q,p_i));
CHECK(assert_equal(q, p_i));
}
/* ************************************************************************* */
TEST( Cal3Unified, spaceNplane)
{
TEST(Cal3Unified, SpaceNplane) {
Point2 q = K.spaceToNPlane(p);
CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q));
CHECK(assert_equal(p, K.nPlaneToSpace(q)));
}
/* ************************************************************************* */
TEST( Cal3Unified, calibrate)
{
TEST(Cal3Unified, Calibrate) {
Point2 pi = K.uncalibrate(p);
Point2 pn_hat = K.calibrate(pi);
CHECK( traits<Point2>::Equals(p, pn_hat, 1e-8));
CHECK(traits<Point2>::Equals(p, pn_hat, 1e-8));
}
Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); }
Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) {
return k.uncalibrate(pt);
}
/* ************************************************************************* */
TEST( Cal3Unified, Duncalibrate1)
{
TEST(Cal3Unified, Duncalibrate1) {
Matrix computed;
K.uncalibrate(p, computed, boost::none);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical,computed,1e-6));
CHECK(assert_equal(numerical, computed, 1e-6));
}
/* ************************************************************************* */
TEST( Cal3Unified, Duncalibrate2)
{
TEST(Cal3Unified, Duncalibrate2) {
Matrix computed;
K.uncalibrate(p, boost::none, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical,computed,1e-6));
CHECK(assert_equal(numerical, computed, 1e-6));
}
Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
@ -87,38 +86,37 @@ Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
}
/* ************************************************************************* */
TEST( Cal3Unified, Dcalibrate)
{
TEST(Cal3Unified, Dcalibrate) {
Point2 pi = K.uncalibrate(p);
Matrix Dcal, Dp;
K.calibrate(pi, Dcal, Dp);
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
CHECK(assert_equal(numerical1,Dcal,1e-5));
CHECK(assert_equal(numerical1, Dcal, 1e-5));
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
CHECK(assert_equal(numerical2,Dp,1e-5));
CHECK(assert_equal(numerical2, Dp, 1e-5));
}
/* ************************************************************************* */
TEST( Cal3Unified, assert_equal)
{
CHECK(assert_equal(K,K,1e-9));
}
TEST(Cal3Unified, Equal) { CHECK(assert_equal(K, K, 1e-9)); }
/* ************************************************************************* */
TEST( Cal3Unified, retract)
{
Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1);
Vector d(10);
TEST(Cal3Unified, Retract) {
Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7,
2.0 * 1e-3 + 8, 3.0 * 1e-3 + 9, 4.0 * 1e-3 + 10,
0.1 + 1);
EXPECT_LONGS_EQUAL(Cal3Unified::Dim(), 10);
EXPECT_LONGS_EQUAL(expected.dim(), 10);
Vector10 d;
d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1;
Cal3Unified actual = K.retract(d);
CHECK(assert_equal(expected,actual,1e-9));
CHECK(assert_equal(d,K.localCoordinates(actual),1e-9));
CHECK(assert_equal(expected, actual, 1e-9));
CHECK(assert_equal(d, K.localCoordinates(actual), 1e-9));
}
/* ************************************************************************* */
TEST( Cal3Unified, DerivedValue)
{
TEST(Cal3Unified, DerivedValue) {
Values values;
Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10);
Key key = 1;
@ -126,9 +124,24 @@ TEST( Cal3Unified, DerivedValue)
Cal3Unified calafter = values.at<Cal3Unified>(key);
CHECK(assert_equal(cal,calafter,1e-9));
CHECK(assert_equal(cal, calafter, 1e-9));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
TEST(Cal3Unified, Print) {
Cal3Unified cal(0, 1, 2, 3, 4, 5, 6, 7, 8, 9);
std::stringstream os;
os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py() << ", k1: " << cal.k1()
<< ", k2: " << cal.k2() << ", p1: " << cal.p1() << ", p2: " << cal.p2()
<< ", xi: " << cal.xi();
EXPECT(assert_stdout_equal(os.str(), cal));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

112
gtsam/geometry/tests/testCal3_S2.cpp
View File

@ -11,7 +11,7 @@
/**
* @file testCal3_S2.cpp
* @brief Unit tests for transform derivatives
* @brief Unit tests for basic Cal3_S2 calibration model.
*/
#include <CppUnitLite/TestHarness.h>
@ -31,90 +31,94 @@ static Point2 p_uv(1320.3, 1740);
static Point2 p_xy(2, 3);
/* ************************************************************************* */
TEST( Cal3_S2, easy_constructor)
{
TEST(Cal3_S2, Constructor) {
Cal3_S2 expected(554.256, 554.256, 0, 640 / 2, 480 / 2);
double fov = 60; // degrees
size_t w=640,h=480;
Cal3_S2 actual(fov,w,h);
double fov = 60; // degrees
size_t w = 640, h = 480;
Cal3_S2 actual(fov, w, h);
CHECK(assert_equal(expected,actual,1e-3));
CHECK(assert_equal(expected, actual, 1e-3));
}
/* ************************************************************************* */
TEST( Cal3_S2, calibrate)
{
Point2 intrinsic(2,3);
TEST(Cal3_S2, Calibrate) {
Point2 intrinsic(2, 3);
Point2 expectedimage(1320.3, 1740);
Point2 imagecoordinates = K.uncalibrate(intrinsic);
CHECK(assert_equal(expectedimage,imagecoordinates));
CHECK(assert_equal(intrinsic,K.calibrate(imagecoordinates)));
CHECK(assert_equal(expectedimage, imagecoordinates));
CHECK(assert_equal(intrinsic, K.calibrate(imagecoordinates)));
}
/* ************************************************************************* */
TEST( Cal3_S2, calibrate_homogeneous) {
TEST(Cal3_S2, CalibrateHomogeneous) {
Vector3 intrinsic(2, 3, 1);
Vector3 image(1320.3, 1740, 1);
CHECK(assert_equal((Vector)intrinsic,(Vector)K.calibrate(image)));
CHECK(assert_equal((Vector)intrinsic, (Vector)K.calibrate(image)));
}
/* ************************************************************************* */
Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); }
TEST( Cal3_S2, Duncalibrate1)
{
Matrix25 computed; K.uncalibrate(p, computed, boost::none);
Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) {
return k.uncalibrate(pt);
}
TEST(Cal3_S2, Duncalibrate1) {
Matrix25 computed;
K.uncalibrate(p, computed, boost::none);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
CHECK(assert_equal(numerical,computed,1e-8));
CHECK(assert_equal(numerical, computed, 1e-8));
}
/* ************************************************************************* */
TEST( Cal3_S2, Duncalibrate2)
{
Matrix computed; K.uncalibrate(p, boost::none, computed);
TEST(Cal3_S2, Duncalibrate2) {
Matrix computed;
K.uncalibrate(p, boost::none, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical,computed,1e-9));
CHECK(assert_equal(numerical, computed, 1e-9));
}
Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {return k.calibrate(pt); }
/* ************************************************************************* */
TEST(Cal3_S2, Dcalibrate1)
{
Matrix computed;
Point2 expected = K.calibrate(p_uv, computed, boost::none);
Matrix numerical = numericalDerivative21(calibrate_, K, p_uv);
CHECK(assert_equal(expected, p_xy, 1e-8));
CHECK(assert_equal(numerical, computed, 1e-8));
Point2 calibrate_(const Cal3_S2& k, const Point2& pt) {
return k.calibrate(pt);
}
/* ************************************************************************* */
TEST(Cal3_S2, Dcalibrate2)
{
Matrix computed;
Point2 expected = K.calibrate(p_uv, boost::none, computed);
Matrix numerical = numericalDerivative22(calibrate_, K, p_uv);
CHECK(assert_equal(expected, p_xy, 1e-8));
CHECK(assert_equal(numerical, computed, 1e-8));
TEST(Cal3_S2, Dcalibrate1) {
Matrix computed;
Point2 expected = K.calibrate(p_uv, computed, boost::none);
Matrix numerical = numericalDerivative21(calibrate_, K, p_uv);
CHECK(assert_equal(expected, p_xy, 1e-8));
CHECK(assert_equal(numerical, computed, 1e-8));
}
/* ************************************************************************* */
TEST( Cal3_S2, assert_equal)
{
CHECK(assert_equal(K,K,1e-9));
TEST(Cal3_S2, Dcalibrate2) {
Matrix computed;
Point2 expected = K.calibrate(p_uv, boost::none, computed);
Matrix numerical = numericalDerivative22(calibrate_, K, p_uv);
CHECK(assert_equal(expected, p_xy, 1e-8));
CHECK(assert_equal(numerical, computed, 1e-8));
}
/* ************************************************************************* */
TEST(Cal3_S2, Equal) {
CHECK(assert_equal(K, K, 1e-9));
Cal3_S2 K1(500, 500, 0.1, 640 / 2, 480 / 2);
CHECK(assert_equal(K,K1,1e-9));
CHECK(assert_equal(K, K1, 1e-9));
}
/* ************************************************************************* */
TEST( Cal3_S2, retract)
{
Cal3_S2 expected(500+1, 500+2, 0.1+3, 640 / 2+4, 480 / 2+5);
Vector d(5);
d << 1,2,3,4,5;
TEST(Cal3_S2, Retract) {
Cal3_S2 expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5);
EXPECT_LONGS_EQUAL(Cal3_S2::Dim(), 5);
EXPECT_LONGS_EQUAL(expected.dim(), 5);
Vector5 d;
d << 1, 2, 3, 4, 5;
Cal3_S2 actual = K.retract(d);
CHECK(assert_equal(expected,actual,1e-7));
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
CHECK(assert_equal(expected, actual, 1e-7));
CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
}
/* ************************************************************************* */
@ -122,18 +126,17 @@ TEST(Cal3_S2, between) {
Cal3_S2 k1(5, 5, 5, 5, 5), k2(5, 6, 7, 8, 9);
Matrix H1, H2;
EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2)));
EXPECT(assert_equal(Cal3_S2(0, 1, 2, 3, 4), k1.between(k2, H1, H2)));
EXPECT(assert_equal(-I_5x5, H1));
EXPECT(assert_equal(I_5x5, H2));
}
/* ************************************************************************* */
TEST(Cal3_S2, Print) {
Cal3_S2 cal(5, 5, 5, 5, 5);
std::stringstream os;
os << "{fx: " << cal.fx() << ", fy: " << cal.fy() << ", s:" << cal.skew() << ", px:" << cal.px()
<< ", py:" << cal.py() << "}";
os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py();
EXPECT(assert_stdout_equal(os.str(), cal));
}
@ -144,4 +147,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

129
gtsam/geometry/tests/testCal3_S2Stereo.cpp Normal file
View File

@ -0,0 +1,129 @@
/* ----------------------------------------------------------------------------
* 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 testCal3_S2Stereo.cpp
* @brief Unit tests for stereo-rig calibration model.
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Cal3_S2Stereo)
GTSAM_CONCEPT_MANIFOLD_INST(Cal3_S2Stereo)
static Cal3_S2Stereo K(500, 500, 0.1, 640 / 2, 480 / 2, 1);
static Point2 p(1, -2);
static Point2 p_uv(1320.3, 1740);
static Point2 p_xy(2, 3);
/* ************************************************************************* */
TEST(Cal3_S2Stereo, Constructor) {
Cal3_S2Stereo expected(554.256, 554.256, 0, 640 / 2, 480 / 2, 3);
double fov = 60; // degrees
size_t w = 640, h = 480;
Cal3_S2Stereo actual(fov, w, h, 3);
CHECK(assert_equal(expected, actual, 1e-3));
}
/* ************************************************************************* */
TEST(Cal3_S2Stereo, Calibrate) {
Point2 intrinsic(2, 3);
Point2 expectedimage(1320.3, 1740);
Point2 imagecoordinates = K.uncalibrate(intrinsic);
CHECK(assert_equal(expectedimage, imagecoordinates));
CHECK(assert_equal(intrinsic, K.calibrate(imagecoordinates)));
}
/* ************************************************************************* */
TEST(Cal3_S2Stereo, CalibrateHomogeneous) {
Vector3 intrinsic(2, 3, 1);
Vector3 image(1320.3, 1740, 1);
CHECK(assert_equal(intrinsic, K.calibrate(image)));
}
/* ************************************************************************* */
Point2 uncalibrate_(const Cal3_S2Stereo& k, const Point2& pt) {
return k.uncalibrate(pt);
}
TEST(Cal3_S2Stereo, Duncalibrate) {
Matrix26 Dcal;
Matrix22 Dp;
K.uncalibrate(p, Dcal, Dp);
Matrix numerical1 = numericalDerivative21(uncalibrate_, K, p);
CHECK(assert_equal(numerical1, Dcal, 1e-8));
Matrix numerical2 = numericalDerivative22(uncalibrate_, K, p);
CHECK(assert_equal(numerical2, Dp, 1e-9));
}
Point2 calibrate_(const Cal3_S2Stereo& K, const Point2& pt) {
return K.calibrate(pt);
}
/* ************************************************************************* */
TEST(Cal3_S2Stereo, Dcalibrate) {
Matrix26 Dcal;
Matrix22 Dp;
Point2 expected = K.calibrate(p_uv, Dcal, Dp);
CHECK(assert_equal(expected, p_xy, 1e-8));
Matrix numerical1 = numericalDerivative21(calibrate_, K, p_uv);
CHECK(assert_equal(numerical1, Dcal, 1e-8));
Matrix numerical2 = numericalDerivative22(calibrate_, K, p_uv);
CHECK(assert_equal(numerical2, Dp, 1e-8));
}
/* ************************************************************************* */
TEST(Cal3_S2Stereo, Equal) {
CHECK(assert_equal(K, K, 1e-9));
Cal3_S2Stereo K1(500, 500, 0.1, 640 / 2, 480 / 2, 1);
CHECK(assert_equal(K, K1, 1e-9));
}
/* ************************************************************************* */
TEST(Cal3_S2Stereo, Retract) {
Cal3_S2Stereo expected(500 + 1, 500 + 2, 0.1 + 3, 640 / 2 + 4, 480 / 2 + 5,
7);
EXPECT_LONGS_EQUAL(Cal3_S2Stereo::Dim(), 6);
EXPECT_LONGS_EQUAL(expected.dim(), 6);
Vector6 d;
d << 1, 2, 3, 4, 5, 6;
Cal3_S2Stereo actual = K.retract(d);
CHECK(assert_equal(expected, actual, 1e-7));
CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
}
/* ************************************************************************* */
TEST(Cal3_S2Stereo, Print) {
Cal3_S2Stereo cal(5, 5, 5, 5, 5, 2);
std::stringstream os;
os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py()
<< ", b: " << cal.baseline();
EXPECT(assert_stdout_equal(os.str(), cal));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

62
gtsam/gtsam.i
View File

@ -849,43 +849,41 @@ namespace gtsam
};
#include <gtsam/geometry/Cal3_S2.h>
class Cal3_S2
{
// Standard Constructors
Cal3_S2();
Cal3_S2(double fx, double fy, double s, double u0, double v0);
Cal3_S2(Vector v);
Cal3_S2(double fov, int w, int h);
class Cal3_S2 {
// Standard Constructors
Cal3_S2();
Cal3_S2(double fx, double fy, double s, double u0, double v0);
Cal3_S2(Vector v);
Cal3_S2(double fov, int w, int h);
// Testable
void print(string s) const;
bool equals(const gtsam::Cal3_S2 &rhs, double tol) const;
// Testable
void print(string s) const;
bool equals(const gtsam::Cal3_S2& rhs, double tol) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Cal3_S2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3_S2 &c) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Cal3_S2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3_S2& c) const;
// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2 &p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2 &p) const;
// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
// Standard Interface
double fx() const;
double fy() const;
double skew() const;
double px() const;
double py() const;
gtsam::Point2 principalPoint() const;
Vector vector() const;
Matrix K() const;
Matrix matrix() const;
Matrix matrix_inverse() const;
// Standard Interface
double fx() const;
double fy() const;
double skew() const;
double px() const;
double py() const;
gtsam::Point2 principalPoint() const;
Vector vector() const;
Matrix K() const;
Matrix inverse() const;
// enabling serialization functionality
void serialize() const;
};
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3DS2_Base.h>
virtual class Cal3DS2_Base

5
gtsam/inference/Symbol.h
View File

@ -167,10 +167,11 @@ inline Key Z(std::uint64_t j) { return Symbol('z', j); }
/** Generates symbol shorthands with alternative names different than the
* one-letter predefined ones. */
class SymbolGenerator {
const char c_;
const unsigned char c_;
public:
SymbolGenerator(const char c) : c_(c) {}
constexpr SymbolGenerator(const unsigned char c) : c_(c) {}
Symbol operator()(const std::uint64_t j) const { return Symbol(c_, j); }
constexpr unsigned char chr() const { return c_; }
};
/// traits

6
gtsam/inference/tests/testKey.cpp
View File

@ -59,6 +59,12 @@ TEST(Key, SymbolGenerator) {
EXPECT(assert_equal(a1, ddz1));
}
/* ************************************************************************* */
TEST(Key, SymbolGeneratorConstexpr) {
constexpr auto Z = gtsam::SymbolGenerator('x');
EXPECT(assert_equal(Z.chr(), 'x'));
}
/* ************************************************************************* */
template<int KeySize>
Key KeyTestValue();

65
gtsam/sfm/TranslationRecovery.cpp
View File

@ -11,13 +11,12 @@
/**
* @file TranslationRecovery.cpp
* @author Frank Dellaert
* @author Frank Dellaert, Akshay Krishnan
* @date March 2020
* @brief Source code for recovering translations when rotations are given
*/
#include <gtsam/sfm/TranslationRecovery.h>
#include <gtsam/base/DSFMap.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
@ -27,11 +26,45 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/TranslationFactor.h>
#include <gtsam/sfm/TranslationRecovery.h>
#include <gtsam/slam/PriorFactor.h>
#include <set>
#include <utility>
using namespace gtsam;
using namespace std;
TranslationRecovery::TranslationRecovery(
const TranslationRecovery::TranslationEdges &relativeTranslations,
const LevenbergMarquardtParams &lmParams)
: params_(lmParams) {
// Some relative translations may be zero. We treat nodes that have a zero
// relativeTranslation as a single node.
// A DSFMap is used to find sets of nodes that have a zero relative
// translation. Add the nodes in each edge to the DSFMap, and merge nodes that
// are connected by a zero relative translation.
DSFMap<Key> sameTranslationDSF;
for (const auto &edge : relativeTranslations) {
Key key1 = sameTranslationDSF.find(edge.key1());
Key key2 = sameTranslationDSF.find(edge.key2());
if (key1 != key2 && edge.measured().equals(Unit3(0.0, 0.0, 0.0))) {
sameTranslationDSF.merge(key1, key2);
}
}
// Use only those edges for which two keys have a distinct root in the DSFMap.
for (const auto &edge : relativeTranslations) {
Key key1 = sameTranslationDSF.find(edge.key1());
Key key2 = sameTranslationDSF.find(edge.key2());
if (key1 == key2) continue;
relativeTranslations_.emplace_back(key1, key2, edge.measured(),
edge.noiseModel());
}
// Store the DSF map for post-processing results.
sameTranslationNodes_ = sameTranslationDSF.sets();
}
NonlinearFactorGraph TranslationRecovery::buildGraph() const {
NonlinearFactorGraph graph;
@ -44,13 +77,14 @@ NonlinearFactorGraph TranslationRecovery::buildGraph() const {
return graph;
}
void TranslationRecovery::addPrior(const double scale,
NonlinearFactorGraph *graph,
const SharedNoiseModel &priorNoiseModel) const {
void TranslationRecovery::addPrior(
const double scale, NonlinearFactorGraph *graph,
const SharedNoiseModel &priorNoiseModel) const {
auto edge = relativeTranslations_.begin();
graph->emplace_shared<PriorFactor<Point3> >(edge->key1(), Point3(0, 0, 0), priorNoiseModel);
graph->emplace_shared<PriorFactor<Point3> >(edge->key2(), scale * edge->measured().point3(),
edge->noiseModel());
graph->emplace_shared<PriorFactor<Point3> >(edge->key1(), Point3(0, 0, 0),
priorNoiseModel);
graph->emplace_shared<PriorFactor<Point3> >(
edge->key2(), scale * edge->measured().point3(), edge->noiseModel());
}
Values TranslationRecovery::initalizeRandomly() const {
@ -77,6 +111,19 @@ Values TranslationRecovery::run(const double scale) const {
const Values initial = initalizeRandomly();
LevenbergMarquardtOptimizer lm(graph, initial, params_);
Values result = lm.optimize();
// Nodes that were not optimized are stored in sameTranslationNodes_ as a map
// from a key that was optimized to keys that were not optimized. Iterate over
// map and add results for keys not optimized.
for (const auto &optimizedAndDuplicateKeys : sameTranslationNodes_) {
Key optimizedKey = optimizedAndDuplicateKeys.first;
std::set<Key> duplicateKeys = optimizedAndDuplicateKeys.second;
// Add the result for the duplicate key if it does not already exist.
for (const Key duplicateKey : duplicateKeys) {
if (result.exists(duplicateKey)) continue;
result.insert<Point3>(duplicateKey, result.at<Point3>(optimizedKey));
}
}
return result;
}

31
gtsam/sfm/TranslationRecovery.h
View File

@ -16,14 +16,16 @@
* @brief Recovering translations in an epipolar graph when rotations are given.
*/
#include <map>
#include <set>
#include <utility>
#include <vector>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/sfm/BinaryMeasurement.h>
#include <utility>
#include <vector>
namespace gtsam {
// Set up an optimization problem for the unknown translations Ti in the world
@ -52,23 +54,30 @@ class TranslationRecovery {
using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
private:
// Translation directions between camera pairs.
TranslationEdges relativeTranslations_;
// Parameters used by the LM Optimizer.
LevenbergMarquardtParams params_;
// Map from a key in the graph to a set of keys that share the same
// translation.
std::map<Key, std::set<Key>> sameTranslationNodes_;
public:
/**
* @brief Construct a new Translation Recovery object
*
* @param relativeTranslations the relative translations, in world coordinate
* frames, vector of BinaryMeasurements of Unit3, where each key of a measurement
* is a point in 3D.
* frames, vector of BinaryMeasurements of Unit3, where each key of a
* measurement is a point in 3D.
* @param lmParams (optional) gtsam::LavenbergMarquardtParams that can be
* used to modify the parameters for the LM optimizer. By default, uses the
* default LM parameters.
* default LM parameters.
*/
TranslationRecovery(const TranslationEdges &relativeTranslations,
const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams())
: relativeTranslations_(relativeTranslations), params_(lmParams) {}
TranslationRecovery(
const TranslationEdges &relativeTranslations,
const LevenbergMarquardtParams &lmParams = LevenbergMarquardtParams());
/**
* @brief Build the factor graph to do the optimization.
@ -108,8 +117,8 @@ class TranslationRecovery {
*
* @param poses SE(3) ground truth poses stored as Values
* @param edges pairs (a,b) for which a measurement w_aZb will be generated.
* @return TranslationEdges vector of binary measurements where the keys are
* the cameras and the measurement is the simulated Unit3 translation
* @return TranslationEdges vector of binary measurements where the keys are
* the cameras and the measurement is the simulated Unit3 translation
* direction between the cameras.
*/
static TranslationEdges SimulateMeasurements(

189
tests/testTranslationRecovery.cpp
View File

@ -11,19 +11,29 @@
/**
* @file testTranslationRecovery.cpp
* @author Frank Dellaert
* @author Frank Dellaert, Akshay Krishnan
* @date March 2020
* @brief test recovering translations when rotations are given.
*/
#include <gtsam/sfm/TranslationRecovery.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/sfm/TranslationRecovery.h>
#include <gtsam/slam/dataset.h>
using namespace std;
using namespace gtsam;
// Returns the Unit3 direction as measured in the binary measurement, but
// computed from the input poses. Helper function used in the unit tests.
Unit3 GetDirectionFromPoses(const Values& poses,
const BinaryMeasurement<Unit3>& unitTranslation) {
const Pose3 wTa = poses.at<Pose3>(unitTranslation.key1()),
wTb = poses.at<Pose3>(unitTranslation.key2());
const Point3 Ta = wTa.translation(), Tb = wTb.translation();
return Unit3(Tb - Ta);
}
/* ************************************************************************* */
// We read the BAL file, which has 3 cameras in it, with poses. We then assume
// the rotations are correct, but translations have to be estimated from
@ -48,43 +58,186 @@ TEST(TranslationRecovery, BAL) {
const auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
poses, {{0, 1}, {0, 2}, {1, 2}});
// Check
Unit3 w_aZb_stored; // measurement between 0 and 1 stored for next unit test
for(auto& unitTranslation : relativeTranslations) {
const Pose3 wTa = poses.at<Pose3>(unitTranslation.key1()),
wTb = poses.at<Pose3>(unitTranslation.key2());
const Point3 Ta = wTa.translation(), Tb = wTb.translation();
const Unit3 w_aZb = unitTranslation.measured();
EXPECT(assert_equal(Unit3(Tb - Ta), w_aZb));
if(unitTranslation.key1() == 0 && unitTranslation.key2() == 1) {
w_aZb_stored = unitTranslation.measured();
}
// Check simulated measurements.
for (auto& unitTranslation : relativeTranslations) {
EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
unitTranslation.measured()));
}
TranslationRecovery algorithm(relativeTranslations);
const auto graph = algorithm.buildGraph();
EXPECT_LONGS_EQUAL(3, graph.size());
// Translation recovery, version 1
// Run translation recovery
const double scale = 2.0;
const auto result = algorithm.run(scale);
// Check result for first two translations, determined by prior
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
EXPECT(assert_equal(Point3(2 * w_aZb_stored.point3()), result.at<Point3>(1)));
EXPECT(assert_equal(
Point3(2 * GetDirectionFromPoses(poses, relativeTranslations[0])),
result.at<Point3>(1)));
// Check that the third translations is correct
Point3 Ta = poses.at<Pose3>(0).translation();
Point3 Tb = poses.at<Pose3>(1).translation();
Point3 Tc = poses.at<Pose3>(2).translation();
Point3 expected =
(Tc - Ta) * (scale / (Tb - Ta).norm());
Point3 expected = (Tc - Ta) * (scale / (Tb - Ta).norm());
EXPECT(assert_equal(expected, result.at<Point3>(2), 1e-4));
// TODO(frank): how to get stats back?
// EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5);
}
TEST(TranslationRecovery, TwoPoseTest) {
// Create a dataset with 2 poses.
// __ __
// \/ \/
// 0 _____ 1
//
// 0 and 1 face in the same direction but have a translation offset.
Values poses;
poses.insert<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
auto relativeTranslations =
TranslationRecovery::SimulateMeasurements(poses, {{0, 1}});
// Check simulated measurements.
for (auto& unitTranslation : relativeTranslations) {
EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
unitTranslation.measured()));
}
TranslationRecovery algorithm(relativeTranslations);
const auto graph = algorithm.buildGraph();
EXPECT_LONGS_EQUAL(1, graph.size());
// Run translation recovery
const auto result = algorithm.run(/*scale=*/3.0);
// Check result for first two translations, determined by prior
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1)));
}
TEST(TranslationRecovery, ThreePoseTest) {
// Create a dataset with 3 poses.
// __ __
// \/ \/
// 0 _____ 1
// \ __ /
// \\//
// 3
//
// 0 and 1 face in the same direction but have a translation offset. 3 is in
// the same direction as 0 and 1, in between 0 and 1, with some Y axis offset.
Values poses;
poses.insert<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
poses, {{0, 1}, {1, 3}, {3, 0}});
// Check simulated measurements.
for (auto& unitTranslation : relativeTranslations) {
EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
unitTranslation.measured()));
}
TranslationRecovery algorithm(relativeTranslations);
const auto graph = algorithm.buildGraph();
EXPECT_LONGS_EQUAL(3, graph.size());
const auto result = algorithm.run(/*scale=*/3.0);
// Check result
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1)));
EXPECT(assert_equal(Point3(1.5, -1.5, 0), result.at<Point3>(3)));
}
TEST(TranslationRecovery, ThreePosesIncludingZeroTranslation) {
// Create a dataset with 3 poses.
// __ __
// \/ \/
// 0 _____ 1
// 2 <|
//
// 0 and 1 face in the same direction but have a translation offset. 2 is at
// the same point as 1 but is rotated, with little FOV overlap.
Values poses;
poses.insert<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
poses.insert<Pose3>(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0)));
auto relativeTranslations =
TranslationRecovery::SimulateMeasurements(poses, {{0, 1}, {1, 2}});
// Check simulated measurements.
for (auto& unitTranslation : relativeTranslations) {
EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
unitTranslation.measured()));
}
TranslationRecovery algorithm(relativeTranslations);
const auto graph = algorithm.buildGraph();
// There is only 1 non-zero translation edge.
EXPECT_LONGS_EQUAL(1, graph.size());
// Run translation recovery
const auto result = algorithm.run(/*scale=*/3.0);
// Check result
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(1)));
EXPECT(assert_equal(Point3(3, 0, 0), result.at<Point3>(2)));
}
TEST(TranslationRecovery, FourPosesIncludingZeroTranslation) {
// Create a dataset with 4 poses.
// __ __
// \/ \/
// 0 _____ 1
// \ __ 2 <|
// \\//
// 3
//
// 0 and 1 face in the same direction but have a translation offset. 2 is at
// the same point as 1 but is rotated, with very little FOV overlap. 3 is in
// the same direction as 0 and 1, in between 0 and 1, with some Y axis offset.
Values poses;
poses.insert<Pose3>(0, Pose3(Rot3(), Point3(0, 0, 0)));
poses.insert<Pose3>(1, Pose3(Rot3(), Point3(2, 0, 0)));
poses.insert<Pose3>(2, Pose3(Rot3::RzRyRx(-M_PI / 2, 0, 0), Point3(2, 0, 0)));
poses.insert<Pose3>(3, Pose3(Rot3(), Point3(1, -1, 0)));
auto relativeTranslations = TranslationRecovery::SimulateMeasurements(
poses, {{0, 1}, {1, 2}, {1, 3}, {3, 0}});
// Check simulated measurements.
for (auto& unitTranslation : relativeTranslations) {
EXPECT(assert_equal(GetDirectionFromPoses(poses, unitTranslation),
unitTranslation.measured()));
}
TranslationRecovery algorithm(relativeTranslations);
const auto graph = algorithm.buildGraph();
EXPECT_LONGS_EQUAL(3, graph.size());
// Run translation recovery
const auto result = algorithm.run(/*scale=*/4.0);
// Check result
EXPECT(assert_equal(Point3(0, 0, 0), result.at<Point3>(0)));
EXPECT(assert_equal(Point3(4, 0, 0), result.at<Point3>(1)));
EXPECT(assert_equal(Point3(4, 0, 0), result.at<Point3>(2)));
EXPECT(assert_equal(Point3(2, -2, 0), result.at<Point3>(3)));
}
/* ************************************************************************* */
int main() {
TestResult tr;