i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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 ctImage(image, std::string(".jpg"));
rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png")); rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
rtabmap::CompressionThread ctDepth2d(laserScan); rtabmap::CompressionThread ctLaserScan(laserScan);
rtabmap::CompressionThread ctUserData(data.userDataRaw()); rtabmap::CompressionThread ctUserData(data.userDataRaw());
ctImage.start(); ctImage.start();
ctDepth.start(); ctDepth.start();
ctDepth2d.start(); ctLaserScan.start();
ctUserData.start(); ctUserData.start();
ctImage.join(); ctImage.join();
ctDepth.join(); ctDepth.join();
ctDepth2d.join(); ctLaserScan.join();
ctUserData.join(); ctUserData.join();
s = new Signature(id, s = new Signature(id,
@@ -3304,7 +3304,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
data.groundTruth(), data.groundTruth(),
stereoCameraModel.isValid()? stereoCameraModel.isValid()?
SensorData( SensorData(
ctDepth2d.getCompressedData(), ctLaserScan.getCompressedData(),
maxLaserScanMaxPts, maxLaserScanMaxPts,
data.laserScanMaxRange(), data.laserScanMaxRange(),
ctImage.getCompressedData(), ctImage.getCompressedData(),
@@ -3314,7 +3314,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
0, 0,
ctUserData.getCompressedData()): ctUserData.getCompressedData()):
SensorData( SensorData(
ctDepth2d.getCompressedData(), ctLaserScan.getCompressedData(),
maxLaserScanMaxPts, maxLaserScanMaxPts,
data.laserScanMaxRange(), data.laserScanMaxRange(),
ctImage.getCompressedData(), ctImage.getCompressedData(),

11
corelib/src/RegistrationIcp.cpp
View File

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

8
guilib/src/DatabaseViewer.cpp
View File

@@ -3582,7 +3582,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
ParametersMap parameters = ui_->parameters_toolbox->getParameters(); ParametersMap parameters = ui_->parameters_toolbox->getParameters();
UTimer timer; UTimer timer;
if(!ui_->checkBox_icp_laserScan->isChecked()) if(ui_->checkBox_icp_laserScan->isChecked())
{ {
// generate laser scans from depth image // generate laser scans from depth image
cv::Mat tmpA, tmpB, tmpC, tmpD; cv::Mat tmpA, tmpB, tmpC, tmpD;
@@ -3599,6 +3599,12 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
int maxLaserScans = cloudFrom->size(); int maxLaserScans = cloudFrom->size();
dataFrom.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0); dataFrom.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0);
dataTo.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0); dataTo.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0);
if(!dataFrom.laserScanCompressed().empty() || !dataTo.laserScanCompressed().empty())
{
UWARN("There are laser scans in data, but generate laser scan from "
"depth image option is activated. Ignoring saved laser scans...");
}
} }
else else
{ {