diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index 792dad80c..f8b089539 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -30,9 +30,11 @@ namespace lago { static const Matrix I = eye(1); 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)); -static const noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); +static const Key keyAnchor = symbol('Z', 9999999); +static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = + noiseModel::Diagonal::Variances((Vector(1) << 1e-8)); +static const noiseModel::Diagonal::shared_ptr priorPose2Noise = + noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); /* ************************************************************************* */ double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, @@ -41,16 +43,16 @@ double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, double nodeTheta = 0; Key key_child = nodeKey; // the node Key key_parent = 0; // the initialization does not matter - while(1){ + while (1) { // We check if we reached the root - if(tree.at(key_child)==key_child) // if we reached the root + if (tree.at(key_child) == key_child) // if we reached the root break; // we sum the delta theta corresponding to the edge parent->child nodeTheta += deltaThetaMap.at(key_child); // we get the parent key_parent = tree.at(key_child); // the parent // we check if we connected to some part of the tree we know - if(thetaFromRootMap.find(key_parent)!=thetaFromRootMap.end()){ + if (thetaFromRootMap.find(key_parent) != thetaFromRootMap.end()) { nodeTheta += thetaFromRootMap.at(key_parent); break; } @@ -64,15 +66,14 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, const PredecessorMap& tree) { key2doubleMap thetaToRootMap; - key2doubleMap::const_iterator it; // Orientation of the roo thetaToRootMap.insert(std::pair(keyAnchor, 0.0)); // for all nodes in the tree - for(it = deltaThetaMap.begin(); it != deltaThetaMap.end(); ++it ){ + BOOST_FOREACH(const key2doubleMap::value_type& it, deltaThetaMap) { // compute the orientation wrt root - Key nodeKey = it->first; + Key nodeKey = it.first; double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, thetaToRootMap); thetaToRootMap.insert(std::pair(nodeKey, nodeTheta)); @@ -82,35 +83,38 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, /* ************************************************************************* */ void getSymbolicGraph( - /*OUTPUTS*/ std::vector& spanningTreeIds, std::vector& chordsIds, key2doubleMap& deltaThetaMap, - /*INPUTS*/ const PredecessorMap& tree, const NonlinearFactorGraph& g){ +/*OUTPUTS*/std::vector& spanningTreeIds, std::vector& chordsIds, + key2doubleMap& deltaThetaMap, + /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g) { // Get keys for which you want the orientation - size_t id=0; + size_t id = 0; // Loop over the factors - BOOST_FOREACH(const boost::shared_ptr& factor, g){ - if (factor->keys().size() == 2){ + BOOST_FOREACH(const boost::shared_ptr& factor, g) { + if (factor->keys().size() == 2) { Key key1 = factor->keys()[0]; Key key2 = factor->keys()[1]; // recast to a between - boost::shared_ptr< BetweenFactor > pose2Between = - boost::dynamic_pointer_cast< BetweenFactor >(factor); - if (!pose2Between) continue; + boost::shared_ptr > pose2Between = + boost::dynamic_pointer_cast >(factor); + if (!pose2Between) + continue; // get the orientation - measured().theta(); double deltaTheta = pose2Between->measured().theta(); // insert (directed) orientations in the map "deltaThetaMap" - bool inTree=false; - if(tree.at(key1)==key2){ // key2 -> key1 + bool inTree = false; + if (tree.at(key1) == key2) { // key2 -> key1 deltaThetaMap.insert(std::pair(key1, -deltaTheta)); inTree = true; - } else if(tree.at(key2)==key1){ // key1 -> key2 - deltaThetaMap.insert(std::pair(key2, deltaTheta)); + } else if (tree.at(key2) == key1) { // key1 -> key2 + deltaThetaMap.insert(std::pair(key2, deltaTheta)); inTree = true; } // store factor slot, distinguishing spanning tree edges from chordsIds - if(inTree == true) + if (inTree == true) spanningTreeIds.push_back(id); - else // it's a chord! + else + // it's a chord! chordsIds.push_back(id); } id++; @@ -125,7 +129,8 @@ void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, boost::shared_ptr > pose2Between = boost::dynamic_pointer_cast >(factor); if (!pose2Between) - throw std::invalid_argument("buildLinearOrientationGraph: invalid between factor!"); + throw std::invalid_argument( + "buildLinearOrientationGraph: invalid between factor!"); deltaTheta = (Vector(1) << pose2Between->measured().theta()); // Retrieve the noise model for the relative rotation @@ -133,63 +138,73 @@ void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, boost::shared_ptr diagonalModel = boost::dynamic_pointer_cast(model); if (!diagonalModel) - throw std::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 + throw std::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& spanningTreeIds, const std::vector& chordsIds, - const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap& tree){ +GaussianFactorGraph buildLinearOrientationGraph( + const std::vector& spanningTreeIds, + const std::vector& chordsIds, const NonlinearFactorGraph& g, + const key2doubleMap& orientationsToRoot, const PredecessorMap& tree) { GaussianFactorGraph lagoGraph; Vector deltaTheta; noiseModel::Diagonal::shared_ptr model_deltaTheta; // put original measurements in the spanning tree - BOOST_FOREACH(const size_t& factorId, spanningTreeIds){ + BOOST_FOREACH(const size_t& factorId, spanningTreeIds) { const FastVector& 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( + JacobianFactor(key1, -I, key2, I, deltaTheta, model_deltaTheta)); } // put regularized measurements in the chordsIds - BOOST_FOREACH(const size_t& factorId, chordsIds){ + BOOST_FOREACH(const size_t& factorId, chordsIds) { const FastVector& keys = g[factorId]->keys(); 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; - 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)); + 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 - Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2*k*M_PI); - lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta)); + Vector deltaThetaRegularized = (Vector(1) + << key1_DeltaTheta_key2 - 2 * k * M_PI); + lagoGraph.add( + JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, + model_deltaTheta)); } // prior on the anchor orientation - lagoGraph.add(JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise)); + lagoGraph.add( + JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise)); return lagoGraph; } /* ************************************************************************* */ -NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph){ +NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { NonlinearFactorGraph pose2Graph; - BOOST_FOREACH(const boost::shared_ptr& factor, graph){ + BOOST_FOREACH(const boost::shared_ptr& factor, graph) { // recast to a between on Pose2 - boost::shared_ptr< BetweenFactor > pose2Between = - boost::dynamic_pointer_cast< BetweenFactor >(factor); + boost::shared_ptr > pose2Between = + boost::dynamic_pointer_cast >(factor); if (pose2Between) pose2Graph.add(pose2Between); // recast PriorFactor to BetweenFactor - boost::shared_ptr< PriorFactor > pose2Prior = - boost::dynamic_pointer_cast< PriorFactor >(factor); + boost::shared_ptr > pose2Prior = + boost::dynamic_pointer_cast >(factor); if (pose2Prior) - pose2Graph.add(BetweenFactor(keyAnchor, pose2Prior->keys()[0], - pose2Prior->prior(), pose2Prior->get_noiseModel())); + pose2Graph.add( + BetweenFactor(keyAnchor, pose2Prior->keys()[0], + pose2Prior->prior(), pose2Prior->get_noiseModel())); } return pose2Graph; } @@ -201,46 +216,49 @@ PredecessorMap findOdometricPath(const NonlinearFactorGraph& pose2Graph) { Key minKey; bool minUnassigned = true; - BOOST_FOREACH(const boost::shared_ptr& factor, pose2Graph){ + BOOST_FOREACH(const boost::shared_ptr& factor, pose2Graph) { Key key1 = std::min(factor->keys()[0], factor->keys()[1]); Key key2 = std::max(factor->keys()[0], factor->keys()[1]); - if(minUnassigned){ + if (minUnassigned) { minKey = key1; minUnassigned = false; } - if( key2 - key1 == 1){ // consecutive keys + if (key2 - key1 == 1) { // consecutive keys tree.insert(key2, key1); - if(key1 < minKey) + if (key1 < minKey) minKey = key1; } } - tree.insert(minKey,keyAnchor); - tree.insert(keyAnchor,keyAnchor); // root + tree.insert(minKey, keyAnchor); + tree.insert(keyAnchor, keyAnchor); // root return tree; } /* ************************************************************************* */ -VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath){ +VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, + bool useOdometricPath) { // Find a minimum spanning tree PredecessorMap tree; if (useOdometricPath) tree = findOdometricPath(pose2Graph); else - tree = findMinimumSpanningTree >(pose2Graph); + tree = findMinimumSpanningTree >(pose2Graph); // Create a linear factor graph (LFG) of scalars key2doubleMap deltaThetaMap; std::vector spanningTreeIds; // ids of between factors forming the spanning tree T - std::vector chordsIds; // ids of between factors corresponding to chordsIds wrt T + std::vector chordsIds; // ids of between factors corresponding to chordsIds wrt T getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph); // temporary structure to correct wraparounds along loops key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree); // regularize measurements and plug everything in a factor graph - GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, chordsIds, pose2Graph, orientationsToRoot, tree); + GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, + chordsIds, pose2Graph, orientationsToRoot, tree); // Solve the LFG VectorValues orientationsLago = lagoGraph.optimize(); @@ -249,7 +267,8 @@ VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool us } /* ************************************************************************* */ -VectorValues initializeOrientations(const NonlinearFactorGraph& graph, bool useOdometricPath) { +VectorValues initializeOrientations(const NonlinearFactorGraph& graph, + bool useOdometricPath) { // We "extract" the Pose2 subgraph of the original graph: this // is done to properly model priors and avoiding operating on a larger graph @@ -260,59 +279,69 @@ VectorValues initializeOrientations(const NonlinearFactorGraph& graph, bool useO } /* ************************************************************************* */ -Values computePoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) { +Values computePoses(const NonlinearFactorGraph& pose2graph, + VectorValues& orientationsLago) { // Linearized graph on full poses GaussianFactorGraph linearPose2graph; // We include the linear version of each between factor - BOOST_FOREACH(const boost::shared_ptr& factor, pose2graph){ + BOOST_FOREACH(const boost::shared_ptr& factor, pose2graph) { - boost::shared_ptr< BetweenFactor > pose2Between = - boost::dynamic_pointer_cast< BetweenFactor >(factor); + boost::shared_ptr > pose2Between = + boost::dynamic_pointer_cast >(factor); - if(pose2Between){ + if (pose2Between) { Key key1 = pose2Between->keys()[0]; double theta1 = orientationsLago.at(key1)(0); - double s1 = sin(theta1); double c1 = cos(theta1); + double s1 = sin(theta1); + double c1 = cos(theta1); Key key2 = pose2Between->keys()[1]; double theta2 = orientationsLago.at(key2)(0); - double linearDeltaRot = theta2 - theta1 - pose2Between->measured().theta(); + double linearDeltaRot = theta2 - theta1 + - pose2Between->measured().theta(); linearDeltaRot = Rot2(linearDeltaRot).theta(); // to normalize double dx = pose2Between->measured().x(); double dy = pose2Between->measured().y(); - 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; - J1(1,2) = -c1*dx + s1*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; + J1(1, 2) = -c1 * dx + s1 * dy; // Retrieve the noise model for the relative rotation boost::shared_ptr diagonalModel = - boost::dynamic_pointer_cast(pose2Between->get_noiseModel()); + boost::dynamic_pointer_cast( + pose2Between->get_noiseModel()); - linearPose2graph.add(JacobianFactor(key1, J1, key2, I3, b, diagonalModel)); - }else{ - throw std::invalid_argument("computeLagoPoses: cannot manage non between factor here!"); + linearPose2graph.add( + JacobianFactor(key1, J1, key2, I3, b, diagonalModel)); + } else { + throw std::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)); + 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)); // optimize VectorValues posesLago = linearPose2graph.optimize(); // put into Values structure Values initialGuessLago; - for(VectorValues::const_iterator it = posesLago.begin(); it != posesLago.end(); ++it ){ - Key key = it->first; - if (key != keyAnchor){ - Vector poseVector = posesLago.at(key); - Pose2 poseLago = Pose2(poseVector(0),poseVector(1),orientationsLago.at(key)(0)+poseVector(2)); + BOOST_FOREACH(const VectorValues::value_type& it, posesLago) { + Key key = it.first; + if (key != keyAnchor) { + const Vector& poseVector = it.second; + Pose2 poseLago = Pose2(poseVector(0), poseVector(1), + orientationsLago.at(key)(0) + poseVector(2)); initialGuessLago.insert(key, poseLago); } } @@ -327,26 +356,28 @@ Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) { NonlinearFactorGraph pose2Graph = buildPose2graph(graph); // Get orientations from relative orientation measurements - VectorValues orientationsLago = computeOrientations(pose2Graph, useOdometricPath); + VectorValues orientationsLago = computeOrientations(pose2Graph, + useOdometricPath); // Compute the full poses return computePoses(pose2Graph, orientationsLago); } /* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph, const Values& initialGuess) { +Values initialize(const NonlinearFactorGraph& graph, + const Values& initialGuess) { Values initialGuessLago; // get the orientation estimates from LAGO VectorValues orientations = initializeOrientations(graph); // for all nodes in the tree - for(VectorValues::const_iterator it = orientations.begin(); it != orientations.end(); ++it ){ - Key key = it->first; - if (key != keyAnchor){ - Pose2 pose = initialGuess.at(key); - Vector orientation = orientations.at(key); - Pose2 poseLago = Pose2(pose.x(),pose.y(),orientation(0)); + BOOST_FOREACH(const VectorValues::value_type& it, orientations) { + Key key = it.first; + if (key != keyAnchor) { + const Pose2& pose = initialGuess.at(key); + const Vector& orientation = it.second; + Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0)); initialGuessLago.insert(key, poseLago); } }