-
Notifications
You must be signed in to change notification settings - Fork 764
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Cal3 Base Class #626
Cal3 Base Class #626
Changes from 14 commits
04fb339
ad66a59
42b0537
17e9b73
8daa6a7
f904184
9bc63fa
04597fe
355d2cf
25b6146
f7d9710
916771c
e488dc8
ecb0af5
fc0fd1a
0a55d31
70bab8e
3ddc999
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,68 @@ | ||
/* ---------------------------------------------------------------------------- | ||
|
||
* 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 | ||
// old formula: fx_ = (double) w * tan(a); | ||
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_; | ||
varunagrawal marked this conversation as resolved.
Show resolved
Hide resolved
|
||
} 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); | ||
} | ||
|
||
/* ************************************************************************* */ | ||
|
||
} // \ namespace gtsam |
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -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)) {} | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. throw is unexpected vector length? There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Nice catch! I updated all the vector constructors to have fixed size vectors so this is caught at compile-time (aka no nasty surprises). |
||
|
||
/** | ||
* 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 | ||
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 { | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. move to cpp? |
||
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; | ||
} | ||
|
||
/// @} | ||
/// @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 |
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -23,16 +23,6 @@ | |
|
||
namespace gtsam { | ||
|
||
/* ************************************************************************* */ | ||
Cal3Bundler::Cal3Bundler() : | ||
f_(1), k1_(0), k2_(0), u0_(0), v0_(0), tol_(1e-5) { | ||
} | ||
|
||
/* ************************************************************************* */ | ||
Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0, | ||
double tol) | ||
: f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0), tol_(tol) {} | ||
|
||
/* ************************************************************************* */ | ||
Matrix3 Cal3Bundler::K() const { | ||
Matrix3 K; | ||
|
@@ -52,18 +42,23 @@ Vector3 Cal3Bundler::vector() const { | |
return Vector3(f_, 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"); | ||
} | ||
|
||
/* ************************************************************************* */ | ||
bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { | ||
if (std::abs(f_ - K.f_) > tol || std::abs(k1_ - K.k1_) > tol | ||
|| std::abs(k2_ - K.k2_) > tol || std::abs(u0_ - K.u0_) > tol | ||
|| std::abs(v0_ - K.v0_) > tol) | ||
return false; | ||
return true; | ||
return (std::fabs(f_ - K.f_) < tol && std::fabs(k1_ - K.k1_) < tol && | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. what about base? And I think f should go :-) There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Nice catch. Don't know how I missed that, then again this is a ginormous PR. |
||
std::fabs(k2_ - K.k2_) < tol && std::fabs(u0_ - K.u0_) < tol && | ||
std::fabs(v0_ - K.v0_) < tol); | ||
} | ||
|
||
/* ************************************************************************* */ | ||
|
@@ -150,14 +145,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 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Yeah I should just kill that.