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

cmath rather than math.h header, in implementation file only

release/4.3a0
Frank Dellaert 2011-03-21 15:05:11 +00:00
parent a923c8f1b3
commit 5fd04188e4
4 changed files with 155 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
gtsam/geometry/Cal3_S2.cpp
View File

@ -15,16 +15,25 @@
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <cmath>
#include <fstream> #include <fstream>
#include <iostream> #include <iostream>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
namespace gtsam { namespace gtsam {
using namespace std; using namespace std;
/* ************************************************************************* */ /* ************************************************************************* */
Cal3_S2::Cal3_S2(const std::string &path) { 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_;
}
/* ************************************************************************* */
Cal3_S2::Cal3_S2(const std::string &path) {
char buffer[200]; char buffer[200];
buffer[0] = 0; buffer[0] = 0;
@ -39,27 +48,28 @@ Cal3_S2::Cal3_S2(const std::string &path) {
} }
infile.close(); infile.close();
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
if (fabs(fx_ - K.fx_) > tol) return false; if (fabs(fx_ - K.fx_) > tol) return false;
if (fabs(fy_ - K.fy_) > tol) return false; if (fabs(fy_ - K.fy_) > tol) return false;
if (fabs(s_ - K.s_) > tol) return false; if (fabs(s_ - K.s_) > tol) return false;
if (fabs(u0_ - K.u0_) > tol) return false; if (fabs(u0_ - K.u0_) > tol) return false;
if (fabs(v0_ - K.v0_) > tol) return false; if (fabs(v0_ - K.v0_) > tol) return false;
return true; return true;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Cal3_S2::uncalibrate(const Point2& p, Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> 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 (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_); if (H2) *H2 = Matrix_(2, 2, fx_, s_, 0.000, fy_);
const double x = p.x(), y = p.y(); const double x = p.x(), y = p.y();
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
} }
/* ************************************************************************* */ /* ************************************************************************* */

54
gtsam/geometry/Cal3_S2.h
View File

@ -19,7 +19,6 @@
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <math.h>
namespace gtsam { namespace gtsam {
@ -49,12 +48,7 @@ namespace gtsam {
* @param w image width * @param w image width
* @param h image height * @param h image height
*/ */
Cal3_S2(double fov, int w, int h) : 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_;
}
void print(const std::string& s = "") const { void print(const std::string& s = "") const {
gtsam::print(matrix(), s); gtsam::print(matrix(), s);
@ -70,17 +64,27 @@ namespace gtsam {
*/ */
Cal3_S2(const std::string &path); Cal3_S2(const std::string &path);
inline double fx() const { return fx_; } inline double fx() const {
inline double fy() const { return fy_; } return fx_;
inline double skew() const { return s_; } }
inline double px() const { return u0_; } inline double fy() const {
inline double py() const { return v0_; } return fy_;
}
inline double skew() const {
return s_;
}
inline double px() const {
return u0_;
}
inline double py() const {
return v0_;
}
/** /**
* return the principal point * return the principal point
*/ */
Point2 principalPoint() const { Point2 principalPoint() const {
return Point2(u0_,v0_); return Point2(u0_, v0_);
} }
/** /**
@ -104,30 +108,33 @@ namespace gtsam {
* convert intrinsic coordinates xy to image coordinates uv * convert intrinsic coordinates xy to image coordinates uv
* with optional derivatives * with optional derivatives
*/ */
Point2 uncalibrate(const Point2& p, Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> H1 =
boost::optional<Matrix&> H1 = boost::none, boost::none, boost::optional<Matrix&> H2 = boost::none) const;
boost::optional<Matrix&> H2 = boost::none) const;
/** /**
* convert image coordinates uv to intrinsic coordinates xy * convert image coordinates uv to intrinsic coordinates xy
*/ */
Point2 calibrate(const Point2& p) const { Point2 calibrate(const Point2& p) const {
const double u = p.x(), v = p.y(); 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 * return DOF, dimensionality of tangent space
*/ */
inline size_t dim() const { return 5; } inline size_t dim() const {
static size_t Dim() { return 5; } return 5;
}
static size_t Dim() {
return 5;
}
/** /**
* Given 5-dim tangent vector, create new calibration * Given 5-dim tangent vector, create new calibration
*/ */
inline Cal3_S2 expmap(const Vector& d) const { inline Cal3_S2 expmap(const Vector& d) const {
return Cal3_S2(fx_ + d(0), fy_ + d(1), return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4));
s_ + d(2), u0_ + d(3), v0_ + d(4));
} }
/** /**
@ -141,8 +148,7 @@ namespace gtsam {
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
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(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_); ar & BOOST_SERIALIZATION_NVP(s_);
@ -153,4 +159,4 @@ namespace gtsam {
typedef boost::shared_ptr<Cal3_S2> shared_ptrK; typedef boost::shared_ptr<Cal3_S2> shared_ptrK;
} } // gtsam

45
gtsam/nonlinear/NonlinearFactorGraph-inl.h
View File

@ -19,6 +19,7 @@
#pragma once #pragma once
#include <cmath>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -33,11 +34,17 @@ using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES> template<class VALUES>
void NonlinearFactorGraph<VALUES>::print(const std::string& str) const { double NonlinearFactorGraph<VALUES>::probPrime(const VALUES& c) const {
return exp(-0.5 * error(c));
}
/* ************************************************************************* */
template<class VALUES>
void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
Base::print(str); Base::print(str);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES> template<class VALUES>
@ -68,23 +75,24 @@ void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
return keys; return keys;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES> template<class VALUES>
Ordering::shared_ptr NonlinearFactorGraph<VALUES>::orderingCOLAMD(const VALUES& config) const { Ordering::shared_ptr NonlinearFactorGraph<VALUES>::orderingCOLAMD(
const VALUES& config) const {
// Create symbolic graph and initial (iterator) ordering // Create symbolic graph and initial (iterator) ordering
SymbolicFactorGraph::shared_ptr symbolic; SymbolicFactorGraph::shared_ptr symbolic;
Ordering::shared_ptr ordering; Ordering::shared_ptr ordering;
boost::tie(symbolic,ordering) = this->symbolic(config); boost::tie(symbolic, ordering) = this->symbolic(config);
// Compute the VariableIndex (column-wise index) // Compute the VariableIndex (column-wise index)
VariableIndex variableIndex(*symbolic, ordering->size()); VariableIndex variableIndex(*symbolic, ordering->size());
if(config.size() != variableIndex.size()) if (config.size() != variableIndex.size()) throw std::runtime_error(
throw std::runtime_error("orderingCOLAMD: some variables in the graph are not constrained!"); "orderingCOLAMD: some variables in the graph are not constrained!");
// Compute a fill-reducing ordering with COLAMD // Compute a fill-reducing ordering with COLAMD
Permutation::shared_ptr colamdPerm(Inference::PermutationCOLAMD(variableIndex)); Permutation::shared_ptr colamdPerm(Inference::PermutationCOLAMD(
variableIndex));
// Permute the Ordering and VariableIndex with the COLAMD ordering // Permute the Ordering and VariableIndex with the COLAMD ordering
ordering->permuteWithInverse(*colamdPerm->inverse()); ordering->permuteWithInverse(*colamdPerm->inverse());
@ -103,16 +111,17 @@ void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
// Generate the symbolic factor graph // Generate the symbolic factor graph
SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph); SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
symbolicfg->reserve(this->size()); symbolicfg->reserve(this->size());
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
BOOST_FOREACH(const sharedFactor& factor, this->factors_)
symbolicfg->push_back(factor->symbolic(ordering)); symbolicfg->push_back(factor->symbolic(ordering));
}
return symbolicfg; return symbolicfg;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES> template<class VALUES>
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph<
NonlinearFactorGraph<VALUES>::symbolic(const VALUES& config) const { VALUES>::symbolic(const VALUES& config) const {
// Generate an initial key ordering in iterator order // Generate an initial key ordering in iterator order
Ordering::shared_ptr ordering(config.orderingArbitrary()); Ordering::shared_ptr ordering(config.orderingArbitrary());
return make_pair(symbolic(config, *ordering), ordering); return make_pair(symbolic(config, *ordering), ordering);
@ -124,11 +133,13 @@ void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
const VALUES& config, const Ordering& ordering) const { const VALUES& config, const Ordering& ordering) const {
// create an empty linear FG // create an empty linear FG
typename FactorGraph<JacobianFactor>::shared_ptr linearFG(new FactorGraph<JacobianFactor>); typename FactorGraph<JacobianFactor>::shared_ptr linearFG(new FactorGraph<
JacobianFactor> );
linearFG->reserve(this->size()); linearFG->reserve(this->size());
// linearize all factors // linearize all factors
BOOST_FOREACH(const sharedFactor& factor, this->factors_) { BOOST_FOREACH(const sharedFactor& factor, this->factors_)
{
JacobianFactor::shared_ptr lf = factor->linearize(config, ordering); JacobianFactor::shared_ptr lf = factor->linearize(config, ordering);
if (lf) linearFG->push_back(lf); if (lf) linearFG->push_back(lf);
} }
@ -136,6 +147,6 @@ void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
return linearFG; return linearFG;
} }
/* ************************************************************************* */ /* ************************************************************************* */
} // namespace gtsam } // namespace gtsam

10
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -21,11 +21,9 @@
#pragma once #pragma once
#include <math.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/SymbolicFactorGraph.h> #include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Ordering.h> #include <gtsam/nonlinear/Ordering.h>
namespace gtsam { namespace gtsam {
@ -60,9 +58,7 @@ namespace gtsam {
Vector unwhitenedError(const VALUES& c) const; Vector unwhitenedError(const VALUES& c) const;
/** Unnormalized probability. O(n) */ /** Unnormalized probability. O(n) */
double probPrime(const VALUES& c) const { double probPrime(const VALUES& c) const;
return exp(-0.5 * error(c));
}
template<class F> template<class F>
void add(const F& factor) { void add(const F& factor) {