i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 00:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:introlab/rtabmap into gtest

Browse Source
This commit is contained in:
matlabbe
2026-06-09 19:53:30 -07:00
parent fcc5ddfe18 1dddb350b9
commit db10ec0ff1
2 changed files with 79 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/MarkerDetector.cpp
+2 -2
View File
@@ -551,7 +551,7 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
} }
if(idsAdded.find(det->id)!=idsAdded.end()) if(idsAdded.find(det->id)!=idsAdded.end())
{ {
UWARN("Marker %d already added by another camera, ignoring detection from camera %d", det->id, cameraIndex); UDEBUG("Marker %d already added by another camera, ignoring detection from camera %d", det->id, cameraIndex);
continue; continue;
} }
@@ -668,7 +668,7 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
} }
if(idsAdded.find(id) != idsAdded.end()) if(idsAdded.find(id) != idsAdded.end())
{ {
UWARN("Marker %d already added by another camera, ignoring detection from camera %d", id, cameraIndex); UDEBUG("Marker %d already added by another camera, ignoring detection from camera %d", id, cameraIndex);
continue; continue;
} }
corelib/src/optimizer/OptimizerTORO.cpp
+77 -15
View File
@@ -380,7 +380,7 @@ bool OptimizerTORO::saveGraph(
if(file) if(file)
{ {
for (std::map<int, Transform>::const_iterator iter = poses.begin(); iter != poses.end(); ++iter) for (std::map<int, Transform>::const_iterator iter = poses.lower_bound(0); iter != poses.end(); ++iter)
{ {
if (isSlam2d()) if (isSlam2d())
{ {
@@ -409,7 +409,7 @@ bool OptimizerTORO::saveGraph(
for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{ {
if (iter->second.type() != Link::kPosePrior && iter->second.type() != Link::kGravity) if (iter->second.from() != iter->second.to() && iter->second.type() != Link::kLandmark)
{ {
if (isSlam2d()) if (isSlam2d())
{ {
@@ -494,9 +494,31 @@ bool OptimizerTORO::loadGraph(
while ( fgets (line , 400 , file) != NULL ) while ( fgets (line , 400 , file) != NULL )
{ {
std::vector<std::string> strList = uListToVector(uSplit(uReplaceChar(line, '\n', ' '), ' ')); std::vector<std::string> strList = uListToVector(uSplit(uReplaceChar(line, '\n', ' '), ' '));
if(strList.size() == 8) if(strList.empty())
{ {
//VERTEX3 continue;
}
const std::string & tag = strList[0];
if(tag.compare("VERTEX2") == 0 && strList.size() == 5)
{
//VERTEX2 id x y theta
int id = atoi(strList[1].c_str());
float x = uStr2Float(strList[2]);
float y = uStr2Float(strList[3]);
float theta = uStr2Float(strList[4]);
Transform pose(x, y, theta);
if(poses.find(id) == poses.end())
{
poses.insert(std::make_pair(id, pose));
}
else
{
UFATAL("Pose %d already added", id);
}
}
else if(tag.compare("VERTEX3") == 0 && strList.size() == 8)
{
//VERTEX3 id x y z roll pitch yaw
int id = atoi(strList[1].c_str()); int id = atoi(strList[1].c_str());
float x = uStr2Float(strList[2]); float x = uStr2Float(strList[2]);
float y = uStr2Float(strList[3]); float y = uStr2Float(strList[3]);
@@ -514,9 +536,44 @@ bool OptimizerTORO::loadGraph(
UFATAL("Pose %d already added", id); UFATAL("Pose %d already added", id);
} }
} }
else if(strList.size() == 30) else if(tag.compare("EDGE2") == 0 && strList.size() == 12)
{ {
//EDGE3 //EDGE2 observed_vertex_id observing_vertex_id x y theta inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
int idFrom = atoi(strList[1].c_str());
int idTo = atoi(strList[2].c_str());
float x = uStr2Float(strList[3]);
float y = uStr2Float(strList[4]);
float theta = uStr2Float(strList[5]);
cv::Mat informationMatrix = cv::Mat::eye(6,6,CV_64FC1);
informationMatrix.at<double>(0,0) = uStr2Float(strList[6]); // x-x
informationMatrix.at<double>(0,1) = uStr2Float(strList[7]); // x-y
informationMatrix.at<double>(0,5) = uStr2Float(strList[8]); // x-theta
informationMatrix.at<double>(1,1) = uStr2Float(strList[9]); // y-y
informationMatrix.at<double>(1,5) = uStr2Float(strList[10]); // y-theta
informationMatrix.at<double>(5,5) = uStr2Float(strList[11]); // theta-theta
// symmetric counterparts
informationMatrix.at<double>(1,0) = informationMatrix.at<double>(0,1);
informationMatrix.at<double>(5,0) = informationMatrix.at<double>(0,5);
informationMatrix.at<double>(5,1) = informationMatrix.at<double>(1,5);
informationMatrix.at<double>(2,2) = 0.00010001; // 9999 cov
informationMatrix.at<double>(3,3) = 0.00010001; // 9999 cov
informationMatrix.at<double>(4,4) = 0.00010001; // 9999 cov
UASSERT_MSG(informationMatrix.at<double>(0,0) > 0.0 && informationMatrix.at<double>(1,1) > 0.0 && informationMatrix.at<double>(5,5) > 0.0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
Transform transform(x, y, theta);
if(poses.find(idFrom) != poses.end() && poses.find(idTo) != poses.end())
{
//Link type is unknown
Link link(idFrom, idTo, Link::kUndef, transform, informationMatrix);
edgeConstraints.insert(std::pair<int, Link>(idFrom, link));
}
else
{
UERROR("Referred poses from the link (%d->%d) don't exist! Link ignored!", idFrom, idTo);
}
}
else if(tag.compare("EDGE3") == 0 && strList.size() == 30)
{
//EDGE3 observed_vertex_id observing_vertex_id x y z roll pitch yaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
int idFrom = atoi(strList[1].c_str()); int idFrom = atoi(strList[1].c_str());
int idTo = atoi(strList[2].c_str()); int idTo = atoi(strList[2].c_str());
float x = uStr2Float(strList[3]); float x = uStr2Float(strList[3]);
@@ -525,15 +582,20 @@ bool OptimizerTORO::loadGraph(
float roll = uStr2Float(strList[6]); float roll = uStr2Float(strList[6]);
float pitch = uStr2Float(strList[7]); float pitch = uStr2Float(strList[7]);
float yaw = uStr2Float(strList[8]); float yaw = uStr2Float(strList[8]);
// upper triangle is stored row by row (same order as saveGraph)
cv::Mat informationMatrix(6,6,CV_64FC1); cv::Mat informationMatrix(6,6,CV_64FC1);
informationMatrix.at<double>(3,3) = uStr2Float(strList[9]); int index = 9;
informationMatrix.at<double>(4,4) = uStr2Float(strList[15]); for(int i=0; i<6; ++i)
informationMatrix.at<double>(5,5) = uStr2Float(strList[20]); {
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()); for(int j=i; j<6; ++j)
informationMatrix.at<double>(0,0) = uStr2Float(strList[24]); {
informationMatrix.at<double>(1,1) = uStr2Float(strList[27]); double value = uStr2Float(strList[index++]);
informationMatrix.at<double>(2,2) = uStr2Float(strList[29]); informationMatrix.at<double>(i,j) = value;
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()); informationMatrix.at<double>(j,i) = value;
}
}
UASSERT_MSG(informationMatrix.at<double>(0,0) > 0.0 && informationMatrix.at<double>(1,1) > 0.0 && informationMatrix.at<double>(2,2) > 0.0 &&
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());
Transform transform(x, y, z, roll, pitch, yaw); Transform transform(x, y, z, roll, pitch, yaw);
if(poses.find(idFrom) != poses.end() && poses.find(idTo) != poses.end()) if(poses.find(idFrom) != poses.end() && poses.find(idTo) != poses.end())
{ {
@@ -546,7 +608,7 @@ bool OptimizerTORO::loadGraph(
UERROR("Referred poses from the link (%d->%d) don't exist! Link ignored!", idFrom, idTo); UERROR("Referred poses from the link (%d->%d) don't exist! Link ignored!", idFrom, idTo);
} }
} }
else if(strList.size()) else
{ {
UFATAL("Error parsing graph file %s on line \"%s\" (strList.size()=%d)", fileName.c_str(), line, (int)strList.size()); UFATAL("Error parsing graph file %s on line \"%s\" (strList.size()=%d)", fileName.c_str(), line, (int)strList.size());
} }