Standard formatting and some BOOST_FOREACH uses
dellaert
parent
e5344d3d92
commit
970d49f60b
|
|
@ -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<Key>& tree,
|
||||
|
|
@ -41,16 +43,16 @@ double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& 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<Key>& tree) {
|
||||
|
||||
key2doubleMap thetaToRootMap;
|
||||
key2doubleMap::const_iterator it;
|
||||
|
||||
// Orientation of the roo
|
||||
thetaToRootMap.insert(std::pair<Key, double>(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<Key, double>(nodeKey, nodeTheta));
|
||||
|
|
@ -82,35 +83,38 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
|
|||
|
||||
/* ************************************************************************* */
|
||||
void getSymbolicGraph(
|
||||
/*OUTPUTS*/ std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds, key2doubleMap& deltaThetaMap,
|
||||
/*INPUTS*/ const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g){
|
||||
/*OUTPUTS*/std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds,
|
||||
key2doubleMap& deltaThetaMap,
|
||||
/*INPUTS*/const PredecessorMap<Key>& 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<NonlinearFactor>& factor, g){
|
||||
if (factor->keys().size() == 2){
|
||||
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& 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<Pose2> > pose2Between =
|
||||
boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor);
|
||||
if (!pose2Between) continue;
|
||||
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(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<Key, double>(key1, -deltaTheta));
|
||||
inTree = true;
|
||||
} else if(tree.at(key2)==key1){ // key1 -> key2
|
||||
deltaThetaMap.insert(std::pair<Key, double>(key2, deltaTheta));
|
||||
} else if (tree.at(key2) == key1) { // key1 -> key2
|
||||
deltaThetaMap.insert(std::pair<Key, double>(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<BetweenFactor<Pose2> > pose2Between =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(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<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)!");
|
||||
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<size_t>& spanningTreeIds, const std::vector<size_t>& chordsIds,
|
||||
const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree){
|
||||
GaussianFactorGraph buildLinearOrientationGraph(
|
||||
const std::vector<size_t>& spanningTreeIds,
|
||||
const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
|
||||
const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& 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<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(
|
||||
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<Key>& 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<NonlinearFactor>& factor, graph){
|
||||
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) {
|
||||
|
||||
// recast to a between on Pose2
|
||||
boost::shared_ptr< BetweenFactor<Pose2> > pose2Between =
|
||||
boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor);
|
||||
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
|
||||
if (pose2Between)
|
||||
pose2Graph.add(pose2Between);
|
||||
|
||||
// recast PriorFactor<Pose2> to BetweenFactor<Pose2>
|
||||
boost::shared_ptr< PriorFactor<Pose2> > pose2Prior =
|
||||
boost::dynamic_pointer_cast< PriorFactor<Pose2> >(factor);
|
||||
boost::shared_ptr<PriorFactor<Pose2> > pose2Prior =
|
||||
boost::dynamic_pointer_cast<PriorFactor<Pose2> >(factor);
|
||||
if (pose2Prior)
|
||||
pose2Graph.add(BetweenFactor<Pose2>(keyAnchor, pose2Prior->keys()[0],
|
||||
pose2Prior->prior(), pose2Prior->get_noiseModel()));
|
||||
pose2Graph.add(
|
||||
BetweenFactor<Pose2>(keyAnchor, pose2Prior->keys()[0],
|
||||
pose2Prior->prior(), pose2Prior->get_noiseModel()));
|
||||
}
|
||||
return pose2Graph;
|
||||
}
|
||||
|
|
@ -201,46 +216,49 @@ PredecessorMap<Key> findOdometricPath(const NonlinearFactorGraph& pose2Graph) {
|
|||
Key minKey;
|
||||
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 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<Key> tree;
|
||||
if (useOdometricPath)
|
||||
tree = findOdometricPath(pose2Graph);
|
||||
else
|
||||
tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, BetweenFactor<Pose2> >(pose2Graph);
|
||||
tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
|
||||
BetweenFactor<Pose2> >(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
|
||||
std::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
|
||||
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<NonlinearFactor>& factor, pose2graph){
|
||||
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2graph) {
|
||||
|
||||
boost::shared_ptr< BetweenFactor<Pose2> > pose2Between =
|
||||
boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor);
|
||||
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(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<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));
|
||||
}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<Pose2>(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<Pose2>(key);
|
||||
const Vector& orientation = it.second;
|
||||
Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0));
|
||||
initialGuessLago.insert(key, poseLago);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue