diff --git a/gtsam/geometry/Cal3.cpp b/gtsam/geometry/Cal3.cpp index 31e9bf834..6183c6e5e 100644 --- a/gtsam/geometry/Cal3.cpp +++ b/gtsam/geometry/Cal3.cpp @@ -22,28 +22,25 @@ #include 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); } /* ************************************************************************* */