mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 09:30:25 +08:00
0.16.1: Added LaserScan class with new "format" field to distinguish easier between all kind of laser scans (XYZ, XYZRGB, XYZI, XYZNormal...)
This commit is contained in:
@@ -157,7 +157,7 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
|
||||
}
|
||||
}
|
||||
|
||||
if(!data.laserScanRaw().empty())
|
||||
if(!data.laserScanRaw().isEmpty())
|
||||
{
|
||||
showScanCheckbox_->setEnabled(true);
|
||||
if(showScanCheckbox_->isChecked())
|
||||
|
||||
@@ -141,7 +141,7 @@ void DataRecorder::addData(const rtabmap::SensorData & data, const Transform & p
|
||||
const Signature * s = memory_->getLastWorkingSignature();
|
||||
totalSizeKB_ += (int)s->sensorData().imageCompressed().total()/1000;
|
||||
totalSizeKB_ += (int)s->sensorData().depthOrRightCompressed().total()/1000;
|
||||
totalSizeKB_ += (int)s->sensorData().laserScanCompressed().total()/1000;
|
||||
totalSizeKB_ += (int)s->sensorData().laserScanCompressed().data().total()/1000;
|
||||
memory_->cleanup();
|
||||
|
||||
if(++count_ % 30)
|
||||
|
||||
@@ -1194,7 +1194,8 @@ void DatabaseViewer::exportDatabase()
|
||||
|
||||
SensorData data;
|
||||
dbDriver_->getNodeData(id, data);
|
||||
cv::Mat depth, rgb, scan, userData;
|
||||
cv::Mat depth, rgb, userData;
|
||||
LaserScan scan;
|
||||
data.uncompressDataConst(
|
||||
!dialog.isRgbExported()?0:&rgb,
|
||||
!dialog.isDepthExported()?0:&depth,
|
||||
@@ -1216,9 +1217,6 @@ void DatabaseViewer::exportDatabase()
|
||||
{
|
||||
sensorData = rtabmap::SensorData(
|
||||
scan,
|
||||
LaserScanInfo(dialog.isDepth2dExported()?data.laserScanInfo().maxPoints():0,
|
||||
dialog.isDepth2dExported()?data.laserScanInfo().maxRange():0,
|
||||
dialog.isDepth2dExported()?data.laserScanInfo().localTransform():Transform::getIdentity()),
|
||||
rgb,
|
||||
depth,
|
||||
data.cameraModels(),
|
||||
@@ -1230,9 +1228,6 @@ void DatabaseViewer::exportDatabase()
|
||||
{
|
||||
sensorData = rtabmap::SensorData(
|
||||
scan,
|
||||
LaserScanInfo(dialog.isDepth2dExported()?data.laserScanInfo().maxPoints():0,
|
||||
dialog.isDepth2dExported()?data.laserScanInfo().maxRange():0,
|
||||
dialog.isDepth2dExported()?data.laserScanInfo().localTransform():Transform::getIdentity()),
|
||||
rgb,
|
||||
depth,
|
||||
data.stereoCameraModel(),
|
||||
@@ -2065,17 +2060,10 @@ void DatabaseViewer::exportPoses(int format)
|
||||
}
|
||||
else
|
||||
{
|
||||
LaserScanInfo info;
|
||||
LaserScan info;
|
||||
if(dbDriver_->getLaserScanInfo(iter->first, info))
|
||||
{
|
||||
if(!info.localTransform().isNull())
|
||||
{
|
||||
localTransform = info.localTransform();
|
||||
}
|
||||
else
|
||||
{
|
||||
UWARN("Invalid scan info for node %d", iter->first);
|
||||
}
|
||||
localTransform = info.localTransform();
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -2380,8 +2368,8 @@ void DatabaseViewer::regenerateLocalMaps()
|
||||
|
||||
if(ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() && s.sensorData().gridCellSize() > 0.0f)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(s.sensorData().gridObstacleCellsRaw());
|
||||
*cloud+=*util3d::laserScanToPointCloudRGB(s.sensorData().gridGroundCellsRaw());
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridObstacleCellsRaw()));
|
||||
*cloud+=*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridGroundCellsRaw()));
|
||||
|
||||
if(cloud->size())
|
||||
{
|
||||
@@ -2502,8 +2490,8 @@ void DatabaseViewer::regenerateCurrentLocalMaps()
|
||||
|
||||
if(ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() && s.sensorData().gridCellSize() > 0.0f)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(s.sensorData().gridObstacleCellsRaw());
|
||||
*cloud+=*util3d::laserScanToPointCloudRGB(s.sensorData().gridGroundCellsRaw());
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridObstacleCellsRaw()));
|
||||
*cloud+=*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridGroundCellsRaw()));
|
||||
|
||||
if(cloud->size())
|
||||
{
|
||||
@@ -2600,7 +2588,7 @@ void DatabaseViewer::view3DMap()
|
||||
mapIds_,
|
||||
QMap<int, Signature>(),
|
||||
std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
|
||||
std::map<int, cv::Mat>(),
|
||||
std::map<int, LaserScan>(),
|
||||
pathDatabase_,
|
||||
ui_->parameters_toolbox->getParameters());
|
||||
}
|
||||
@@ -2643,7 +2631,7 @@ void DatabaseViewer::generate3DMap()
|
||||
mapIds_,
|
||||
QMap<int, Signature>(),
|
||||
std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
|
||||
std::map<int, cv::Mat>(),
|
||||
std::map<int, LaserScan>(),
|
||||
pathDatabase_,
|
||||
ui_->parameters_toolbox->getParameters());
|
||||
}
|
||||
@@ -3196,29 +3184,29 @@ void DatabaseViewer::update(int value,
|
||||
}
|
||||
|
||||
//add scan
|
||||
if(ui_->checkBox_showScan->isChecked() && data.laserScanRaw().cols)
|
||||
if(ui_->checkBox_showScan->isChecked() && data.laserScanRaw().size())
|
||||
{
|
||||
if(data.laserScanRaw().channels() == 7)
|
||||
if(data.laserScanRaw().hasRGB() && data.laserScanRaw().hasNormals())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scan = util3d::laserScanToPointCloudRGBNormal(data.laserScanRaw(), data.laserScanInfo().localTransform());
|
||||
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scan = util3d::laserScanToPointCloudRGBNormal(data.laserScanRaw(), data.laserScanRaw().localTransform());
|
||||
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
|
||||
{
|
||||
scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
|
||||
}
|
||||
cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
|
||||
}
|
||||
else if(data.laserScanRaw().channels() == 6)
|
||||
else if(data.laserScanRaw().hasNormals())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr scan = util3d::laserScanToPointCloudNormal(data.laserScanRaw(), data.laserScanInfo().localTransform());
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr scan = util3d::laserScanToPointCloudNormal(data.laserScanRaw(), data.laserScanRaw().localTransform());
|
||||
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
|
||||
{
|
||||
scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
|
||||
}
|
||||
cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
|
||||
}
|
||||
else if(data.laserScanRaw().channels() == 4)
|
||||
else if(data.laserScanRaw().hasRGB())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan = util3d::laserScanToPointCloudRGB(data.laserScanRaw(), data.laserScanInfo().localTransform());
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan = util3d::laserScanToPointCloudRGB(data.laserScanRaw(), data.laserScanRaw().localTransform());
|
||||
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
|
||||
{
|
||||
scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
|
||||
@@ -3227,7 +3215,7 @@ void DatabaseViewer::update(int value,
|
||||
}
|
||||
else
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw(), data.laserScanInfo().localTransform());
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw(), data.laserScanRaw().localTransform());
|
||||
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
|
||||
{
|
||||
scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
|
||||
@@ -3340,11 +3328,11 @@ void DatabaseViewer::update(int value,
|
||||
{
|
||||
// occupancy cloud
|
||||
cloudViewer_->addCloud("ground",
|
||||
util3d::laserScanToPointCloud(localMaps.begin()->second.first.first),
|
||||
util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(localMaps.begin()->second.first.first)),
|
||||
pose,
|
||||
QColor(ui_->lineEdit_groundColor->text()));
|
||||
cloudViewer_->addCloud("obstacles",
|
||||
util3d::laserScanToPointCloud(localMaps.begin()->second.first.second),
|
||||
util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(localMaps.begin()->second.first.second)),
|
||||
pose,
|
||||
QColor(ui_->lineEdit_obstacleColor->text()));
|
||||
cloudViewer_->setCloudPointSize("ground", 5);
|
||||
@@ -3353,7 +3341,7 @@ void DatabaseViewer::update(int value,
|
||||
if(ui_->checkBox_grid_empty->isChecked())
|
||||
{
|
||||
cloudViewer_->addCloud("empty_cells",
|
||||
util3d::laserScanToPointCloud(localMaps.begin()->second.second),
|
||||
util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(localMaps.begin()->second.second)),
|
||||
pose,
|
||||
QColor(ui_->lineEdit_emptyColor->text()));
|
||||
cloudViewer_->setCloudPointSize("empty_cells", 5);
|
||||
@@ -4257,17 +4245,17 @@ void DatabaseViewer::updateConstraintView(
|
||||
//create scan
|
||||
SensorData data;
|
||||
dbDriver_->getNodeData(iter->first, data);
|
||||
cv::Mat scan;
|
||||
LaserScan scan;
|
||||
data.uncompressDataConst(0, 0, &scan, 0);
|
||||
if(!scan.empty())
|
||||
if(!scan.isEmpty())
|
||||
{
|
||||
if(scan.channels() >= 5 && ui_->doubleSpinBox_voxelSize->value() == 0.0)
|
||||
if(scan.hasNormals() && ui_->doubleSpinBox_voxelSize->value() == 0.0)
|
||||
{
|
||||
*assembledNormalScans += *util3d::laserScanToPointCloudNormal(scan, iter->second*data.laserScanInfo().localTransform());
|
||||
*assembledNormalScans += *util3d::laserScanToPointCloudNormal(scan, iter->second*scan.localTransform());
|
||||
}
|
||||
else
|
||||
{
|
||||
*assembledScans += *util3d::laserScanToPointCloud(scan, iter->second*data.laserScanInfo().localTransform());
|
||||
*assembledScans += *util3d::laserScanToPointCloud(scan, iter->second*scan.localTransform());
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -4297,12 +4285,12 @@ void DatabaseViewer::updateConstraintView(
|
||||
// Added loop closure scans
|
||||
constraintsViewer_->removeCloud("scan0");
|
||||
constraintsViewer_->removeCloud("scan1");
|
||||
if(!dataFrom.laserScanRaw().empty())
|
||||
if(!dataFrom.laserScanRaw().isEmpty())
|
||||
{
|
||||
if(dataFrom.laserScanRaw().channels() >= 5)
|
||||
if(dataFrom.laserScanRaw().hasNormals())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr scan;
|
||||
scan = rtabmap::util3d::laserScanToPointCloudNormal(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform());
|
||||
scan = rtabmap::util3d::laserScanToPointCloudNormal(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
|
||||
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
|
||||
{
|
||||
scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
|
||||
@@ -4312,7 +4300,7 @@ void DatabaseViewer::updateConstraintView(
|
||||
else
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
|
||||
scan = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform());
|
||||
scan = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
|
||||
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
|
||||
{
|
||||
scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
|
||||
@@ -4320,12 +4308,12 @@ void DatabaseViewer::updateConstraintView(
|
||||
constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
|
||||
}
|
||||
}
|
||||
if(!dataTo.laserScanRaw().empty())
|
||||
if(!dataTo.laserScanRaw().isEmpty())
|
||||
{
|
||||
if(dataTo.laserScanRaw().channels() >= 5)
|
||||
if(dataTo.laserScanRaw().hasNormals())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr scan;
|
||||
scan = rtabmap::util3d::laserScanToPointCloudNormal(dataTo.laserScanRaw(), t*dataTo.laserScanInfo().localTransform());
|
||||
scan = rtabmap::util3d::laserScanToPointCloudNormal(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
|
||||
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
|
||||
{
|
||||
scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
|
||||
@@ -4335,7 +4323,7 @@ void DatabaseViewer::updateConstraintView(
|
||||
else
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
|
||||
scan = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw(), t*dataTo.laserScanInfo().localTransform());
|
||||
scan = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
|
||||
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
|
||||
{
|
||||
scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
|
||||
@@ -4676,22 +4664,22 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
|
||||
{
|
||||
if(iter->second.first.first.channels() == 4)
|
||||
{
|
||||
*groundRGB += *util3d::laserScanToPointCloudRGB(iter->second.first.first, pose);
|
||||
*groundRGB += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(iter->second.first.first), pose);
|
||||
}
|
||||
else
|
||||
{
|
||||
*groundXYZ += *util3d::laserScanToPointCloud(iter->second.first.first, iter->second.first.first.channels()==2?pose2d:pose);
|
||||
*groundXYZ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(iter->second.first.first), iter->second.first.first.channels()==2?pose2d:pose);
|
||||
}
|
||||
}
|
||||
if(!iter->second.first.second.empty())
|
||||
{
|
||||
if(iter->second.first.second.channels() == 4)
|
||||
{
|
||||
*obstaclesRGB += *util3d::laserScanToPointCloudRGB(iter->second.first.second, pose);
|
||||
*obstaclesRGB += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(iter->second.first.second), pose);
|
||||
}
|
||||
else
|
||||
{
|
||||
*obstaclesXYZ += *util3d::laserScanToPointCloud(iter->second.first.second, iter->second.first.second.channels()==2?pose2d:pose);
|
||||
*obstaclesXYZ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(iter->second.first.second), iter->second.first.second.channels()==2?pose2d:pose);
|
||||
}
|
||||
}
|
||||
if(ui_->checkBox_grid_empty->isChecked())
|
||||
@@ -4700,11 +4688,11 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
|
||||
{
|
||||
if(iter->second.second.channels() == 4)
|
||||
{
|
||||
*emptyCellsRGB += *util3d::laserScanToPointCloudRGB(iter->second.second, pose);
|
||||
*emptyCellsRGB += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(iter->second.second), pose);
|
||||
}
|
||||
else
|
||||
{
|
||||
*emptyCellsXYZ += *util3d::laserScanToPointCloud(iter->second.second, iter->second.second.channels()==2?pose2d:pose);
|
||||
*emptyCellsXYZ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(iter->second.second), iter->second.second.channels()==2?pose2d:pose);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -5365,46 +5353,35 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
|
||||
}
|
||||
|
||||
Transform toPoseInv = filteredScanPoses.at(currentLink.to()).inverse();
|
||||
cv::Mat fromScan;
|
||||
LaserScan fromScan;
|
||||
dataFrom.uncompressData(0,0,&fromScan);
|
||||
int maxPoints = fromScan.cols;
|
||||
int maxPoints = fromScan.size();
|
||||
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 = filteredScanPoses.begin(); iter!=filteredScanPoses.end(); ++iter)
|
||||
{
|
||||
if(iter->first != currentLink.from())
|
||||
{
|
||||
SensorData data;
|
||||
dbDriver_->getNodeData(iter->first, data);
|
||||
cv::Mat scan;
|
||||
if(!data.laserScanCompressed().empty())
|
||||
if(!data.laserScanCompressed().isEmpty())
|
||||
{
|
||||
cv::Mat scan;
|
||||
LaserScan scan;
|
||||
data.uncompressData(0, 0, &scan);
|
||||
if(!scan.empty())
|
||||
if(!scan.isEmpty() && fromScan.format() == scan.format())
|
||||
{
|
||||
if(scan.channels() != 2 && scan.channels() != 5)
|
||||
if(scan.hasNormals())
|
||||
{
|
||||
is2D = false;
|
||||
}
|
||||
|
||||
if(scan.channels() >= 5)
|
||||
{
|
||||
*assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(
|
||||
scan,
|
||||
toPoseInv * iter->second * data.laserScanInfo().localTransform());
|
||||
*assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(scan, toPoseInv * iter->second * scan.localTransform());
|
||||
}
|
||||
else
|
||||
{
|
||||
*assembledToClouds += *util3d::laserScanToPointCloud(
|
||||
scan,
|
||||
toPoseInv * iter->second * data.laserScanInfo().localTransform());
|
||||
*assembledToClouds += *util3d::laserScanToPointCloud(scan, toPoseInv * iter->second * scan.localTransform());
|
||||
}
|
||||
|
||||
if(scan.cols > maxPoints)
|
||||
if(scan.size() > maxPoints)
|
||||
{
|
||||
maxPoints = scan.cols;
|
||||
maxPoints = scan.size();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -5418,19 +5395,20 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
|
||||
cv::Mat assembledScan;
|
||||
if(assembledToNormalClouds->size())
|
||||
{
|
||||
assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
|
||||
assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
|
||||
}
|
||||
else if(assembledToClouds->size())
|
||||
{
|
||||
assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
|
||||
assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
|
||||
}
|
||||
SensorData assembledData;
|
||||
// scans are in base frame but for 2d scans, set the height so that correspondences matching works
|
||||
assembledData.setLaserScanRaw(assembledScan,
|
||||
LaserScanInfo(
|
||||
dataFrom.laserScanInfo().maxPoints()?dataFrom.laserScanInfo().maxPoints():maxPoints,
|
||||
dataFrom.laserScanInfo().maxRange(),
|
||||
is2D?Transform(0,0,dataFrom.laserScanInfo().localTransform().z(),0,0,0):Transform::getIdentity()));
|
||||
assembledData.setLaserScanRaw(LaserScan(
|
||||
assembledScan,
|
||||
fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
|
||||
fromScan.maxRange(),
|
||||
fromScan.format(),
|
||||
fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
|
||||
|
||||
RegistrationIcp registrationIcp(parameters);
|
||||
transform = registrationIcp.computeTransformation(dataFrom, assembledData, currentLink.transform(), &info);
|
||||
@@ -5463,10 +5441,10 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
|
||||
0,
|
||||
ui_->parameters_toolbox->getParameters());
|
||||
int maxLaserScans = cloudFrom->size();
|
||||
dataFrom.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), LaserScanInfo(maxLaserScans, 0));
|
||||
dataTo.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), LaserScanInfo(maxLaserScans, 0));
|
||||
dataFrom.setLaserScanRaw(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0, LaserScan::kXYZ));
|
||||
dataTo.setLaserScanRaw(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0, LaserScan::kXYZ));
|
||||
|
||||
if(!dataFrom.laserScanCompressed().empty() || !dataTo.laserScanCompressed().empty())
|
||||
if(!dataFrom.laserScanCompressed().isEmpty() || !dataTo.laserScanCompressed().isEmpty())
|
||||
{
|
||||
UWARN("There are laser scans in data, but generate laser scan from "
|
||||
"depth image option is activated. Ignoring saved laser scans...");
|
||||
@@ -5474,7 +5452,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
|
||||
}
|
||||
else
|
||||
{
|
||||
cv::Mat tmpA, tmpB;
|
||||
LaserScan tmpA, tmpB;
|
||||
dataFrom.uncompressData(0, 0, &tmpA);
|
||||
dataTo.uncompressData(0, 0, &tmpB);
|
||||
}
|
||||
|
||||
@@ -315,7 +315,8 @@ void DepthCalibrationDialog::calibrate(
|
||||
const Signature & s = cachedSignatures.find(iter->first).value();
|
||||
SensorData data = s.sensorData();
|
||||
|
||||
cv::Mat depth, laserScan;
|
||||
cv::Mat depth;
|
||||
LaserScan laserScan;
|
||||
data.uncompressData(0, &depth, _ui->checkBox_laserScan->isChecked()?&laserScan:0);
|
||||
if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection() && !depth.empty())
|
||||
{
|
||||
|
||||
@@ -734,7 +734,7 @@ void ExportCloudsDialog::exportClouds(
|
||||
const std::map<int, int> & mapIds,
|
||||
const QMap<int, Signature> & cachedSignatures,
|
||||
const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > & cachedClouds,
|
||||
const std::map<int, cv::Mat> & cachedScans,
|
||||
const std::map<int, LaserScan> & cachedScans,
|
||||
const QString & workingDirectory,
|
||||
const ParametersMap & parameters)
|
||||
{
|
||||
@@ -796,7 +796,7 @@ void ExportCloudsDialog::viewClouds(
|
||||
const std::map<int, int> & mapIds,
|
||||
const QMap<int, Signature> & cachedSignatures,
|
||||
const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > & cachedClouds,
|
||||
const std::map<int, cv::Mat> & cachedScans,
|
||||
const std::map<int, LaserScan> & cachedScans,
|
||||
const QString & workingDirectory,
|
||||
const ParametersMap & parameters)
|
||||
{
|
||||
@@ -1099,7 +1099,7 @@ bool ExportCloudsDialog::getExportedClouds(
|
||||
const std::map<int, int> & mapIds,
|
||||
const QMap<int, Signature> & cachedSignatures,
|
||||
const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > & cachedClouds,
|
||||
const std::map<int, cv::Mat> & cachedScans,
|
||||
const std::map<int, LaserScan> & cachedScans,
|
||||
const QString & workingDirectory,
|
||||
const ParametersMap & parameters,
|
||||
std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & cloudsWithNormals,
|
||||
@@ -1297,20 +1297,27 @@ bool ExportCloudsDialog::getExportedClouds(
|
||||
}
|
||||
else
|
||||
{
|
||||
LaserScanInfo info;
|
||||
if(cachedSignatures.contains(iter->first))
|
||||
if(uContains(cachedScans, iter->first))
|
||||
{
|
||||
iter->second *= cachedScans.at(iter->first).localTransform();
|
||||
}
|
||||
else if(cachedSignatures.contains(iter->first))
|
||||
{
|
||||
const SensorData & data = cachedSignatures.find(iter->first)->sensorData();
|
||||
info = data.laserScanInfo();
|
||||
if(!data.laserScanCompressed().isEmpty())
|
||||
{
|
||||
iter->second *= data.laserScanCompressed().localTransform();
|
||||
}
|
||||
else if(!data.laserScanRaw().isEmpty())
|
||||
{
|
||||
iter->second *= data.laserScanRaw().localTransform();
|
||||
}
|
||||
}
|
||||
else if(_dbDriver)
|
||||
{
|
||||
_dbDriver->getLaserScanInfo(iter->first, info);
|
||||
}
|
||||
|
||||
if(!info.localTransform().isNull())
|
||||
{
|
||||
iter->second *= info.localTransform();
|
||||
LaserScan scan;
|
||||
_dbDriver->getLaserScanInfo(iter->first, scan);
|
||||
iter->second *= scan.localTransform();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -2407,7 +2414,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
|
||||
const std::map<int, Transform> & poses,
|
||||
const QMap<int, Signature> & cachedSignatures,
|
||||
const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > & cachedClouds,
|
||||
const std::map<int, cv::Mat> & cachedScans,
|
||||
const std::map<int, LaserScan> & cachedScans,
|
||||
const ParametersMap & parameters,
|
||||
bool & has2dScans) const
|
||||
{
|
||||
@@ -2429,7 +2436,8 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
|
||||
if(_ui->checkBox_regenerate->isChecked())
|
||||
{
|
||||
SensorData data;
|
||||
cv::Mat image, depth, scan;
|
||||
cv::Mat image, depth;
|
||||
LaserScan scan;
|
||||
if(cachedSignatures.contains(iter->first))
|
||||
{
|
||||
const Signature & s = cachedSignatures.find(iter->first).value();
|
||||
@@ -2558,19 +2566,17 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(!_ui->checkBox_fromDepth->isChecked() && !scan.empty())
|
||||
else if(!_ui->checkBox_fromDepth->isChecked() && !scan.isEmpty())
|
||||
{
|
||||
bool is2D = scan.channels() == 2;
|
||||
bool is2D = scan.is2d();
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithoutNormals;
|
||||
localTransform = Transform::getIdentity();
|
||||
Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
|
||||
if(!data.laserScanInfo().localTransform().isNull())
|
||||
{
|
||||
localTransform = data.laserScanInfo().localTransform();
|
||||
viewPoint[0] = localTransform.x();
|
||||
viewPoint[1] = localTransform.y();
|
||||
viewPoint[2] = localTransform.z();
|
||||
}
|
||||
localTransform = scan.localTransform();
|
||||
viewPoint[0] = localTransform.x();
|
||||
viewPoint[1] = localTransform.y();
|
||||
viewPoint[2] = localTransform.z();
|
||||
|
||||
cloudWithoutNormals = util3d::laserScanToPointCloudRGB(scan, localTransform); // put in base frame by default
|
||||
if(cloudWithoutNormals->size())
|
||||
{
|
||||
@@ -2686,30 +2692,15 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
|
||||
else if(!_ui->checkBox_fromDepth->isChecked() && uContains(cachedScans, iter->first))
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithoutNormals;
|
||||
localTransform = Transform::getIdentity();
|
||||
|
||||
localTransform = cachedScans.at(iter->first).localTransform();
|
||||
Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
|
||||
viewPoint[0] = localTransform.x();
|
||||
viewPoint[1] = localTransform.y();
|
||||
viewPoint[2] = localTransform.z();
|
||||
|
||||
LaserScanInfo info;
|
||||
if(cachedSignatures.contains(iter->first))
|
||||
{
|
||||
const Signature & s = cachedSignatures.find(iter->first).value();
|
||||
info = s.sensorData().laserScanInfo();
|
||||
}
|
||||
else if(_dbDriver)
|
||||
{
|
||||
_dbDriver->getLaserScanInfo(iter->first, info);
|
||||
}
|
||||
|
||||
if(!info.localTransform().isNull())
|
||||
{
|
||||
viewPoint[0] = localTransform.x();
|
||||
viewPoint[1] = localTransform.y();
|
||||
viewPoint[2] = localTransform.z();
|
||||
localTransform = info.localTransform().inverse();
|
||||
}
|
||||
|
||||
bool is2D = cachedScans.at(iter->first).channels() == 2;
|
||||
cloudWithoutNormals = util3d::laserScanToPointCloudRGB(cachedScans.at(iter->first)); // already in base frame
|
||||
bool is2D = cachedScans.at(iter->first).is2d();
|
||||
cloudWithoutNormals = util3d::laserScanToPointCloudRGB(cachedScans.at(iter->first), cachedScans.at(iter->first).localTransform());
|
||||
if(cloudWithoutNormals->size())
|
||||
{
|
||||
if(_ui->doubleSpinBox_voxelSize_assembled->value()>0.0)
|
||||
|
||||
@@ -1109,12 +1109,12 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
|
||||
if(_preferencesDialog->isScansShown(1))
|
||||
{
|
||||
// F2M: scan local map
|
||||
if(!odom.info().localScanMap.empty())
|
||||
if(!odom.info().localScanMap.isEmpty())
|
||||
{
|
||||
if(!lost)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
|
||||
cloud = util3d::laserScanToPointCloudNormal(odom.info().localScanMap);
|
||||
cloud = util3d::laserScanToPointCloudNormal(odom.info().localScanMap, odom.info().localScanMap.localTransform());
|
||||
if(!_cloudViewer->addCloud("scanMapOdom", cloud, _odometryCorrection, Qt::blue))
|
||||
{
|
||||
UERROR("Adding scanMapOdom to viewer failed!");
|
||||
@@ -1129,9 +1129,9 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
|
||||
scanUpdated = true;
|
||||
}
|
||||
// scan cloud
|
||||
if(!odom.data().laserScanRaw().empty())
|
||||
if(!odom.data().laserScanRaw().isEmpty())
|
||||
{
|
||||
cv::Mat scan = odom.data().laserScanRaw();
|
||||
LaserScan scan = odom.data().laserScanRaw();
|
||||
|
||||
if(_preferencesDialog->getDownsamplingStepScan(1) > 0)
|
||||
{
|
||||
@@ -1139,7 +1139,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
|
||||
}
|
||||
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
|
||||
cloud = util3d::laserScanToPointCloudNormal(scan, pose*odom.data().laserScanInfo().localTransform());
|
||||
cloud = util3d::laserScanToPointCloudNormal(scan, pose*scan.localTransform());
|
||||
if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
|
||||
{
|
||||
cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
|
||||
@@ -1953,7 +1953,13 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
|
||||
s.sensorData().setImageRaw(cv::Mat());
|
||||
s.sensorData().setDepthOrRightRaw(cv::Mat());
|
||||
s.sensorData().setUserDataRaw(cv::Mat());
|
||||
s.sensorData().setLaserScanRaw(cv::Mat(), signature.sensorData().laserScanInfo());
|
||||
s.sensorData().setLaserScanRaw(
|
||||
LaserScan(
|
||||
cv::Mat(),
|
||||
signature.sensorData().laserScanRaw().maxPoints(),
|
||||
signature.sensorData().laserScanRaw().maxRange(),
|
||||
signature.sensorData().laserScanRaw().format(),
|
||||
signature.sensorData().laserScanRaw().localTransform()));
|
||||
s.sensorData().clearOccupancyGridRaw();
|
||||
_cachedMemoryUsage += s.sensorData().getMemoryUsed();
|
||||
}
|
||||
@@ -2210,7 +2216,7 @@ void MainWindow::updateMapCloud(
|
||||
else if(_cachedSignatures.contains(iter->first))
|
||||
{
|
||||
QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
|
||||
if(!jter->sensorData().laserScanCompressed().empty() || !jter->sensorData().laserScanRaw().empty())
|
||||
if(!jter->sensorData().laserScanCompressed().isEmpty() || !jter->sensorData().laserScanRaw().isEmpty())
|
||||
{
|
||||
this->createAndAddScanToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
|
||||
}
|
||||
@@ -3043,9 +3049,9 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
|
||||
return;
|
||||
}
|
||||
|
||||
if(!iter->sensorData().laserScanCompressed().empty() || !iter->sensorData().laserScanRaw().empty())
|
||||
if(!iter->sensorData().laserScanCompressed().isEmpty() || !iter->sensorData().laserScanRaw().isEmpty())
|
||||
{
|
||||
cv::Mat scan;
|
||||
LaserScan scan;
|
||||
iter->sensorData().uncompressData(0, 0, &scan);
|
||||
|
||||
if(_preferencesDialog->getDownsamplingStepScan(0) > 0)
|
||||
@@ -3057,21 +3063,21 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals;
|
||||
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudRGBWithNormals;
|
||||
if(scan.channels() == 7 && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
|
||||
if(scan.hasNormals() && scan.hasRGB() && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
|
||||
{
|
||||
cloudRGBWithNormals = util3d::laserScanToPointCloudRGBNormal(scan, iter->sensorData().laserScanInfo().localTransform());
|
||||
cloudRGBWithNormals = util3d::laserScanToPointCloudRGBNormal(scan, scan.localTransform());
|
||||
}
|
||||
else if((scan.channels() == 5 || scan.channels() == 6) && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
|
||||
else if((scan.hasNormals()) && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
|
||||
{
|
||||
cloudWithNormals = util3d::laserScanToPointCloudNormal(scan, iter->sensorData().laserScanInfo().localTransform());
|
||||
cloudWithNormals = util3d::laserScanToPointCloudNormal(scan, scan.localTransform());
|
||||
}
|
||||
else if(scan.channels() == 4)
|
||||
else if(scan.hasRGB())
|
||||
{
|
||||
cloudRGB = util3d::laserScanToPointCloudRGB(scan, iter->sensorData().laserScanInfo().localTransform());
|
||||
cloudRGB = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
|
||||
}
|
||||
else
|
||||
{
|
||||
cloud = util3d::laserScanToPointCloud(scan, iter->sensorData().laserScanInfo().localTransform());
|
||||
cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
|
||||
}
|
||||
|
||||
if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
|
||||
@@ -3087,7 +3093,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
|
||||
}
|
||||
|
||||
// Do ceiling/floor filtering
|
||||
if((scan.channels() > 2 && scan.channels() != 5) && // don't filter 2D scans
|
||||
if((!scan.is2d()) && // don't filter 2D scans
|
||||
(_preferencesDialog->getScanFloorFilteringHeight() != 0.0 ||
|
||||
_preferencesDialog->getScanCeilingFilteringHeight() != 0.0))
|
||||
{
|
||||
@@ -3149,14 +3155,14 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
|
||||
(_preferencesDialog->getScanNormalKSearch() > 0 || _preferencesDialog->getScanNormalRadiusSearch() > 0.0))
|
||||
{
|
||||
Eigen::Vector3f scanViewpoint(
|
||||
iter->sensorData().laserScanInfo().localTransform().x(),
|
||||
iter->sensorData().laserScanInfo().localTransform().y(),
|
||||
iter->sensorData().laserScanInfo().localTransform().z());
|
||||
scan.localTransform().x(),
|
||||
scan.localTransform().y(),
|
||||
scan.localTransform().z());
|
||||
|
||||
pcl::PointCloud<pcl::Normal>::Ptr normals;
|
||||
if(cloud.get() && cloud->size())
|
||||
{
|
||||
if(scan.channels() == 2 || scan.channels() == 5)
|
||||
if(scan.is2d())
|
||||
{
|
||||
normals = util3d::computeFastOrganizedNormals2D(cloud, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
|
||||
}
|
||||
@@ -3189,7 +3195,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
|
||||
added = _cloudViewer->addCloud(scanName, cloudRGBWithNormals, pose, color);
|
||||
if(added && nodeId > 0)
|
||||
{
|
||||
scan = util3d::laserScanFromPointCloud(*cloudRGBWithNormals);
|
||||
scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGBWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZRGBNormal, scan.localTransform());
|
||||
}
|
||||
}
|
||||
else if(cloudWithNormals.get())
|
||||
@@ -3197,13 +3203,13 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
|
||||
added = _cloudViewer->addCloud(scanName, cloudWithNormals, pose, color);
|
||||
if(added && nodeId > 0)
|
||||
{
|
||||
if(scan.channels() == 5)
|
||||
if(scan.is2d())
|
||||
{
|
||||
scan = util3d::laserScan2dFromPointCloud(*cloudWithNormals);
|
||||
scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloudWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYNormal, scan.localTransform());
|
||||
}
|
||||
else
|
||||
{
|
||||
scan = util3d::laserScanFromPointCloud(*cloudWithNormals);
|
||||
scan = LaserScan(util3d::laserScanFromPointCloud(*cloudWithNormals, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZNormal, scan.localTransform());
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -3212,7 +3218,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
|
||||
added = _cloudViewer->addCloud(scanName, cloudRGB, pose, color);
|
||||
if(added && nodeId > 0)
|
||||
{
|
||||
scan = util3d::laserScanFromPointCloud(*cloudRGB);
|
||||
scan = LaserScan(util3d::laserScanFromPointCloud(*cloudRGB, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZRGB, scan.localTransform());
|
||||
}
|
||||
}
|
||||
else
|
||||
@@ -3221,13 +3227,13 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
|
||||
added = _cloudViewer->addCloud(scanName, cloud, pose, color);
|
||||
if(added && nodeId > 0)
|
||||
{
|
||||
if(scan.channels() == 2)
|
||||
if(scan.is2d())
|
||||
{
|
||||
scan = util3d::laserScan2dFromPointCloud(*cloud);
|
||||
scan = LaserScan(util3d::laserScan2dFromPointCloud(*cloud, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXY, scan.localTransform());
|
||||
}
|
||||
else
|
||||
{
|
||||
scan = util3d::laserScanFromPointCloud(*cloud);
|
||||
scan = LaserScan(util3d::laserScanFromPointCloud(*cloud, scan.localTransform().inverse()), scan.maxPoints(), scan.maxRange(), LaserScan::kXYZ, scan.localTransform());
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -3239,7 +3245,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
|
||||
{
|
||||
if(nodeId > 0)
|
||||
{
|
||||
_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
|
||||
_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in scan frame
|
||||
}
|
||||
|
||||
_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
|
||||
@@ -4955,9 +4961,13 @@ void MainWindow::exportPoses(int format)
|
||||
}
|
||||
else
|
||||
{
|
||||
if(!_cachedSignatures[iter->first].sensorData().laserScanInfo().localTransform().isNull())
|
||||
if(!_cachedSignatures[iter->first].sensorData().laserScanRaw().localTransform().isNull())
|
||||
{
|
||||
localTransform = _cachedSignatures[iter->first].sensorData().laserScanInfo().localTransform();
|
||||
localTransform = _cachedSignatures[iter->first].sensorData().laserScanRaw().localTransform();
|
||||
}
|
||||
else if(!_cachedSignatures[iter->first].sensorData().laserScanCompressed().localTransform().isNull())
|
||||
{
|
||||
localTransform = _cachedSignatures[iter->first].sensorData().laserScanCompressed().localTransform();
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -5489,12 +5499,12 @@ void MainWindow::postProcessing()
|
||||
Signature & signatureFrom = _cachedSignatures[from];
|
||||
Signature & signatureTo = _cachedSignatures[to];
|
||||
|
||||
cv::Mat tmp;
|
||||
LaserScan tmp;
|
||||
signatureFrom.sensorData().uncompressData(0,0,&tmp);
|
||||
signatureTo.sensorData().uncompressData(0,0,&tmp);
|
||||
|
||||
if(!signatureFrom.sensorData().laserScanRaw().empty() &&
|
||||
!signatureTo.sensorData().laserScanRaw().empty())
|
||||
if(!signatureFrom.sensorData().laserScanRaw().isEmpty() &&
|
||||
!signatureTo.sensorData().laserScanRaw().isEmpty())
|
||||
{
|
||||
RegistrationInfo info;
|
||||
Transform transform = regIcp.computeTransformation(signatureFrom.sensorData(), signatureTo.sensorData(), iter->second.transform(), &info);
|
||||
|
||||
@@ -302,10 +302,10 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
|
||||
if(scanShown_->isChecked())
|
||||
{
|
||||
// scan local map
|
||||
if(!odom.info().localScanMap.empty())
|
||||
if(!odom.info().localScanMap.isEmpty())
|
||||
{
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
|
||||
cloud = util3d::laserScanToPointCloudNormal(odom.info().localScanMap);
|
||||
cloud = util3d::laserScanToPointCloudNormal(odom.info().localScanMap, odom.info().localScanMap.localTransform());
|
||||
if(!cloudView_->addCloud("scanMapOdom", cloud, Transform::getIdentity(), Qt::blue))
|
||||
{
|
||||
UERROR("Adding scanMapOdom to viewer failed!");
|
||||
@@ -317,12 +317,12 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
|
||||
}
|
||||
}
|
||||
// scan cloud
|
||||
if(!odom.data().laserScanRaw().empty())
|
||||
if(!odom.data().laserScanRaw().isEmpty())
|
||||
{
|
||||
cv::Mat scan = odom.data().laserScanRaw();
|
||||
LaserScan scan = odom.data().laserScanRaw();
|
||||
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
|
||||
cloud = util3d::laserScanToPointCloudNormal(scan, odom.pose());
|
||||
cloud = util3d::laserScanToPointCloudNormal(scan, odom.pose() * scan.localTransform());
|
||||
|
||||
if(!cloudView_->addCloud("scanOdom", cloud, Transform::getIdentity(), Qt::magenta))
|
||||
{
|
||||
|
||||
@@ -7,7 +7,7 @@
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>976</width>
|
||||
<height>747</height>
|
||||
<height>739</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="sizePolicy">
|
||||
@@ -63,25 +63,16 @@
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>0</y>
|
||||
<width>678</width>
|
||||
<height>2811</height>
|
||||
<y>-1377</y>
|
||||
<width>673</width>
|
||||
<height>2834</height>
|
||||
</rect>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_16">
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
@@ -95,7 +86,7 @@
|
||||
<enum>QFrame::Raised</enum>
|
||||
</property>
|
||||
<property name="currentIndex">
|
||||
<number>16</number>
|
||||
<number>5</number>
|
||||
</property>
|
||||
<widget class="QWidget" name="page_22">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
|
||||
@@ -2592,7 +2583,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
|
||||
<item row="3" column="0">
|
||||
<widget class="QLineEdit" name="lineEdit_sourceLocalTransform">
|
||||
<property name="toolTip">
|
||||
<string><html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /gray_camera = 0 0 1 -1 0 0 0 -1 0<br/>KITTI: /base_link to /color_camera = 0 0 1 0 -1 0 0 -0.06 0 -1 0 0</p><p>EuRoC MAV: /base_link to /cam0 = [0 0 1 0; 0 -1 0 0; 1 0 0 0; 0 0 0 1]*T_BS</p></body></html></string>
|
||||
<string><html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /gray_camera = 0 0 1 -1 0 0 0 -1 0<br/>KITTI: /base_link to /color_camera = 0 0 1 0 -1 0 0 -0.06 0 -1 0 0<br/>KITTI: /base_footprint to /gray_camera = 0 0 1 0 -1 0 0 0 0 -1 0 1.67<br/>KITTI: /base_footprint to /gray_camera = 0 0 1 0 -1 0 0 -0.06 0 -1 0 1.67</p><p>EuRoC MAV: /base_link to /cam0 = [0 0 1 0; 0 -1 0 0; 1 0 0 0; 0 0 0 1]*T_BS</p></body></html></string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>0 0 1 -1 0 0 0 -1 0</string>
|
||||
@@ -4722,16 +4713,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
|
||||
<string>Directory of images (optional settings)</string>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_93">
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
@@ -4976,7 +4958,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
|
||||
<item row="10" column="1">
|
||||
<widget class="QLineEdit" name="lineEdit_cameraImages_laser_transform">
|
||||
<property name="toolTip">
|
||||
<string><html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /scan = -0.27 0 0.08 0 0 0</p></body></html></string>
|
||||
<string><html><head/><body><p>Format (3 values): x y z<br/>Format (6 values): x y z roll pitch yaw<br/>Format (7 values): x y z qx qy qz qw<br/>Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33<br/>Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz</p><p>KITTI: /base_link to /scan = -0.27 0 0.08 0 0 0<br/>KITTI: /base_footprint to /scan = -0.27 0 1.75 0 0 0</p></body></html></string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>0 0 0 0 0 0</string>
|
||||
@@ -13579,16 +13561,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
@@ -13668,16 +13641,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
@@ -13789,16 +13753,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare
|
||||
<property name="spacing">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="leftMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="topMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="rightMargin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="bottomMargin">
|
||||
<property name="margin">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<item>
|
||||
|
||||
Reference in New Issue
Block a user