cmath rather than math.h header, in implementation file only
Frank Dellaert
parent
a923c8f1b3
commit
5fd04188e4
|
|
@ -15,51 +15,61 @@
|
|||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
|
||||
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<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_);
|
||||
}
|
||||
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<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_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -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,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<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; }
|
||||
/**
|
||||
* 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<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
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
|
@ -33,18 +34,24 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES>
|
||||
void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
|
||||
Base::print(str);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
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 {
|
||||
Base::print(str);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES>
|
||||
Vector NonlinearFactorGraph<VALUES>::unwhitenedError(const VALUES& c) const {
|
||||
list<Vector> 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<VALUES>::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<VALUES>::print(const std::string& str) const {
|
|||
std::set<Symbol> NonlinearFactorGraph<VALUES>::keys() const {
|
||||
std::set<Symbol> keys;
|
||||
BOOST_FOREACH(const sharedFactor& factor, this->factors_)
|
||||
keys.insert(factor->begin(), factor->end());
|
||||
keys.insert(factor->begin(), factor->end());
|
||||
return keys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES>
|
||||
Ordering::shared_ptr NonlinearFactorGraph<VALUES>::orderingCOLAMD(
|
||||
const VALUES& config) const {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class VALUES>
|
||||
Ordering::shared_ptr NonlinearFactorGraph<VALUES>::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<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>
|
||||
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);
|
||||
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>
|
||||
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 {
|
||||
|
||||
// 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_) {
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue