From efd966b45adc9bd2c9a49b436889cb0010b440ef Mon Sep 17 00:00:00 2001 From: chrisbeall Date: Tue, 1 Nov 2016 15:11:57 -0400 Subject: [PATCH] Move print methods to cpp files wherever possible --- gtsam/base/GenericValue.h | 3 +-- gtsam/geometry/Cal3_S2Stereo.cpp | 39 +++++++++++++++++++++++++++++++ gtsam/geometry/Cal3_S2Stereo.h | 12 +++------- gtsam/geometry/Quaternion.h | 2 +- gtsam/geometry/SO3.cpp | 7 ++++++ gtsam/geometry/SO3.h | 6 ++--- gtsam_unstable/geometry/Event.cpp | 14 +++++++++++ gtsam_unstable/geometry/Event.h | 11 +++------ 8 files changed, 70 insertions(+), 24 deletions(-) create mode 100644 gtsam/geometry/Cal3_S2Stereo.cpp diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 7c4d0398d..5b59f4872 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -26,9 +26,8 @@ #include #include -#include -#include // operator typeid #include +#include // operator typeid namespace gtsam { diff --git a/gtsam/geometry/Cal3_S2Stereo.cpp b/gtsam/geometry/Cal3_S2Stereo.cpp new file mode 100644 index 000000000..22966ee37 --- /dev/null +++ b/gtsam/geometry/Cal3_S2Stereo.cpp @@ -0,0 +1,39 @@ +/* ---------------------------------------------------------------------------- + + * 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_S2Stereo.cpp + * @brief The most common 5DOF 3D->2D calibration + Stereo baseline + * @author Chris Beall + */ + +#include + +#include + +namespace gtsam { +using namespace std; + +/* ************************************************************************* */ +void Cal3_S2Stereo::print(const std::string& s) const { + K_.print(s+"K: "); + std::cout << s << "Baseline: " << b_ << std::endl; + } + +/* ************************************************************************* */ +bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const { + if (fabs(b_ - other.b_) > tol) return false; + return K_.equals(other.K_,tol); +} + +/* ************************************************************************* */ + +} // namespace gtsam diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 29ccd194d..db49448ec 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -18,7 +18,7 @@ #pragma once #include -#include +#include namespace gtsam { @@ -63,16 +63,10 @@ namespace gtsam { /// @name Testable /// @{ - void print(const std::string& s = "") const { - K_.print(s+"K: "); - std::cout << s << "Baseline: " << b_ << std::endl; - } + void print(const std::string& s = "") const; /// Check if equal up to specified tolerance - bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const { - if (fabs(b_ - other.b_) > tol) return false; - return K_.equals(other.K_,tol); - } + bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const; /// @} /// @name Standard Interface diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 4f5495263..af9481181 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -21,7 +21,7 @@ #include #include // Logmap/Expmap derivatives #include -#include +#include #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index bd111d9b1..07c03ab49 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace gtsam { @@ -116,6 +117,12 @@ SO3 SO3::AxisAngle(const Vector3& axis, double theta) { return so3::ExpmapFunctor(axis, theta).expmap(); } +/* ************************************************************************* */ +void SO3::print(const std::string& s) const { + std::cout << s << *this << std::endl; + } + +/* ************************************************************************* */ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { if (H) { so3::DexpFunctor impl(omega); diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 0396fbce0..53f2c2130 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -24,7 +24,7 @@ #include #include -#include +#include namespace gtsam { @@ -68,9 +68,7 @@ public: /// @name Testable /// @{ - void print(const std::string& s) const { - std::cout << s << *this << std::endl; - } + void print(const std::string& s) const; bool equals(const SO3 & R, double tol) const { return equal_with_abs_tol(*this, R, tol); diff --git a/gtsam_unstable/geometry/Event.cpp b/gtsam_unstable/geometry/Event.cpp index dda2c32e6..c503514a6 100644 --- a/gtsam_unstable/geometry/Event.cpp +++ b/gtsam_unstable/geometry/Event.cpp @@ -18,7 +18,21 @@ */ #include +#include namespace gtsam { +/* ************************************************************************* */ +void Event::print(const std::string& s) const { + std::cout << s << "time = " << time_ << "location = " << location_.transpose(); +} + +/* ************************************************************************* */ +bool Event::equals(const Event& other, double tol) const { + return std::abs(time_ - other.time_) < tol + && traits::Equals(location_, other.location_, tol); +} + +/* ************************************************************************* */ + } //\ namespace gtsam diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h index 5bd37d2d2..d9bacd106 100644 --- a/gtsam_unstable/geometry/Event.h +++ b/gtsam_unstable/geometry/Event.h @@ -21,7 +21,7 @@ #include #include -#include +#include namespace gtsam { @@ -60,15 +60,10 @@ public: } /** print with optional string */ - void print(const std::string& s = "") const { - std::cout << s << "time = " << time_ << "location = " << location_.transpose(); - } + void print(const std::string& s = "") const; /** equals with an tolerance */ - bool equals(const Event& other, double tol = 1e-9) const { - return std::abs(time_ - other.time_) < tol - && traits::Equals(location_, other.location_, tol); - } + bool equals(const Event& other, double tol = 1e-9) const; /// Updates a with tangent space delta inline Event retract(const Vector4& v) const {