76 lines
2.4 KiB
C++
76 lines
2.4 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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
|