Finish timeLago
						commit
						bc69c0a94e
					
				|  | @ -2857,6 +2857,14 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="timeLago.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>timeLago.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j4</buildArguments> | ||||
|  |  | |||
|  | @ -23,43 +23,35 @@ namespace gtsam { | |||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| template<class FG> | ||||
| void VariableIndex::augment(const FG& factors, boost::optional<const FastVector<size_t>&> newFactorIndices) | ||||
| { | ||||
| void VariableIndex::augment(const FG& factors, | ||||
|     boost::optional<const FastVector<size_t>&> newFactorIndices) { | ||||
|   gttic(VariableIndex_augment); | ||||
| 
 | ||||
|   // Augment index for each factor
 | ||||
|   for(size_t i = 0; i < factors.size(); ++i) | ||||
|   { | ||||
|     if(factors[i]) | ||||
|     { | ||||
|   for (size_t i = 0; i < factors.size(); ++i) { | ||||
|     if (factors[i]) { | ||||
|       const size_t globalI = | ||||
|         newFactorIndices ? | ||||
|         (*newFactorIndices)[i] : | ||||
|         nFactors_; | ||||
|       BOOST_FOREACH(const Key key, *factors[i]) | ||||
|       { | ||||
|           newFactorIndices ? (*newFactorIndices)[i] : nFactors_; | ||||
|       BOOST_FOREACH(const Key key, *factors[i]) { | ||||
|         index_[key].push_back(globalI); | ||||
|         ++ nEntries_; | ||||
|         ++nEntries_; | ||||
|       } | ||||
|     } | ||||
| 
 | ||||
|     // Increment factor count even if factors are null, to keep indices consistent
 | ||||
|     if(newFactorIndices) | ||||
|     { | ||||
|       if((*newFactorIndices)[i] >= nFactors_) | ||||
|     if (newFactorIndices) { | ||||
|       if ((*newFactorIndices)[i] >= nFactors_) | ||||
|         nFactors_ = (*newFactorIndices)[i] + 1; | ||||
|     } | ||||
|     else | ||||
|     { | ||||
|       ++ nFactors_; | ||||
|     } else { | ||||
|       ++nFactors_; | ||||
|     } | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| template<typename ITERATOR, class FG> | ||||
| void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors) | ||||
| { | ||||
| void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, | ||||
|     const FG& factors) { | ||||
|   gttic(VariableIndex_remove); | ||||
| 
 | ||||
|   // NOTE: We intentionally do not decrement nFactors_ because the factor
 | ||||
|  | @ -68,17 +60,20 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& | |||
|   // one greater than the highest-numbered factor referenced in a VariableIndex.
 | ||||
|   ITERATOR factorIndex = firstFactor; | ||||
|   size_t i = 0; | ||||
|   for( ; factorIndex != lastFactor; ++factorIndex, ++i) { | ||||
|     if(i >= factors.size()) | ||||
|       throw std::invalid_argument("Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove"); | ||||
|     if(factors[i]) { | ||||
|   for (; factorIndex != lastFactor; ++factorIndex, ++i) { | ||||
|     if (i >= factors.size()) | ||||
|       throw std::invalid_argument( | ||||
|           "Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove"); | ||||
|     if (factors[i]) { | ||||
|       BOOST_FOREACH(Key j, *factors[i]) { | ||||
|         Factors& factorEntries = internalAt(j); | ||||
|         Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), *factorIndex); | ||||
|         if(entry == factorEntries.end()) | ||||
|           throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index"); | ||||
|         Factors::iterator entry = std::find(factorEntries.begin(), | ||||
|             factorEntries.end(), *factorIndex); | ||||
|         if (entry == factorEntries.end()) | ||||
|           throw std::invalid_argument( | ||||
|               "Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index"); | ||||
|         factorEntries.erase(entry); | ||||
|         -- nEntries_; | ||||
|         --nEntries_; | ||||
|       } | ||||
|     } | ||||
|   } | ||||
|  | @ -87,10 +82,11 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& | |||
| /* ************************************************************************* */ | ||||
| template<typename ITERATOR> | ||||
| void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) { | ||||
|   for(ITERATOR key = firstKey; key != lastKey; ++key) { | ||||
|   for (ITERATOR key = firstKey; key != lastKey; ++key) { | ||||
|     KeyMap::iterator entry = index_.find(*key); | ||||
|     if(!entry->second.empty()) | ||||
|       throw std::invalid_argument("Asking to remove variables from the variable index that are not unused"); | ||||
|     if (!entry->second.empty()) | ||||
|       throw std::invalid_argument( | ||||
|           "Asking to remove variables from the variable index that are not unused"); | ||||
|     index_.erase(entry); | ||||
|   } | ||||
| } | ||||
|  |  | |||
|  | @ -21,9 +21,12 @@ | |||
| #include <gtsam/slam/BetweenFactor.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/base/timing.h> | ||||
| 
 | ||||
| #include <boost/math/special_functions.hpp> | ||||
| 
 | ||||
| using namespace std; | ||||
| 
 | ||||
| namespace gtsam { | ||||
| namespace lago { | ||||
| 
 | ||||
|  | @ -32,7 +35,7 @@ static const Matrix I3 = eye(3); | |||
| 
 | ||||
| static const Key keyAnchor = symbol('Z', 9999999); | ||||
| static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = | ||||
|     noiseModel::Diagonal::Variances((Vector(1) << 1e-8)); | ||||
|     noiseModel::Diagonal::Sigmas((Vector(1) << 0)); | ||||
| static const noiseModel::Diagonal::shared_ptr priorPose2Noise = | ||||
|     noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); | ||||
| 
 | ||||
|  | @ -76,7 +79,7 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, | |||
|   key2doubleMap thetaToRootMap; | ||||
| 
 | ||||
|   // Orientation of the roo
 | ||||
|   thetaToRootMap.insert(std::pair<Key, double>(keyAnchor, 0.0)); | ||||
|   thetaToRootMap.insert(pair<Key, double>(keyAnchor, 0.0)); | ||||
| 
 | ||||
|   // for all nodes in the tree
 | ||||
|   BOOST_FOREACH(const key2doubleMap::value_type& it, deltaThetaMap) { | ||||
|  | @ -84,14 +87,14 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, | |||
|     Key nodeKey = it.first; | ||||
|     double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, | ||||
|         thetaToRootMap); | ||||
|     thetaToRootMap.insert(std::pair<Key, double>(nodeKey, nodeTheta)); | ||||
|     thetaToRootMap.insert(pair<Key, double>(nodeKey, nodeTheta)); | ||||
|   } | ||||
|   return thetaToRootMap; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void getSymbolicGraph( | ||||
| /*OUTPUTS*/std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds, | ||||
| /*OUTPUTS*/vector<size_t>& spanningTreeIds, vector<size_t>& chordsIds, | ||||
|     key2doubleMap& deltaThetaMap, | ||||
|     /*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g) { | ||||
| 
 | ||||
|  | @ -112,10 +115,10 @@ void getSymbolicGraph( | |||
|       // insert (directed) orientations in the map "deltaThetaMap"
 | ||||
|       bool inTree = false; | ||||
|       if (tree.at(key1) == key2) { // key2 -> key1
 | ||||
|         deltaThetaMap.insert(std::pair<Key, double>(key1, -deltaTheta)); | ||||
|         deltaThetaMap.insert(pair<Key, double>(key1, -deltaTheta)); | ||||
|         inTree = true; | ||||
|       } else if (tree.at(key2) == key1) { // key1 -> key2
 | ||||
|         deltaThetaMap.insert(std::pair<Key, double>(key2, deltaTheta)); | ||||
|         deltaThetaMap.insert(pair<Key, double>(key2, deltaTheta)); | ||||
|         inTree = true; | ||||
|       } | ||||
|       // store factor slot, distinguishing spanning tree edges from chordsIds
 | ||||
|  | @ -138,7 +141,7 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, | |||
|   boost::shared_ptr<BetweenFactor<Pose2> > pose2Between = | ||||
|       boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor); | ||||
|   if (!pose2Between) | ||||
|     throw std::invalid_argument( | ||||
|     throw invalid_argument( | ||||
|         "buildLinearOrientationGraph: invalid between factor!"); | ||||
|   deltaTheta = (Vector(1) << pose2Between->measured().theta()); | ||||
| 
 | ||||
|  | @ -147,18 +150,17 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, | |||
|   boost::shared_ptr<noiseModel::Diagonal> diagonalModel = | ||||
|       boost::dynamic_pointer_cast<noiseModel::Diagonal>(model); | ||||
|   if (!diagonalModel) | ||||
|     throw std::invalid_argument( | ||||
|         "buildLinearOrientationGraph: invalid noise model " | ||||
|             "(current version assumes diagonal noise model)!"); | ||||
|     throw invalid_argument("buildLinearOrientationGraph: invalid noise model " | ||||
|         "(current version assumes diagonal noise model)!"); | ||||
|   Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2)); // std on the angular measurement
 | ||||
|   model_deltaTheta = noiseModel::Diagonal::Sigmas(std_deltaTheta); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| 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 vector<size_t>& spanningTreeIds, const vector<size_t>& chordsIds, | ||||
|     const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, | ||||
|     const PredecessorMap<Key>& tree) { | ||||
| 
 | ||||
|   GaussianFactorGraph lagoGraph; | ||||
|   Vector deltaTheta; | ||||
|  | @ -169,8 +171,7 @@ GaussianFactorGraph buildLinearOrientationGraph( | |||
|     const FastVector<Key>& keys = g[factorId]->keys(); | ||||
|     Key key1 = keys[0], key2 = keys[1]; | ||||
|     getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); | ||||
|     lagoGraph.add( | ||||
|         JacobianFactor(key1, -I, key2, I, deltaTheta, model_deltaTheta)); | ||||
|     lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta); | ||||
|   } | ||||
|   // put regularized measurements in the chordsIds
 | ||||
|   BOOST_FOREACH(const size_t& factorId, chordsIds) { | ||||
|  | @ -178,26 +179,24 @@ GaussianFactorGraph buildLinearOrientationGraph( | |||
|     Key key1 = keys[0], key2 = keys[1]; | ||||
|     getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); | ||||
|     double key1_DeltaTheta_key2 = deltaTheta(0); | ||||
|     ///std::cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << std::endl;
 | ||||
|     ///cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << 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 = 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) cout << k2pi_noise - 2*k*M_PI << endl; // for debug
 | ||||
|     Vector deltaThetaRegularized = (Vector(1) | ||||
|         << key1_DeltaTheta_key2 - 2 * k * M_PI); | ||||
|     lagoGraph.add( | ||||
|         JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, | ||||
|             model_deltaTheta)); | ||||
|     lagoGraph.add(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta); | ||||
|   } | ||||
|   // prior on the anchor orientation
 | ||||
|   lagoGraph.add( | ||||
|       JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise)); | ||||
|   lagoGraph.add(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise); | ||||
|   return lagoGraph; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
 | ||||
| static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { | ||||
|   gttic(lago_buildPose2graph); | ||||
|   NonlinearFactorGraph pose2Graph; | ||||
| 
 | ||||
|   BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) { | ||||
|  | @ -250,6 +249,7 @@ static PredecessorMap<Key> findOdometricPath( | |||
| // Return the orientations of a graph including only BetweenFactors<Pose2>
 | ||||
| static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, | ||||
|     bool useOdometricPath) { | ||||
|   gttic(lago_computeOrientations); | ||||
| 
 | ||||
|   // Find a minimum spanning tree
 | ||||
|   PredecessorMap<Key> tree; | ||||
|  | @ -261,8 +261,8 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, | |||
| 
 | ||||
|   // Create a linear factor graph (LFG) of scalars
 | ||||
|   key2doubleMap deltaThetaMap; | ||||
|   std::vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
 | ||||
|   std::vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
 | ||||
|   vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
 | ||||
|   vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
 | ||||
|   getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph); | ||||
| 
 | ||||
|   // temporary structure to correct wraparounds along loops
 | ||||
|  | @ -293,6 +293,7 @@ VectorValues initializeOrientations(const NonlinearFactorGraph& graph, | |||
| /* ************************************************************************* */ | ||||
| Values computePoses(const NonlinearFactorGraph& pose2graph, | ||||
|     VectorValues& orientationsLago) { | ||||
|   gttic(lago_computePoses); | ||||
| 
 | ||||
|   // Linearized graph on full poses
 | ||||
|   GaussianFactorGraph linearPose2graph; | ||||
|  | @ -319,8 +320,8 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, | |||
|       double dx = pose2Between->measured().x(); | ||||
|       double dy = pose2Between->measured().y(); | ||||
| 
 | ||||
|       Vector globalDeltaCart = (Vector(2) << c1 * dx - s1 * dy, s1 * dx | ||||
|           + c1 * dy); | ||||
|       Vector globalDeltaCart = //
 | ||||
|           (Vector(2) << c1 * dx - s1 * dy, s1 * dx + c1 * dy); | ||||
|       Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot); // rhs
 | ||||
|       Matrix J1 = -I3; | ||||
|       J1(0, 2) = s1 * dx + c1 * dy; | ||||
|  | @ -330,18 +331,15 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, | |||
|           boost::dynamic_pointer_cast<noiseModel::Diagonal>( | ||||
|               pose2Between->get_noiseModel()); | ||||
| 
 | ||||
|       linearPose2graph.add( | ||||
|           JacobianFactor(key1, J1, key2, I3, b, diagonalModel)); | ||||
|       linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel); | ||||
|     } else { | ||||
|       throw std::invalid_argument( | ||||
|       throw invalid_argument( | ||||
|           "computeLagoPoses: cannot manage non between factor here!"); | ||||
|     } | ||||
|   } | ||||
|   // add prior
 | ||||
|   noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances( | ||||
|       (Vector(3) << 1e-2, 1e-2, 1e-4)); | ||||
|   linearPose2graph.add( | ||||
|       JacobianFactor(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0), priorModel)); | ||||
|   linearPose2graph.add(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0), | ||||
|       priorPose2Noise); | ||||
| 
 | ||||
|   // optimize
 | ||||
|   VectorValues posesLago = linearPose2graph.optimize(); | ||||
|  | @ -362,6 +360,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, | |||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) { | ||||
|   gttic(lago_initialize); | ||||
| 
 | ||||
|   // We "extract" the Pose2 subgraph of the original graph: this
 | ||||
|   // is done to properly model priors and avoiding operating on a larger graph
 | ||||
|  |  | |||
|  | @ -1 +1 @@ | |||
| gtsamAddTestsGlob(nonlinear "test*.cpp" "" "gtsam") | ||||
| gtsamAddTestsGlob(nonlinear "*.cpp" "" "gtsam") | ||||
|  |  | |||
|  | @ -0,0 +1,82 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||||
|  * Atlanta, Georgia 30332-0415 | ||||
|  * All Rights Reserved | ||||
|  * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  * @file    timeVirtual.cpp | ||||
|  * @brief   Time the overhead of using virtual destructors and methods | ||||
|  * @author  Richard Roberts | ||||
|  * @date    Dec 3, 2010 | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/slam/dataset.h> | ||||
| #include <gtsam/slam/PriorFactor.h> | ||||
| #include <gtsam/nonlinear/lago.h> | ||||
| #include <gtsam/nonlinear/GaussNewtonOptimizer.h> | ||||
| #include <gtsam/linear/Sampler.h> | ||||
| #include <gtsam/base/timing.h> | ||||
| 
 | ||||
| #include <iostream> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| int main(int argc, char *argv[]) { | ||||
| 
 | ||||
|   size_t trials = 1; | ||||
| 
 | ||||
|   // read graph
 | ||||
|   Values::shared_ptr solution; | ||||
|   NonlinearFactorGraph::shared_ptr g; | ||||
|   string inputFile = findExampleDataFile("w10000"); | ||||
|   SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0)); | ||||
|   boost::tie(g, solution) = load2D(inputFile, model); | ||||
| 
 | ||||
|   // add noise to create initial estimate
 | ||||
|   Values initial; | ||||
|   Sampler sampler(42u); | ||||
|   Values::ConstFiltered<Pose2> poses = solution->filter<Pose2>(); | ||||
|   SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0)); | ||||
|   BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& it, poses) | ||||
|     initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise))); | ||||
| 
 | ||||
