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

16
gtsam/geometry/Cal3_S2.cpp
View File

@ -15,6 +15,7 @@
* @author Frank Dellaert
*/
#include <cmath>
#include <fstream>
#include <iostream>
@ -23,6 +24,14 @@
namespace gtsam {
using namespace std;
/* ************************************************************************* */
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) {
@ -53,9 +62,10 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
}
/* ************************************************************************* */
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);
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_);

52
gtsam/geometry/Cal3_S2.h
View File

@ -19,7 +19,6 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
#include <math.h>
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,11 +64,21 @@ 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
@ -104,30 +108,33 @@ namespace gtsam {
* convert intrinsic coordinates xy to image coordinates uv
* with optional derivatives
*/
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> 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; }
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));
return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4));
}
/**
@ -141,8 +148,7 @@ namespace gtsam {
/** Serialization function */
friend class boost::serialization::access;
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(fy_);
ar & BOOST_SERIALIZATION_NVP(s_);
@ -153,4 +159,4 @@ namespace gtsam {
typedef boost::shared_ptr<Cal3_S2> shared_ptrK;
}
} // gtsam

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

@ -19,6 +19,7 @@
#pragma once
#include <cmath>
#include <boost/foreach.hpp>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -33,6 +34,12 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
template<class VALUES>
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 {
@ -68,10 +75,10 @@ void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
return keys;
}
/* ************************************************************************* */
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
SymbolicFactorGraph::shared_ptr symbolic;
@ -80,11 +87,12 @@ void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
// 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!");
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));
Permutation::shared_ptr colamdPerm(Inference::PermutationCOLAMD(
variableIndex));
// Permute the Ordering and VariableIndex with the COLAMD ordering
ordering->permuteWithInverse(*colamdPerm->inverse());
@ -103,16 +111,17 @@ void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
// Generate the symbolic factor graph
SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
symbolicfg->reserve(this->size());
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
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 {
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,11 +133,13 @@ void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
const VALUES& config, const Ordering& ordering) const {
// 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());
// 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);
}

10
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -21,11 +21,9 @@
#pragma once
#include <math.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Ordering.h>
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<class F>
void add(const F& factor) {