56 const std::map<int, Transform> & poses,
57 const std::multimap<int, Link> & edgeConstraints,
58 cv::Mat & outputCovariance,
59 std::list<std::map<int, Transform> > * intermediateGraphes,
63 outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
64 std::map<int, Transform> optimizedPoses;
66 UDEBUG(
"Optimizing graph (pose=%d constraints=%d)...", (
int)poses.size(), (
int)edgeConstraints.size());
67 if(edgeConstraints.size()>=1 && poses.size()>=2 &&
iterations() > 0)
75 UDEBUG(
"fill poses to TORO...");
78 for(std::map<int, Transform>::const_iterator
iter = poses.begin();
iter!=poses.end(); ++
iter)
84 AISNavigation::TreePoseGraph2::Vertex*
v = pg2.
addVertex(
iter->first,
p);
91 for(std::map<int, Transform>::const_iterator
iter = poses.begin();
iter!=poses.end(); ++
iter)
99 AISNavigation::TreePoseGraph3::Vertex*
v = pg3.
addVertex(
iter->first,
p);
107 UDEBUG(
"fill edges to TORO...");
110 for(std::multimap<int, Link>::const_iterator
iter=edgeConstraints.begin();
iter!=edgeConstraints.end(); ++
iter)
124 inf.
values[0][0] =
iter->second.infMatrix().at<
double>(0,0);
125 inf.
values[0][1] =
iter->second.infMatrix().at<
double>(0,1);
126 inf.
values[0][2] =
iter->second.infMatrix().at<
double>(0,5);
127 inf.
values[1][0] =
iter->second.infMatrix().at<
double>(1,0);
128 inf.
values[1][1] =
iter->second.infMatrix().at<
double>(1,1);
129 inf.
values[1][2] =
iter->second.infMatrix().at<
double>(1,5);
130 inf.
values[2][0] =
iter->second.infMatrix().at<
double>(5,0);
131 inf.
values[2][1] =
iter->second.infMatrix().at<
double>(5,1);
132 inf.
values[2][2] =
iter->second.infMatrix().at<
double>(5,5);
135 int id1 =
iter->second.from();
136 int id2 =
iter->second.to();
137 if(id1 != id2 && id1 > 0 && id2 > 0)
139 AISNavigation::TreePoseGraph2::Vertex*
v1=pg2.
vertex(id1);
140 AISNavigation::TreePoseGraph2::Vertex*
v2=pg2.
vertex(id2);
146 UERROR(
"Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
151 UWARN(
"TORO optimizer doesn't support prior or gravity links, use GTSAM or g2o optimizers (see parameter %s). Link %d ignored...", Parameters::kOptimizerStrategy().
c_str(), id1);
153 else if(id1 < 0 || id2 < 0)
155 UWARN(
"TORO optimizer doesn't support landmark links, use GTSAM or g2o optimizers (see parameter %s). Link %d->%d ignored...", Parameters::kOptimizerStrategy().
c_str(), id1, id2);
161 for(std::multimap<int, Link>::const_iterator
iter=edgeConstraints.begin();
iter!=edgeConstraints.end(); ++
iter)
171 memcpy(inf[0],
iter->second.infMatrix().data,
iter->second.infMatrix().total()*
sizeof(
double));
174 int id1 =
iter->second.from();
175 int id2 =
iter->second.to();
176 if(id1 != id2 && id1 > 0 && id2 > 0)
178 AISNavigation::TreePoseGraph3::Vertex*
v1=pg3.
vertex(id1);
179 AISNavigation::TreePoseGraph3::Vertex*
v2=pg3.
vertex(id2);
185 UERROR(
"Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
190 UWARN(
"TORO optimizer doesn't support prior or gravity links, use GTSAM or g2o optimizers (see parameter %s). Link %d ignored...", Parameters::kOptimizerStrategy().
c_str(), id1);
192 else if(id1 < 0 || id2 < 0)
194 UWARN(
"TORO optimizer doesn't support landmark links, use GTSAM or g2o optimizers (see parameter %s). Link %d->%d ignored...", Parameters::kOptimizerStrategy().
c_str(), id1, id2);
198 UDEBUG(
"buildMST... root=%d", rootId);
205 UDEBUG(
"initializeTreeParameters()");
207 UDEBUG(
"Building TORO tree... (if a crash happens just after this msg, "
208 "TORO is not able to find the root of the graph!)");
216 UDEBUG(
"initializeTreeParameters()");
218 UDEBUG(
"Building TORO tree... (if a crash happens just after this msg, "
219 "TORO is not able to find the root of the graph!)");
225 double lastError = 0;
230 if(intermediateGraphes &&
i>0)
232 std::map<int, Transform> tmpPoses;
235 for(std::map<int, Transform>::const_iterator
iter = poses.begin();
iter!=poses.end(); ++
iter)
239 AISNavigation::TreePoseGraph2::Vertex*
v=pg2.
vertex(
iter->first);
245 tmpPoses.insert(std::pair<int, Transform>(
iter->first, newPose));
251 for(std::map<int, Transform>::const_iterator
iter = poses.begin();
iter!=poses.end(); ++
iter)
255 AISNavigation::TreePoseGraph3::Vertex*
v=pg3.
vertex(
iter->first);
260 tmpPoses.insert(std::pair<int, Transform>(
iter->first, newPose));
264 intermediateGraphes->push_back(tmpPoses);
281 double mte, mre, are, ate;
283 UDEBUG(
"i %d RotGain=%f global error=%f error/constraint=%f",
288 double errorDelta = lastError -
error;
289 if(
i>0 && errorDelta < this->
epsilon())
293 UDEBUG(
"Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->
epsilon());
297 UINFO(
"Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->
epsilon());
301 else if(i==0 && error < this->
epsilon())
303 UINFO(
"Stop optimizing, error is already under epsilon (%f < %f)",
error, this->
epsilon());
310 *finalError = lastError;
316 UINFO(
"TORO optimizing end (%d iterations done, error=%f, time = %f s)",
i, lastError,
timer.ticks());
320 for(std::map<int, Transform>::const_iterator
iter = poses.begin();
iter!=poses.end(); ++
iter)
324 AISNavigation::TreePoseGraph2::Vertex*
v=pg2.
vertex(
iter->first);
330 optimizedPoses.insert(std::pair<int, Transform>(
iter->first, newPose));
336 for(std::map<int, Transform>::const_iterator
iter = poses.begin();
iter!=poses.end(); ++
iter)
340 AISNavigation::TreePoseGraph3::Vertex*
v=pg3.
vertex(
iter->first);
345 optimizedPoses.insert(std::pair<int, Transform>(
iter->first, newPose));
352 else if(poses.size() == 1 ||
iterations() <= 0)
354 optimizedPoses = poses;
358 UWARN(
"This method should be called at least with 1 pose!");
360 UDEBUG(
"Optimizing graph...end!");
362 UERROR(
"Not built with TORO support!");
364 return optimizedPoses;
368 const std::string & fileName,
369 const std::map<int, Transform> & poses,
370 const std::multimap<int, Link> & edgeConstraints)
375 fopen_s(&
file, fileName.c_str(),
"w");
377 file = fopen(fileName.c_str(),
"w");
383 for (std::map<int, Transform>::const_iterator
iter = poses.begin();
iter != poses.end(); ++
iter)
388 fprintf(
file,
"VERTEX2 %d %f %f %f\n",
392 iter->second.theta());
399 fprintf(
file,
"VERTEX3 %d %f %f %f %f %f %f\n",
410 for(std::multimap<int, Link>::const_iterator
iter = edgeConstraints.begin();
iter!=edgeConstraints.end(); ++
iter)
417 fprintf(
file,
"EDGE2 %d %d %f %f %f %f %f %f %f %f %f\n",
420 iter->second.transform().x(),
421 iter->second.transform().y(),
422 iter->second.transform().theta(),
423 iter->second.infMatrix().at<
double>(0, 0),
424 iter->second.infMatrix().at<
double>(0, 1),
425 iter->second.infMatrix().at<
double>(0, 5),
426 iter->second.infMatrix().at<
double>(1, 1),
427 iter->second.infMatrix().at<
double>(1, 5),
428 iter->second.infMatrix().at<
double>(5, 5));
435 fprintf(
file,
"EDGE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
444 iter->second.infMatrix().at<
double>(0, 0),
445 iter->second.infMatrix().at<
double>(0, 1),
446 iter->second.infMatrix().at<
double>(0, 2),
447 iter->second.infMatrix().at<
double>(0, 3),
448 iter->second.infMatrix().at<
double>(0, 4),
449 iter->second.infMatrix().at<
double>(0, 5),
450 iter->second.infMatrix().at<
double>(1, 1),
451 iter->second.infMatrix().at<
double>(1, 2),
452 iter->second.infMatrix().at<
double>(1, 3),
453 iter->second.infMatrix().at<
double>(1, 4),
454 iter->second.infMatrix().at<
double>(1, 5),
455 iter->second.infMatrix().at<
double>(2, 2),
456 iter->second.infMatrix().at<
double>(2, 3),
457 iter->second.infMatrix().at<
double>(2, 4),
458 iter->second.infMatrix().at<
double>(2, 5),
459 iter->second.infMatrix().at<
double>(3, 3),
460 iter->second.infMatrix().at<
double>(3, 4),
461 iter->second.infMatrix().at<
double>(3, 5),
462 iter->second.infMatrix().at<
double>(4, 4),
463 iter->second.infMatrix().at<
double>(4, 5),
464 iter->second.infMatrix().at<
double>(5, 5));
468 UINFO(
"Graph saved to %s", fileName.c_str());
473 UERROR(
"Cannot save to file %s", fileName.c_str());
480 const std::string & fileName,
481 std::map<int, Transform> & poses,
482 std::multimap<int, Link> & edgeConstraints)
486 fopen_s(&
file, fileName.c_str(),
"r");
488 file = fopen(fileName.c_str(),
"r");
494 while ( fgets (line , 400 ,
file) !=
NULL )
497 if(strList.size() == 8)
500 int id = atoi(strList[1].
c_str());
508 if(poses.find(
id) == poses.end())
510 poses.insert(std::make_pair(
id, pose));
514 UFATAL(
"Pose %d already added",
id);
517 else if(strList.size() == 30)
520 int idFrom = atoi(strList[1].
c_str());
521 int idTo = atoi(strList[2].
c_str());
528 cv::Mat informationMatrix(6,6,CV_64FC1);
529 informationMatrix.at<
double>(3,3) =
uStr2Float(strList[9]);
530 informationMatrix.at<
double>(4,4) =
uStr2Float(strList[15]);
531 informationMatrix.at<
double>(5,5) =
uStr2Float(strList[20]);
532 UASSERT_MSG(informationMatrix.at<
double>(3,3) > 0.0 && informationMatrix.at<
double>(4,4) > 0.0 && informationMatrix.at<
double>(5,5) > 0.0,
uFormat(
"Information matrix should not be null! line=\"%s\"", line).
c_str());
533 informationMatrix.at<
double>(0,0) =
uStr2Float(strList[24]);
534 informationMatrix.at<
double>(1,1) =
uStr2Float(strList[27]);
535 informationMatrix.at<
double>(2,2) =
uStr2Float(strList[29]);
536 UASSERT_MSG(informationMatrix.at<
double>(0,0) > 0.0 && informationMatrix.at<
double>(1,1) > 0.0 && informationMatrix.at<
double>(2,2) > 0.0,
uFormat(
"Information matrix should not be null! line=\"%s\"", line).
c_str());
538 if(poses.find(idFrom) != poses.end() && poses.find(idTo) != poses.end())
542 edgeConstraints.insert(std::pair<int, Link>(idFrom, link));
546 UFATAL(
"Referred poses from the link not exist!");
549 else if(strList.size())
551 UFATAL(
"Error parsing graph file %s on line \"%s\" (strList.size()=%d)", fileName.c_str(), line, (
int)strList.size());
555 UINFO(
"Graph loaded from %s", fileName.c_str());
560 UERROR(
"Cannot open file %s", fileName.c_str());