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/nonlinear/LagoInitializer.h>
#include <gtsam/slam/dataset.h> #include <gtsam/slam/dataset.h>
#include <boost/math/special_functions.hpp>
namespace gtsam { namespace gtsam {
@ -152,7 +153,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const std::vector<size_t>& spann
double key1_DeltaTheta_key2 = deltaTheta(0); double key1_DeltaTheta_key2 = deltaTheta(0);
///std::cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << std::endl; ///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 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 //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); Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2*k*M_PI);
lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta)); 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" * summing up the (directed) rotation measurements. Relative measurements are encoded in "deltaThetaMap"
* The root is assumed to have orientation zero. * 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); const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap);
/* This function computes the cumulative orientations (without wrapping) /* This function computes the cumulative orientations (without wrapping)
* for all node wrt the root (root has zero orientation) * 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); 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, /* 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: * 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] * 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, /*OUTPUTS*/ std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds, key2doubleMap& deltaThetaMap,
/*INPUTS*/ const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g); /*INPUTS*/ const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g);
/* Retrieves the deltaTheta and the corresponding noise model from a BetweenFactor<Pose2> */ /* 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); Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta);
/* Linear factor graph with regularized orientation measurements */ /* 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); const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
/* Selects the subgraph of betweenFactors and transforms priors into between wrt a fictitious node */ /* 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> */ /* 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 */ /* 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 */ /* 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 */ /* 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 } // 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) * @return initial Values containing the initial guess (VERTEX_SE2)
*/ */
enum kernelFunctionType { QUADRATIC, HUBER, TUKEY }; 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 * @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) * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2)
* @return estimate Values containing the values (VERTEX_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 * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a