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

76
gtsam/geometry/Cal3_S2.cpp
View File

@ -15,51 +15,61 @@
* @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) {
char buffer[200]; double a = fov * M_PI / 360.0; // fov/2 in radians
buffer[0] = 0; fx_ = (double) w * tan(a);
sprintf(buffer, "%s/calibration_info.txt", path.c_str()); fy_ = fx_;
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);
} }
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 (infile)
if (fabs(fx_ - K.fx_) > tol) return false; infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
if (fabs(fy_ - K.fy_) > tol) return false; else {
if (fabs(s_ - K.s_) > tol) return false; printf("Unable to load the calibration\n");
if (fabs(u0_ - K.u0_) > tol) return false; exit(0);
if (fabs(v0_ - K.v0_) > tol) return false; }
return true;
}
/* ************************************************************************* */ infile.close();
Point2 Cal3_S2::uncalibrate(const Point2& p, }
boost::optional<Matrix&> H1, 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 (H2) *H2 = Matrix_(2, 2, fx_, s_, 0.000, fy_);
const double x = p.x(), y = p.y(); bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); 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<Matrix&> H1,
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 (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_);
}
/* ************************************************************************* */ /* ************************************************************************* */

82
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,45 +108,47 @@ 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)); }
}
/** /**
* logmap for the calibration * logmap for the calibration
*/ */
Vector logmap(const Cal3_S2& T2) const { Vector logmap(const Cal3_S2& T2) const {
return vector() - T2.vector(); return vector() - T2.vector();
} }
private: private:
/** 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

119
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,18 +34,24 @@ 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 {
Base::print(str); return exp(-0.5 * error(c));
} }
/* ************************************************************************* */
template<class VALUES>
void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
Base::print(str);
}
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES> template<class VALUES>
Vector NonlinearFactorGraph<VALUES>::unwhitenedError(const VALUES& c) const { Vector NonlinearFactorGraph<VALUES>::unwhitenedError(const VALUES& c) const {
list<Vector> errors; list<Vector> errors;
BOOST_FOREACH(const sharedFactor& factor, this->factors_) BOOST_FOREACH(const sharedFactor& factor, this->factors_)
errors.push_back(factor->unwhitenedError(c)); errors.push_back(factor->unwhitenedError(c));
return concatVectors(errors); return concatVectors(errors);
} }
@ -54,7 +61,7 @@ void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
double total_error = 0.; double total_error = 0.;
// iterate over all the factors_ to accumulate the log probabilities // iterate over all the factors_ to accumulate the log probabilities
BOOST_FOREACH(const sharedFactor& factor, this->factors_) BOOST_FOREACH(const sharedFactor& factor, this->factors_)
total_error += factor->error(c); total_error += factor->error(c);
return total_error; return total_error;
} }
@ -64,58 +71,60 @@ void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
std::set<Symbol> NonlinearFactorGraph<VALUES>::keys() const { std::set<Symbol> NonlinearFactorGraph<VALUES>::keys() const {
std::set<Symbol> keys; std::set<Symbol> keys;
BOOST_FOREACH(const sharedFactor& factor, this->factors_) BOOST_FOREACH(const sharedFactor& factor, this->factors_)
keys.insert(factor->begin(), factor->end()); keys.insert(factor->begin(), factor->end());
return keys; return keys;
} }
/* ************************************************************************* */
template<class VALUES>
Ordering::shared_ptr NonlinearFactorGraph<VALUES>::orderingCOLAMD(
const VALUES& config) const {
/* ************************************************************************* */ // Create symbolic graph and initial (iterator) ordering
template<class VALUES> SymbolicFactorGraph::shared_ptr symbolic;
Ordering::shared_ptr NonlinearFactorGraph<VALUES>::orderingCOLAMD(const VALUES& config) const { Ordering::shared_ptr ordering;
boost::tie(symbolic, ordering) = this->symbolic(config);
// Create symbolic graph and initial (iterator) ordering // Compute the VariableIndex (column-wise index)
SymbolicFactorGraph::shared_ptr symbolic; VariableIndex variableIndex(*symbolic, ordering->size());
Ordering::shared_ptr ordering; if (config.size() != variableIndex.size()) throw std::runtime_error(
boost::tie(symbolic,ordering) = this->symbolic(config); "orderingCOLAMD: some variables in the graph are not constrained!");
// Compute the VariableIndex (column-wise index) // Compute a fill-reducing ordering with COLAMD
VariableIndex variableIndex(*symbolic, ordering->size()); Permutation::shared_ptr colamdPerm(Inference::PermutationCOLAMD(
if(config.size() != variableIndex.size()) variableIndex));
throw std::runtime_error("orderingCOLAMD: some variables in the graph are not constrained!");
// Compute a fill-reducing ordering with COLAMD // Permute the Ordering and VariableIndex with the COLAMD ordering
Permutation::shared_ptr colamdPerm(Inference::PermutationCOLAMD(variableIndex)); ordering->permuteWithInverse(*colamdPerm->inverse());
// variableIndex.permute(*colamdPerm);
// SL-FIX: fix permutation
// Permute the Ordering and VariableIndex with the COLAMD ordering // Return the Ordering and VariableIndex to be re-used during linearization
ordering->permuteWithInverse(*colamdPerm->inverse()); // and elimination
// variableIndex.permute(*colamdPerm); return ordering;
// SL-FIX: fix permutation
// Return the Ordering and VariableIndex to be re-used during linearization
// and elimination
return ordering;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class VALUES>
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph<VALUES>::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<class VALUES> template<class VALUES>
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> SymbolicFactorGraph::shared_ptr NonlinearFactorGraph<VALUES>::symbolic(
NonlinearFactorGraph<VALUES>::symbolic(const VALUES& config) const { const VALUES& config, const Ordering& ordering) const {
// Generate an initial key ordering in iterator order // Generate the symbolic factor graph
Ordering::shared_ptr ordering(config.orderingArbitrary()); SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
return make_pair(symbolic(config, *ordering), ordering); symbolicfg->reserve(this->size());
BOOST_FOREACH(const sharedFactor& factor, this->factors_)
symbolicfg->push_back(factor->symbolic(ordering));
return symbolicfg;
}
/* ************************************************************************* */
template<class VALUES>
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> 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<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); {
if (lf) linearFG->push_back(lf); JacobianFactor::shared_ptr lf = factor->linearize(config, ordering);
} if (lf) linearFG->push_back(lf);
}
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) {