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 voxelize assert when voxel size is set and there are no laser scans in database

Browse Source
This commit is contained in:
matlabbe
2017-08-04 10:47:25 -04:00
parent 459bbda60c
commit d8a6ed4ba6
2 changed files with 49 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
corelib/src/util3d_filtering.cpp
View File

@@ -142,7 +142,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
float voxelSize) float voxelSize)
{ {
UASSERT(voxelSize > 0.0f); UASSERT(voxelSize > 0.0f);
UASSERT((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size())); UASSERT_MSG((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()),
uFormat("Cloud size=%d indices=%d is_dense=%s", (int)cloud->size(), (int)indices->size(), cloud->is_dense?"true":"false").c_str());
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
pcl::VoxelGrid<pcl::PointXYZ> filter; pcl::VoxelGrid<pcl::PointXYZ> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize); filter.setLeafSize(voxelSize, voxelSize, voxelSize);
@@ -160,7 +161,8 @@ pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(
float voxelSize) float voxelSize)
{ {
UASSERT(voxelSize > 0.0f); UASSERT(voxelSize > 0.0f);
UASSERT((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size())); UASSERT_MSG((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()),
uFormat("Cloud size=%d indices=%d is_dense=%s", (int)cloud->size(), (int)indices->size(), cloud->is_dense?"true":"false").c_str());
pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>); pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
pcl::VoxelGrid<pcl::PointNormal> filter; pcl::VoxelGrid<pcl::PointNormal> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize); filter.setLeafSize(voxelSize, voxelSize, voxelSize);
@@ -178,7 +180,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
float voxelSize) float voxelSize)
{ {
UASSERT(voxelSize > 0.0f); UASSERT(voxelSize > 0.0f);
UASSERT((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size())); UASSERT_MSG((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()),
uFormat("Cloud size=%d indices=%d is_dense=%s", (int)cloud->size(), (int)indices->size(), cloud->is_dense?"true":"false").c_str());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::VoxelGrid<pcl::PointXYZRGB> filter; pcl::VoxelGrid<pcl::PointXYZRGB> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize); filter.setLeafSize(voxelSize, voxelSize, voxelSize);
@@ -196,7 +199,8 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(
float voxelSize) float voxelSize)
{ {
UASSERT(voxelSize > 0.0f); UASSERT(voxelSize > 0.0f);
UASSERT((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size())); UASSERT_MSG((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()),
uFormat("Cloud size=%d indices=%d is_dense=%s", (int)cloud->size(), (int)indices->size(), cloud->is_dense?"true":"false").c_str());
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>); pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::VoxelGrid<pcl::PointXYZRGBNormal> filter; pcl::VoxelGrid<pcl::PointXYZRGBNormal> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize); filter.setLeafSize(voxelSize, voxelSize, voxelSize);

76
guilib/src/DatabaseViewer.cpp
View File

@@ -3502,15 +3502,15 @@ void DatabaseViewer::updateConstraintView(
cloudTo=util3d::cloudRGBFromSensorData(dataTo, 1, 0, 0, indicesTo.get(), ui_->parameters_toolbox->getParameters()); cloudTo=util3d::cloudRGBFromSensorData(dataTo, 1, 0, 0, indicesTo.get(), ui_->parameters_toolbox->getParameters());
} }
if(cloudTo.get() && cloudTo->size()) if(cloudTo.get() && indicesTo->size())
{ {
cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t); cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
} }
// Gain compensation // Gain compensation
if(ui_->doubleSpinBox_gainCompensationRadius->value()>0.0 && if(ui_->doubleSpinBox_gainCompensationRadius->value()>0.0 &&
cloudFrom.get() && cloudFrom->size() && cloudFrom.get() && indicesFrom->size() &&
cloudTo.get() && cloudTo->size()) cloudTo.get() && indicesTo->size())
{ {
UTimer t; UTimer t;
GainCompensator compensator(ui_->doubleSpinBox_gainCompensationRadius->value()); GainCompensator compensator(ui_->doubleSpinBox_gainCompensationRadius->value());
@@ -3520,7 +3520,7 @@ void DatabaseViewer::updateConstraintView(
UINFO("Gain compensation time = %fs", t.ticks()); UINFO("Gain compensation time = %fs", t.ticks());
} }
if(cloudFrom.get() && cloudFrom->size()) if(cloudFrom.get() && indicesFrom->size())
{ {
if(ui_->doubleSpinBox_voxelSize->value() > 0.0) if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
{ {
@@ -3528,7 +3528,7 @@ void DatabaseViewer::updateConstraintView(
} }
constraintsViewer_->addCloud("cloud0", cloudFrom, pose, Qt::red); constraintsViewer_->addCloud("cloud0", cloudFrom, pose, Qt::red);
} }
if(cloudTo.get() && cloudTo->size()) if(cloudTo.get() && indicesTo->size())
{ {
if(ui_->doubleSpinBox_voxelSize->value() > 0.0) if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
{ {
@@ -3755,45 +3755,51 @@ void DatabaseViewer::updateConstraintView(
// Added loop closure scans // Added loop closure scans
constraintsViewer_->removeCloud("scan0"); constraintsViewer_->removeCloud("scan0");
constraintsViewer_->removeCloud("scan1"); constraintsViewer_->removeCloud("scan1");
if(dataFrom.laserScanRaw().channels() == 6) if(!dataFrom.laserScanRaw().empty())
{ {
pcl::PointCloud<pcl::PointNormal>::Ptr scan; if(dataFrom.laserScanRaw().channels() == 6)
scan = rtabmap::util3d::laserScanToPointCloudNormal(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform());
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
{ {
scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value()); pcl::PointCloud<pcl::PointNormal>::Ptr scan;
scan = rtabmap::util3d::laserScanToPointCloudNormal(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform());
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
{
scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
}
constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
scan = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform());
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
{
scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
}
constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
} }
constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
} }
else if(!dataTo.laserScanRaw().empty())
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr scan; if(dataTo.laserScanRaw().channels() == 6)
scan = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform());
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
{ {
scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value()); pcl::PointCloud<pcl::PointNormal>::Ptr scan;
scan = rtabmap::util3d::laserScanToPointCloudNormal(dataTo.laserScanRaw(), t*dataTo.laserScanInfo().localTransform());
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
{
scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
}
constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
} }
constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow); else
}
if(dataTo.laserScanRaw().channels() == 6)
{
pcl::PointCloud<pcl::PointNormal>::Ptr scan;
scan = rtabmap::util3d::laserScanToPointCloudNormal(dataTo.laserScanRaw(), t*dataTo.laserScanInfo().localTransform());
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
{ {
scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value()); pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
scan = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw(), t*dataTo.laserScanInfo().localTransform());
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
{
scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
}
constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
} }
constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
scan = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw(), t*dataTo.laserScanInfo().localTransform());
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
{
scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
}
constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
} }
} }