From 432fce817e78a6729f2ae3bca8ad4b728d265423 Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Wed, 28 May 2014 21:29:23 -0400 Subject: [PATCH] Fixes for Windows. Everything compiles and passes again. --- gtsam/nonlinear/LagoInitializer.cpp | 3 ++- gtsam/nonlinear/LagoInitializer.h | 20 ++++++++++---------- gtsam/slam/dataset.h | 4 ++-- 3 files changed, 14 insertions(+), 13 deletions(-) diff --git a/gtsam/nonlinear/LagoInitializer.cpp b/gtsam/nonlinear/LagoInitializer.cpp index 789ff8d87..3451beaf3 100644 --- a/gtsam/nonlinear/LagoInitializer.cpp +++ b/gtsam/nonlinear/LagoInitializer.cpp @@ -18,6 +18,7 @@ #include #include +#include namespace gtsam { @@ -152,7 +153,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const std::vector& 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)); diff --git a/gtsam/nonlinear/LagoInitializer.h b/gtsam/nonlinear/LagoInitializer.h index 92f23a66d..b351365c1 100644 --- a/gtsam/nonlinear/LagoInitializer.h +++ b/gtsam/nonlinear/LagoInitializer.h @@ -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& tree, +GTSAM_EXPORT double computeThetaToRoot(const Key nodeKey, const PredecessorMap& 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& 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& spanningTreeIds, std::vector& chordsIds, key2doubleMap& deltaThetaMap, /*INPUTS*/ const PredecessorMap& tree, const NonlinearFactorGraph& g); /* Retrieves the deltaTheta and the corresponding noise model from a BetweenFactor */ -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& spanningTreeIds, const std::vector& chordsIds, +GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const std::vector& spanningTreeIds, const std::vector& chordsIds, const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap& 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 */ -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 diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 2f73db37f..98297ac04 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -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