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

fixed multi scans matching with normals, removed some debug logs

Browse Source
This commit is contained in:
matlabbe
2017-08-28 16:15:22 -04:00
parent 423b47a5ff
commit b70ffb6331
8 changed files with 135 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/Graph.cpp
View File

@@ -1603,7 +1603,7 @@ int findNearestNode(
kdTree->nearestKSearch(pt, 1, ind, dist);
if(ind.size() && dist.size() && ind[0] >= 0)
{
UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]);
//UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]);
id = ids[ind[0]];
}
}

67
corelib/src/Memory.cpp
View File

@@ -2406,6 +2406,18 @@ Transform Memory::computeIcpTransformMulti(
UASSERT(uContains(poses, toId) && uContains(_signatures, toId));
UDEBUG("Guess=%s", (poses.at(fromId).inverse() * poses.at(toId)).prettyPrint().c_str());
if(ULogger::level() == ULogger::kDebug)
{
std::string ids;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first != fromId)
{
ids += uNumber2Str(iter->first) + " ";
}
}
UDEBUG("%d vs %s", fromId, ids.c_str());
}
// make sure that all laser scans are loaded
std::list<Signature*> depthToLoad;
@@ -2438,6 +2450,7 @@ Transform Memory::computeIcpTransformMulti(
std::string msg;
int maxPoints = fromScan.cols;
pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
bool is2D = true;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
@@ -2454,14 +2467,26 @@ Transform Memory::computeIcpTransformMulti(
{
is2D = false;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(
scan,
s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
if(scan.channels() >= 5)
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormal = util3d::laserScanToPointCloudNormal(
scan,
s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
*assembledToNormalClouds += *cloudNormal;
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(
scan,
s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
*assembledToClouds += *cloud;
}
if(scan.cols > maxPoints)
{
maxPoints = scan.cols;
}
*assembledToClouds += *cloud;
}
}
else
@@ -2470,28 +2495,22 @@ Transform Memory::computeIcpTransformMulti(
}
}
}
if(assembledToClouds->size())
{
if(is2D)
{
assembledData.setLaserScanRaw(
util3d::laserScan2dFromPointCloud(*assembledToClouds),
LaserScanInfo(
fromS->sensorData().laserScanInfo().maxPoints()?fromS->sensorData().laserScanInfo().maxPoints():maxPoints,
fromS->sensorData().laserScanInfo().maxRange(),
Transform::getIdentity())); // scans are in base frame
}
else
{
assembledData.setLaserScanRaw(
util3d::laserScanFromPointCloud(*assembledToClouds),
LaserScanInfo(
fromS->sensorData().laserScanInfo().maxPoints()?fromS->sensorData().laserScanInfo().maxPoints():maxPoints,
fromS->sensorData().laserScanInfo().maxRange(),
Transform::getIdentity())); // scans are in base frame
}
cv::Mat assembledScan;
if(assembledToNormalClouds->size())
{
assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
}
else if(assembledToClouds->size())
{
assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
}
// scans are in base frame but for 2d scans, set the height so that correspondences matching works
assembledData.setLaserScanRaw(assembledScan,
LaserScanInfo(
fromS->sensorData().laserScanInfo().maxPoints()?fromS->sensorData().laserScanInfo().maxPoints():maxPoints,
fromS->sensorData().laserScanInfo().maxRange(),
is2D?Transform(0,0,fromS->sensorData().laserScanInfo().localTransform().z(),0,0,0):Transform::getIdentity()));
Transform guess = poses.at(fromId).inverse() * poses.at(toId);
std::vector<int> inliersV;

6
corelib/src/RegistrationIcp.cpp
View File

@@ -425,8 +425,8 @@ Transform RegistrationIcp::computeTransformationImpl(
if( _pointToPlane &&
_voxelSize == 0.0f &&
fromScan.channels() >= 6 &&
toScan.channels() >= 6 &&
fromScan.channels() >= 5 &&
toScan.channels() >= 5 &&
!((fromScan.channels() == 5 || toScan.channels() == 5) && !_libpointmatcher)) // PCL crashes if 2D)
{
//special case if we have already normals computed and there is no filtering
@@ -454,8 +454,8 @@ Transform RegistrationIcp::computeTransformationImpl(
PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
T = icp(data, ref);
UDEBUG("libpointmatcher icp...done!");
icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
UDEBUG("libpointmatcher icp...done! T=%s", icpT.prettyPrint().c_str());
float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
UDEBUG("match ratio: %f", matchRatio);

16
corelib/src/Rtabmap.cpp
View File

@@ -1228,9 +1228,7 @@ bool Rtabmap::process(
//============================================================
if(_proximityByTime &&
rehearsedId == 0 && // don't do it if rehearsal happened
signature->getWords3().size() &&
_memory->isIncremental() && // don't do it in localization mode
!signature->isBadSignature() &&
signature->getWeight()>=0)
{
const std::set<int> & stm = _memory->getStMem();
@@ -1511,7 +1509,7 @@ bool Rtabmap::process(
++immunizedGlobally;
}
UDEBUG("nt=%d m=%d immunized=1", iter->first, iter->second);
//UDEBUG("nt=%d m=%d immunized=1", iter->first, iter->second);
}
neighbors.erase(iter++);
}
@@ -1557,7 +1555,7 @@ bool Rtabmap::process(
{
++nbDirectNeighborsInDb;
}
UDEBUG("nt=%d m=%d", iter->first, iter->second);
//UDEBUG("nt=%d m=%d", iter->first, iter->second);
}
neighbors.erase(iter++);
}
@@ -1698,7 +1696,7 @@ bool Rtabmap::process(
{
++immunizedLocally;
}
UDEBUG("local node %d on path immunized=1", iter->first);
//UDEBUG("local node %d on path immunized=1", iter->first);
}
}
}
@@ -1745,7 +1743,7 @@ bool Rtabmap::process(
{
++immunizedLocally;
}
UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first);
//UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first);
}
}
}
@@ -2006,7 +2004,7 @@ bool Rtabmap::process(
//find the nearest pose on the path
int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
UASSERT(nearestId > 0);
UDEBUG("Path %d (size=%d) distance=%fm", nearestId, (int)path.size(), _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)));
//UDEBUG("Path %d (size=%d) distance=%fm", nearestId, (int)path.size(), _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)));
// nearest pose must be close and not linked to current location
if(!signature->hasLink(nearestId) &&
@@ -2101,7 +2099,7 @@ bool Rtabmap::process(
}
else
{
UDEBUG("Path %d ignored", nearestId);
//UDEBUG("Path %d ignored", nearestId);
}
}
}
@@ -2996,7 +2994,7 @@ std::map<int, std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transfo
if(valid)
{
UDEBUG("%d <- %d", nearestId, jter->first);
//UDEBUG("%d <- %d", nearestId, jter->first);
path.insert(*jter);
poses.erase(jter);
}