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

DbViewer: fixed reversed "Generate laser scan from depth" option

Browse Source
This commit is contained in:
matlabbe
2016-01-10 12:57:49 -05:00
parent 75f85f6b2a
commit 182fe5f1d2
3 changed files with 19 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
corelib/src/Memory.cpp
View File

@@ -3284,15 +3284,15 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
rtabmap::CompressionThread ctImage(image, std::string(".jpg"));
rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
rtabmap::CompressionThread ctDepth2d(laserScan);
rtabmap::CompressionThread ctLaserScan(laserScan);
rtabmap::CompressionThread ctUserData(data.userDataRaw());
ctImage.start();
ctDepth.start();
ctDepth2d.start();
ctLaserScan.start();
ctUserData.start();
ctImage.join();
ctDepth.join();
ctDepth2d.join();
ctLaserScan.join();
ctUserData.join();
s = new Signature(id,
@@ -3304,7 +3304,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
data.groundTruth(),
stereoCameraModel.isValid()?
SensorData(
ctDepth2d.getCompressedData(),
ctLaserScan.getCompressedData(),
maxLaserScanMaxPts,
data.laserScanMaxRange(),
ctImage.getCompressedData(),
@@ -3314,7 +3314,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
0,
ctUserData.getCompressedData()):
SensorData(
ctDepth2d.getCompressedData(),
ctLaserScan.getCompressedData(),
maxLaserScanMaxPts,
data.laserScanMaxRange(),
ctImage.getCompressedData(),

11
corelib/src/RegistrationIcp.cpp
View File

@@ -101,7 +101,11 @@ Transform RegistrationIcp::computeTransformationImpl(
SensorData & dataFrom = fromSignature.sensorData();
SensorData & dataTo = toSignature.sensorData();
UDEBUG("size from=%d to=%d", dataFrom.laserScanRaw().cols, dataTo.laserScanRaw().cols);
UDEBUG("size from=%d (channels=%d) to=%d (channels=%d)",
dataFrom.laserScanRaw().cols,
dataFrom.laserScanRaw().channels(),
dataTo.laserScanRaw().cols,
dataTo.laserScanRaw().channels());
// ICP with guess transform
if(!dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
@@ -117,9 +121,6 @@ Transform RegistrationIcp::computeTransformationImpl(
UDEBUG("Downsampling time (step=%d) = %f s", _downsamplingStep, timer.ticks());
}
UDEBUG("Conversion time = %f s", timer.ticks());
if(fromScan.cols && toScan.cols)
{
Transform icpT;
@@ -137,6 +138,7 @@ Transform RegistrationIcp::computeTransformationImpl(
//special case if we have already normals computed and there is no filtering
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, Transform());
pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess);
UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
icpT = util3d::icpPointToPlane(
fromCloudNormals,
@@ -159,6 +161,7 @@ Transform RegistrationIcp::computeTransformationImpl(
{
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, Transform());
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess);
UDEBUG("Conversion time = %f s", timer.ticks());
pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
bool filtered = false;