From 5fd04188e46a9453e9b4495844bc0081789fc2b0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 21 Mar 2011 15:05:11 +0000 Subject: [PATCH] cmath rather than math.h header, in implementation file only --- gtsam/geometry/Cal3_S2.cpp | 76 +++++++------ gtsam/geometry/Cal3_S2.h | 82 +++++++------- gtsam/nonlinear/NonlinearFactorGraph-inl.h | 119 +++++++++++---------- gtsam/nonlinear/NonlinearFactorGraph.h | 10 +- 4 files changed, 155 insertions(+), 132 deletions(-) diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index 8695c1b3b..a2807587f 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -15,51 +15,61 @@ * @author Frank Dellaert */ +#include #include #include #include namespace gtsam { -using namespace std; + using namespace std; -/* ************************************************************************* */ -Cal3_S2::Cal3_S2(const std::string &path) { - - char buffer[200]; - buffer[0] = 0; - sprintf(buffer, "%s/calibration_info.txt", path.c_str()); - std::ifstream infile(buffer, std::ios::in); - - if (infile) - infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_; - else { - printf("Unable to load the calibration\n"); - exit(0); + /* ************************************************************************* */ + Cal3_S2::Cal3_S2(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 * tan(a); + fy_ = fx_; } - infile.close(); -} + /* ************************************************************************* */ + Cal3_S2::Cal3_S2(const std::string &path) { -/* ************************************************************************* */ + char buffer[200]; + buffer[0] = 0; + sprintf(buffer, "%s/calibration_info.txt", path.c_str()); + std::ifstream infile(buffer, std::ios::in); -bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { - if (fabs(fx_ - K.fx_) > tol) return false; - if (fabs(fy_ - K.fy_) > tol) return false; - if (fabs(s_ - K.s_) > tol) return false; - if (fabs(u0_ - K.u0_) > tol) return false; - if (fabs(v0_ - K.v0_) > tol) return false; - return true; -} + if (infile) + infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_; + else { + printf("Unable to load the calibration\n"); + exit(0); + } -/* ************************************************************************* */ -Point2 Cal3_S2::uncalibrate(const Point2& p, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = Matrix_(2, 5, p.x(), 000.0, p.y(), 1.0, 0.0, 0.000, p.y(), 0.000, 0.0,1.0); - if (H2) *H2 = Matrix_(2, 2, fx_, s_, 0.000, fy_); - const double x = p.x(), y = p.y(); - return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); -} + infile.close(); + } + + /* ************************************************************************* */ + + bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { + if (fabs(fx_ - K.fx_) > tol) return false; + if (fabs(fy_ - K.fy_) > tol) return false; + if (fabs(s_ - K.s_) > tol) return false; + if (fabs(u0_ - K.u0_) > tol) return false; + if (fabs(v0_ - K.v0_) > tol) return false; + return true; + } + + /* ************************************************************************* */ + Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional H1, + boost::optional H2) const { + if (H1) *H1 = Matrix_(2, 5, p.x(), 000.0, p.y(), 1.0, 0.0, 0.000, p.y(), + 0.000, 0.0, 1.0); + if (H2) *H2 = Matrix_(2, 2, fx_, s_, 0.000, fy_); + const double x = p.x(), y = p.y(); + return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); + } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index f471acb44..dc654cbda 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -19,7 +19,6 @@ #include #include -#include namespace gtsam { @@ -49,12 +48,7 @@ namespace gtsam { * @param w image width * @param h image height */ - Cal3_S2(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*tan(a); - fy_=fx_; - } + Cal3_S2(double fov, int w, int h); void print(const std::string& s = "") const { gtsam::print(matrix(), s); @@ -70,17 +64,27 @@ namespace gtsam { */ Cal3_S2(const std::string &path); - inline double fx() const { return fx_; } - inline double fy() const { return fy_; } - inline double skew() const { return s_; } - inline double px() const { return u0_; } - inline double py() const { return v0_; } + inline double fx() const { + return fx_; + } + inline double fy() const { + return fy_; + } + inline double skew() const { + return s_; + } + inline double px() const { + return u0_; + } + inline double py() const { + return v0_; + } /** * return the principal point */ Point2 principalPoint() const { - return Point2(u0_,v0_); + return Point2(u0_, v0_); } /** @@ -104,45 +108,47 @@ namespace gtsam { * convert intrinsic coordinates xy to image coordinates uv * with optional derivatives */ - Point2 uncalibrate(const Point2& p, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Point2 uncalibrate(const Point2& p, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) const; /** * convert image coordinates uv to intrinsic coordinates xy */ Point2 calibrate(const Point2& p) const { const double u = p.x(), v = p.y(); - return Point2((1/fx_)*(u-u0_ - (s_/fy_)*(v-v0_)), (1/fy_)*(v-v0_)); + return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)), (1 / fy_) + * (v - v0_)); } - /** - * return DOF, dimensionality of tangent space - */ - inline size_t dim() const { return 5; } - static size_t Dim() { return 5; } + /** + * return DOF, dimensionality of tangent space + */ + inline size_t dim() const { + return 5; + } + static size_t Dim() { + return 5; + } - /** - * Given 5-dim tangent vector, create new calibration - */ - inline Cal3_S2 expmap(const Vector& d) const { - return Cal3_S2(fx_ + d(0), fy_ + d(1), - s_ + d(2), u0_ + d(3), v0_ + d(4)); - } + /** + * Given 5-dim tangent vector, create new calibration + */ + inline Cal3_S2 expmap(const Vector& d) const { + return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4)); + } - /** - * logmap for the calibration - */ - Vector logmap(const Cal3_S2& T2) const { - return vector() - T2.vector(); - } + /** + * logmap for the calibration + */ + Vector logmap(const Cal3_S2& T2) const { + return vector() - T2.vector(); + } private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(Archive & ar, const unsigned int version) - { + void serialize(Archive & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(s_); @@ -153,4 +159,4 @@ namespace gtsam { typedef boost::shared_ptr shared_ptrK; -} +} // gtsam diff --git a/gtsam/nonlinear/NonlinearFactorGraph-inl.h b/gtsam/nonlinear/NonlinearFactorGraph-inl.h index 8d4730b0e..a968e1327 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph-inl.h +++ b/gtsam/nonlinear/NonlinearFactorGraph-inl.h @@ -19,6 +19,7 @@ #pragma once +#include #include #include #include @@ -33,18 +34,24 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ -template -void NonlinearFactorGraph::print(const std::string& str) const { - Base::print(str); -} + /* ************************************************************************* */ + template + double NonlinearFactorGraph::probPrime(const VALUES& c) const { + return exp(-0.5 * error(c)); + } + + /* ************************************************************************* */ + template + void NonlinearFactorGraph::print(const std::string& str) const { + Base::print(str); + } /* ************************************************************************* */ template Vector NonlinearFactorGraph::unwhitenedError(const VALUES& c) const { list errors; BOOST_FOREACH(const sharedFactor& factor, this->factors_) - errors.push_back(factor->unwhitenedError(c)); + errors.push_back(factor->unwhitenedError(c)); return concatVectors(errors); } @@ -54,7 +61,7 @@ void NonlinearFactorGraph::print(const std::string& str) const { double total_error = 0.; // iterate over all the factors_ to accumulate the log probabilities BOOST_FOREACH(const sharedFactor& factor, this->factors_) - total_error += factor->error(c); + total_error += factor->error(c); return total_error; } @@ -64,58 +71,60 @@ void NonlinearFactorGraph::print(const std::string& str) const { std::set NonlinearFactorGraph::keys() const { std::set keys; BOOST_FOREACH(const sharedFactor& factor, this->factors_) - keys.insert(factor->begin(), factor->end()); + keys.insert(factor->begin(), factor->end()); return keys; } + /* ************************************************************************* */ + template + Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD( + const VALUES& config) const { - /* ************************************************************************* */ - template - Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(const VALUES& config) const { + // Create symbolic graph and initial (iterator) ordering + SymbolicFactorGraph::shared_ptr symbolic; + Ordering::shared_ptr ordering; + boost::tie(symbolic, ordering) = this->symbolic(config); - // Create symbolic graph and initial (iterator) ordering - SymbolicFactorGraph::shared_ptr symbolic; - Ordering::shared_ptr ordering; - boost::tie(symbolic,ordering) = this->symbolic(config); + // Compute the VariableIndex (column-wise index) + VariableIndex variableIndex(*symbolic, ordering->size()); + if (config.size() != variableIndex.size()) throw std::runtime_error( + "orderingCOLAMD: some variables in the graph are not constrained!"); - // Compute the VariableIndex (column-wise index) - VariableIndex variableIndex(*symbolic, ordering->size()); - if(config.size() != variableIndex.size()) - throw std::runtime_error("orderingCOLAMD: some variables in the graph are not constrained!"); + // Compute a fill-reducing ordering with COLAMD + Permutation::shared_ptr colamdPerm(Inference::PermutationCOLAMD( + variableIndex)); - // Compute a fill-reducing ordering with COLAMD - Permutation::shared_ptr colamdPerm(Inference::PermutationCOLAMD(variableIndex)); + // Permute the Ordering and VariableIndex with the COLAMD ordering + ordering->permuteWithInverse(*colamdPerm->inverse()); + // variableIndex.permute(*colamdPerm); + // SL-FIX: fix permutation - // Permute the Ordering and VariableIndex with the COLAMD ordering - ordering->permuteWithInverse(*colamdPerm->inverse()); - // variableIndex.permute(*colamdPerm); - // SL-FIX: fix permutation - - // Return the Ordering and VariableIndex to be re-used during linearization - // and elimination - return ordering; + // Return the Ordering and VariableIndex to be re-used during linearization + // and elimination + return ordering; } - /* ************************************************************************* */ - template - SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic( - const VALUES& config, const Ordering& ordering) const { - // Generate the symbolic factor graph - SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph); - symbolicfg->reserve(this->size()); - BOOST_FOREACH(const sharedFactor& factor, this->factors_) { - symbolicfg->push_back(factor->symbolic(ordering)); - } - return symbolicfg; - } - - /* ************************************************************************* */ + /* ************************************************************************* */ template - pair - NonlinearFactorGraph::symbolic(const VALUES& config) const { - // Generate an initial key ordering in iterator order - Ordering::shared_ptr ordering(config.orderingArbitrary()); - return make_pair(symbolic(config, *ordering), ordering); + SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic( + const VALUES& config, const Ordering& ordering) const { + // Generate the symbolic factor graph + SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph); + symbolicfg->reserve(this->size()); + + BOOST_FOREACH(const sharedFactor& factor, this->factors_) + symbolicfg->push_back(factor->symbolic(ordering)); + + return symbolicfg; + } + + /* ************************************************************************* */ + template + pair NonlinearFactorGraph< + VALUES>::symbolic(const VALUES& config) const { + // Generate an initial key ordering in iterator order + Ordering::shared_ptr ordering(config.orderingArbitrary()); + return make_pair(symbolic(config, *ordering), ordering); } /* ************************************************************************* */ @@ -124,18 +133,20 @@ void NonlinearFactorGraph::print(const std::string& str) const { const VALUES& config, const Ordering& ordering) const { // create an empty linear FG - typename FactorGraph::shared_ptr linearFG(new FactorGraph); + typename FactorGraph::shared_ptr linearFG(new FactorGraph< + JacobianFactor> ); linearFG->reserve(this->size()); // linearize all factors - BOOST_FOREACH(const sharedFactor& factor, this->factors_) { - JacobianFactor::shared_ptr lf = factor->linearize(config, ordering); - if (lf) linearFG->push_back(lf); - } + BOOST_FOREACH(const sharedFactor& factor, this->factors_) + { + JacobianFactor::shared_ptr lf = factor->linearize(config, ordering); + if (lf) linearFG->push_back(lf); + } return linearFG; } - /* ************************************************************************* */ +/* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 27255430f..e3f3f2eea 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -21,11 +21,9 @@ #pragma once -#include - -#include -#include #include +#include +#include #include namespace gtsam { @@ -60,9 +58,7 @@ namespace gtsam { Vector unwhitenedError(const VALUES& c) const; /** Unnormalized probability. O(n) */ - double probPrime(const VALUES& c) const { - return exp(-0.5 * error(c)); - } + double probPrime(const VALUES& c) const; template void add(const F& factor) {