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

Cal3 code improvements

release/4.3a0
Varun Agrawal 2020-12-02 06:11:02 -05:00
parent 04597feaa4
commit 355d2cff06
1 changed files with 11 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
gtsam/geometry/Cal3.cpp
View File

@ -22,28 +22,25 @@
#include <iostream>
namespace gtsam {
using namespace std;
/* ************************************************************************* */
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)); // old formula: fx_ = (double) w * tan(a);
// old formula: fx_ = (double) w * tan(a);
fx_ = double(w) / (2.0 * tan(a));
fy_ = fx_;
}
/* ************************************************************************* */
Cal3::Cal3(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());
const auto buffer = path + std::string("/calibration_info.txt");
std::ifstream infile(buffer, std::ios::in);
if (infile)
if (infile) {
infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
else {
} else {
throw std::runtime_error("Cal3: Unable to load the calibration");
}
@ -51,9 +48,9 @@ Cal3::Cal3(const std::string& path)
}
/* ************************************************************************* */
ostream& operator<<(ostream& os, const Cal3& 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& cal) {
os << "{ fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
<< ", px: " << cal.px() << ", py: " << cal.py() << " }";
return os;
}
@ -62,9 +59,9 @@ 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);
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);
}
/* ************************************************************************* */