|   // Add prior on the pose having index (key) = 0
 | ||||
|   noiseModel::Diagonal::shared_ptr priorModel = //
 | ||||
|       noiseModel::Diagonal::Sigmas(Vector3(1e-6, 1e-6, 1e-8)); | ||||
|   g->add(PriorFactor<Pose2>(0, Pose2(), priorModel)); | ||||
| 
 | ||||
|   // LAGO
 | ||||
|   for (size_t i = 0; i < trials; i++) { | ||||
|     { | ||||
|       gttic_(lago); | ||||
| 
 | ||||
|       gttic_(init); | ||||
|       Values lagoInitial = lago::initialize(*g); | ||||
|       gttoc_(init); | ||||
| 
 | ||||
|       gttic_(refine); | ||||
|       GaussNewtonOptimizer optimizer(*g, lagoInitial); | ||||
|       Values result = optimizer.optimize(); | ||||
|       gttoc_(refine); | ||||
|     } | ||||
| 
 | ||||
|     { | ||||
|       gttic_(optimize); | ||||
|       GaussNewtonOptimizer optimizer(*g, initial); | ||||
|       Values result = optimizer.optimize(); | ||||
|     } | ||||
| 
 | ||||
|     tictoc_finishedIteration_(); | ||||
|   } | ||||
| 
 | ||||
|   tictoc_print_(); | ||||
| 
 | ||||
|   return 0; | ||||
| } | ||||
|  | @ -100,7 +100,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D( | |||
|   cout << "Will try to read " << filename << endl; | ||||
|   ifstream is(filename.c_str()); | ||||
|   if (!is) | ||||
|     throw std::invalid_argument("load2D: can not find the file!"); | ||||
|     throw std::invalid_argument("load2D: can not find file " + filename); | ||||
| 
 | ||||
|   Values::shared_ptr initial(new Values); | ||||
|   NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue