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

Standard formatting and some BOOST_FOREACH uses

release/4.3a0
dellaert 2014-05-31 13:37:10 -04:00
parent e5344d3d92
commit 970d49f60b
1 changed files with 118 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

205
gtsam/nonlinear/lago.cpp
View File

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