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

Fixes for Windows. Everything compiles and passes again.

release/4.3a0
Chris Beall 2014-05-28 21:29:23 -04:00
parent 4157648d0c
commit 432fce817e
3 changed files with 14 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/nonlinear/LagoInitializer.cpp
View File

@ -18,6 +18,7 @@
#include <gtsam/nonlinear/LagoInitializer.h>
#include <gtsam/slam/dataset.h>
#include <boost/math/special_functions.hpp>
namespace gtsam {
@ -152,7 +153,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const std::vector<size_t>& spann
double key1_DeltaTheta_key2 = deltaTheta(0);
///std::cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << std::endl;
double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1) - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord
double k = round(k2pi_noise/(2*M_PI));
double k = boost::math::round(k2pi_noise/(2*M_PI));
//if (k2pi_noise - 2*k*M_PI > 1e-5) std::cout << k2pi_noise - 2*k*M_PI << std::endl; // for debug
Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2*k*M_PI);
lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta));

20
gtsam/nonlinear/LagoInitializer.h
View File

@ -56,13 +56,13 @@ noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Varianc
* summing up the (directed) rotation measurements. Relative measurements are encoded in "deltaThetaMap"
* The root is assumed to have orientation zero.
*/
double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree,
GTSAM_EXPORT double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree,
const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap);
/* This function computes the cumulative orientations (without wrapping)
* for all node wrt the root (root has zero orientation)
*/
key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
GTSAM_EXPORT key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
const PredecessorMap<Key>& tree);
/* Given a factor graph "g", and a spanning tree "tree", the function selects the nodes belonging to the tree and to g,
@ -70,31 +70,31 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
* Also it computes deltaThetaMap which is a fast way to encode relative orientations along the tree:
* for a node key2, s.t. tree[key2]=key1, the values deltaThetaMap[key2] is the relative orientation theta[key2]-theta[key1]
*/
void getSymbolicGraph(
GTSAM_EXPORT void getSymbolicGraph(
/*OUTPUTS*/ std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds, key2doubleMap& deltaThetaMap,
/*INPUTS*/ const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g);
/* Retrieves the deltaTheta and the corresponding noise model from a BetweenFactor<Pose2> */
void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
GTSAM_EXPORT void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta);
/* Linear factor graph with regularized orientation measurements */
GaussianFactorGraph buildLinearOrientationGraph(const std::vector<size_t>& spanningTreeIds, const std::vector<size_t>& chordsIds,
GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const std::vector<size_t>& spanningTreeIds, const std::vector<size_t>& chordsIds,
const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
/* Selects the subgraph of betweenFactors and transforms priors into between wrt a fictitious node */
NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph);
GTSAM_EXPORT NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph);
/* Returns the orientations of a graph including only BetweenFactors<Pose2> */
VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true);
GTSAM_EXPORT VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true);
/* LAGO: Returns the orientations of the Pose2 in a generic factor graph */
VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true);
GTSAM_EXPORT VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true);
/* Returns the values for the Pose2 in a generic factor graph */
Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true);
GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true);
/* Only corrects the orientation part in initialGuess */
Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess);
GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess);
} // end of namespace gtsam

4
gtsam/slam/dataset.h
View File

@ -125,7 +125,7 @@ GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data);
* @return initial Values containing the initial guess (VERTEX_SE2)
*/
enum kernelFunctionType { QUADRATIC, HUBER, TUKEY };
bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction = QUADRATIC);
GTSAM_EXPORT bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction = QUADRATIC);
/**
* @brief This function writes a g2o file from
@ -134,7 +134,7 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in
* @param graph NonlinearFactor graph storing the measurements (EDGE_SE2)
* @return estimate Values containing the values (VERTEX_SE2)
*/
bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate);
GTSAM_EXPORT bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate);
/**
* @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a