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

DatabaseViewer: Added ExportCloudsDialog, added Occupancy Grid View

Browse Source
This commit is contained in:
matlabbe
2017-03-21 21:51:39 -04:00
parent 622352b411
commit 2574f3a8ee
8 changed files with 892 additions and 522 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
corelib/src/OctoMap.cpp
View File

@@ -456,8 +456,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
(*cloud)[oi].g = g*255.0f;
(*cloud)[oi].b = b*255.0f;
}
(*cloud)[oi].x = pt.x();
(*cloud)[oi].y = pt.y();
(*cloud)[oi].x = pt.x()-octree_->getResolution()/2.0;
(*cloud)[oi].y = pt.y()-octree_->getResolution()/2.0;
(*cloud)[oi].z = pt.z();
if(obstacleIndices)
{
@@ -469,8 +469,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
{
octomap::point3d pt = octree_->keyToCoord(it.getKey());
(*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
(*cloud)[oi].x = pt.x();
(*cloud)[oi].y = pt.y();
(*cloud)[oi].x = pt.x()-octree_->getResolution()/2.0f;
(*cloud)[oi].y = pt.y()-octree_->getResolution()/2.0f;
(*cloud)[oi].z = pt.z();
if(emptyIndices)
{
@@ -501,23 +501,21 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
pcl::PointCloud<pcl::PointXYZ>::Ptr ground(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles(new pcl::PointCloud<pcl::PointXYZ>);
ground->resize(occupiedCells_.size());
obstacles->resize(occupiedCells_.size());
ground->resize(octree_->size());
obstacles->resize(octree_->size());
int gi=0;
int oi=0;
for(std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo>::const_iterator iter = occupiedCells_.begin();
iter!=occupiedCells_.end();
++iter)
for (octomap::ColorOcTree::iterator it = octree_->begin(octree_->getTreeDepth()); it != octree_->end(); ++it)
{
if(iter->second.isObstacle_ && octree_->isNodeOccupied(iter->first))
if(octree_->isNodeOccupied(*it))
{
octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
(*obstacles)[oi++] = pcl::PointXYZ(pt.x(), pt.y(), 0); // projected on ground
octomap::point3d pt = octree_->keyToCoord(it.getKey());
(*obstacles)[oi++] = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
}
else if(!iter->second.isObstacle_)
else
{
octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
(*ground)[gi++] = pcl::PointXYZ(pt.x(), pt.y(), 0); // projected on ground
octomap::point3d pt = octree_->keyToCoord(it.getKey());
(*ground)[gi++] = pcl::PointXYZ(pt.x()-gridCellSize/2.0f, pt.y()-gridCellSize/2.0f, 0); // projected on ground
}
}
obstacles->resize(oi);
@@ -525,11 +523,11 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
if(obstacles->size())
{
obstacles = util3d::voxelize(obstacles, gridCellSize);
obstacles = util3d::voxelize(obstacles, gridCellSize/2.0f);
}
if(ground->size())
{
ground = util3d::voxelize(ground, gridCellSize);
ground = util3d::voxelize(ground, gridCellSize/2.0f);
}
cv::Mat obstaclesMat = cv::Mat(1, (int)obstacles->size(), CV_32FC2);

20
corelib/src/util3d_mapping.cpp
View File

@@ -308,7 +308,9 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
//Get map size
float margin = cellSize*10.0f;
xMin = minX-margin;
xMin -= cellSize/2.0f;
yMin = minY-margin;
yMin += cellSize/2.0f;
float xMax = maxX+margin;
float yMax = maxY+margin;
if(fabs((yMax - yMin) / cellSize) > 99999 ||
@@ -323,7 +325,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax);
map = cv::Mat::ones((yMax - yMin) / cellSize + 0.5f, (xMax - xMin) / cellSize + 0.5f, CV_8S)*-1;
map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
for(std::list<std::pair<int, Transform> >::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
{
std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
@@ -333,7 +335,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
for(int i=0; i<iter->second.cols; ++i)
{
float * ptf = iter->second.ptr<float>(0, i);
cv::Point2i pt((ptf[0]-xMin)/cellSize + 0.5f, (ptf[1]-yMin)/cellSize + 0.5f);
cv::Point2i pt((ptf[0]-xMin)/cellSize, (ptf[1]-yMin)/cellSize);
char & value = map.at<char>(pt.y, pt.x);
if(value != -2)
{
@@ -345,8 +347,8 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
if(footprintRadius >= cellSize*1.5f)
{
// place free space under the footprint of the robot
cv::Point2i ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize + 0.5f, (kter->second.y()-footprintRadius-yMin)/cellSize + 0.5f);
cv::Point2i ptEnd((kter->second.x()+footprintRadius-xMin)/cellSize + 0.5f, (kter->second.y()+footprintRadius-yMin)/cellSize + 0.5f);
cv::Point2i ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize, (kter->second.y()-footprintRadius-yMin)/cellSize);
cv::Point2i ptEnd((kter->second.x()+footprintRadius-xMin)/cellSize, (kter->second.y()+footprintRadius-yMin)/cellSize);
if(ptBegin.x < 0)
ptBegin.x = 0;
if(ptEnd.x >= map.cols)
@@ -370,7 +372,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
for(int i=0; i<jter->second.cols; ++i)
{
float * ptf = jter->second.ptr<float>(0, i);
cv::Point2i pt((ptf[0]-xMin)/cellSize + 0.5f, (ptf[1]-yMin)/cellSize + 0.5f);
cv::Point2i pt((ptf[0]-xMin)/cellSize, (ptf[1]-yMin)/cellSize);
char & value = map.at<char>(pt.y, pt.x);
if(value != -2)
{
@@ -595,7 +597,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
UTimer timer;
map = cv::Mat::ones((yMax - yMin) / cellSize + 0.5f, (xMax - xMin) / cellSize + 0.5f, CV_8S)*-1;
map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
int j=0;
for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
{
@@ -606,7 +608,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
{
viewpoint = kter->second;
}
cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize + 0.5f, ((pose.y()+viewpoint.y)-yMin)/cellSize + 0.5f);
cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
for(unsigned int i=0; i<iter->second->size(); ++i)
{
cv::Point2i end((iter->second->points[i].x-xMin)/cellSize, (iter->second->points[i].y-yMin)/cellSize);
@@ -638,7 +640,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
{
viewpoint = kter->second;
}
cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize + 0.5f, ((pose.y()+viewpoint.y)-yMin)/cellSize + 0.5f);
cv::Point2i start(((pose.x()+viewpoint.x)-xMin)/cellSize, ((pose.y()+viewpoint.y)-yMin)/cellSize);
//UWARN("maxLength = %f", maxLength);
//rotate counterclockwise from the first point until we pass the last point
@@ -680,7 +682,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
angle = angle<-1.0f?-1.0f:angle>1.0f?1.0f:angle;
while(acos(angle) > M_PI_4 || endRotatedVector.cross(endLastVector).at<float>(2) > 0.0f)
{
cv::Point2i end((endRotated.at<float>(0)-xMin)/cellSize + 0.5f, (endRotated.at<float>(1)-yMin)/cellSize + 0.5f);
cv::Point2i end((endRotated.at<float>(0)-xMin)/cellSize, (endRotated.at<float>(1)-yMin)/cellSize);
//end must be inside the grid
end.x = end.x < 0?0:end.x;
end.x = end.x >= map.cols?map.cols-1:end.x;

7
guilib/include/rtabmap/gui/DatabaseViewer.h
View File

@@ -54,6 +54,8 @@ class DBDriver;
class ImageView;
class SensorData;
class CloudViewer;
class OctoMap;
class ExportCloudsDialog;
class RTABMAPGUI_EXP DatabaseViewer : public QMainWindow
{
@@ -102,6 +104,7 @@ private slots:
void sliderLoopValueChanged(int);
void sliderIterationsValueChanged(int);
void updateGrid();
void updateOctomapView();
void updateGraphView();
void refineConstraint();
void addConstraint();
@@ -159,6 +162,7 @@ private:
CloudViewer * cloudViewerA_;
CloudViewer * cloudViewerB_;
CloudViewer * stereoViewer_;
CloudViewer * occupancyGridViewer_;
QList<int> ids_;
std::map<int, int> mapIds_;
QMap<int, int> idToIndex_;
@@ -176,8 +180,11 @@ private:
std::multimap<int, rtabmap::Link> linksAdded_;
std::multimap<int, rtabmap::Link> linksRemoved_;
std::map<int, std::pair<cv::Mat, cv::Mat> > localMaps_; // <ground, obstacles>
std::map<int, std::pair<float, cv::Point3f> > localMapsInfo_; // <cell size, viewpoint>
std::map<int, std::pair<cv::Mat, cv::Mat> > generatedLocalMaps_; // <ground, obstacles>
std::map<int, std::pair<float, cv::Point3f> > generatedLocalMapsInfo_; // <cell size, viewpoint>
OctoMap * octomap_;
ExportCloudsDialog * exportDialog_;
bool savedMaximized_;
bool firstCall_;

12
guilib/src/CloudViewer.cpp
View File

@@ -753,11 +753,15 @@ bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth)
pcl::IndicesPtr obstacles(new std::vector<int>);
if(treeDepth > octomap->octree()->getTreeDepth())
if(treeDepth == 0 || treeDepth > octomap->octree()->getTreeDepth())
{
UWARN("Tree depth requested (%d) is deeper than the "
"actual maximum tree depth of %d. Using maximum depth.",
(int)treeDepth, (int)octomap->octree()->getTreeDepth());
if(treeDepth>0)
{
UWARN("Tree depth requested (%d) is deeper than the "
"actual maximum tree depth of %d. Using maximum depth.",
(int)treeDepth, (int)octomap->octree()->getTreeDepth());
}
treeDepth = octomap->octree()->getTreeDepth();
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(treeDepth, obstacles.get());

713
guilib/src/DatabaseViewer.cpp
View File

File diff suppressed because it is too large Load Diff

504
guilib/src/ExportCloudsDialog.cpp
View File

@@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Graph.h"
#include "rtabmap/core/GainCompensator.h"
#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
#include "rtabmap/core/DBDriver.h"
#include <pcl/conversions.h>
#include <pcl/io/pcd_io.h>
@@ -65,7 +66,8 @@ namespace rtabmap {
ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
QDialog(parent),
_canceled(false),
_compensator(0)
_compensator(0),
_dbDriver(0)
{
_ui = new Ui_ExportCloudsDialog();
_ui->setupUi(this);
@@ -392,9 +394,9 @@ void ExportCloudsDialog::restoreDefaults()
{
_ui->comboBox_pipeline->setCurrentIndex(1);
_ui->checkBox_binary->setChecked(true);
_ui->spinBox_normalKSearch->setValue(10);
_ui->spinBox_normalKSearch->setValue(20);
_ui->checkBox_regenerate->setChecked(false);
_ui->checkBox_regenerate->setChecked(_dbDriver!=0?true:false);
_ui->spinBox_decimation->setValue(1);
_ui->doubleSpinBox_maxDepth->setValue(4);
_ui->doubleSpinBox_minDepth->setValue(0);
@@ -749,8 +751,17 @@ void ExportCloudsDialog::viewClouds(
uIsInteger(mesh->tex_materials[0].tex_file, false))
{
int textureId = uStr2Int(mesh->tex_materials[0].tex_file);
UASSERT(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty());
cachedSignatures.value(textureId).sensorData().uncompressDataConst(&globalTexture, 0);
SensorData data;
if(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty())
{
data = cachedSignatures.value(textureId).sensorData();
}
else if(_dbDriver)
{
_dbDriver->getNodeData(textureId, data, true, false, false, false);
}
UASSERT(!data.imageCompressed().empty());
data.uncompressDataConst(&globalTexture, 0);
UASSERT(!globalTexture.empty());
if (_ui->checkBox_gainCompensation->isChecked() && _compensator && _compensator->getIndex(textureId) >= 0)
{
@@ -864,7 +875,11 @@ bool ExportCloudsDialog::getExportedClouds(
{
_canceled = false;
_workingDirectory = workingDirectory;
enableRegeneration(cachedSignatures.size());
enableRegeneration(_dbDriver || cachedSignatures.size());
if(cachedSignatures.empty() && _dbDriver)
{
_ui->checkBox_regenerate->setChecked(true);
}
if(_compensator)
{
delete _compensator;
@@ -1073,17 +1088,26 @@ bool ExportCloudsDialog::getExportedClouds(
// Adjust view points with local transforms
for(std::map<int, Transform>::iterator iter= viewPoints.begin(); iter!=viewPoints.end(); ++iter)
{
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
if(cachedSignatures.contains(iter->first))
{
const SensorData & data = cachedSignatures.find(iter->first)->sensorData();
if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
{
iter->second *= data.cameraModels()[0].localTransform();
}
else if(!data.stereoCameraModel().localTransform().isNull())
{
iter->second *= data.stereoCameraModel().localTransform();
}
models = data.cameraModels();
stereoModel = data.stereoCameraModel();
}
else if(_dbDriver)
{
_dbDriver->getCalibration(iter->first, models, stereoModel);
}
if(models.size() && !models[0].localTransform().isNull())
{
iter->second *= models[0].localTransform();
}
else if(!stereoModel.localTransform().isNull())
{
iter->second *= stereoModel.localTransform();
}
}
}
@@ -1194,22 +1218,33 @@ bool ExportCloudsDialog::getExportedClouds(
if(iter->second->size())
{
Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
if(cachedSignatures.contains(iter->first))
{
const SensorData & data = cachedSignatures.find(iter->first)->sensorData();
if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
{
viewpoint[0] = data.cameraModels()[0].localTransform().x();
viewpoint[1] = data.cameraModels()[0].localTransform().y();
viewpoint[2] = data.cameraModels()[0].localTransform().z();
}
else if(!data.stereoCameraModel().localTransform().isNull())
{
viewpoint[0] = data.stereoCameraModel().localTransform().x();
viewpoint[1] = data.stereoCameraModel().localTransform().y();
viewpoint[2] = data.stereoCameraModel().localTransform().z();
}
models = data.cameraModels();
stereoModel = data.stereoCameraModel();
}
else if(_dbDriver)
{
_dbDriver->getCalibration(iter->first, models, stereoModel);
}
if(models.size() && !models[0].localTransform().isNull())
{
viewpoint[0] = models[0].localTransform().x();
viewpoint[1] = models[0].localTransform().y();
viewpoint[2] = models[0].localTransform().z();
}
else if(!stereoModel.localTransform().isNull())
{
viewpoint[0] = stereoModel.localTransform().x();
viewpoint[1] = stereoModel.localTransform().y();
viewpoint[2] = stereoModel.localTransform().z();
}
std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
iter->second,
_ui->doubleSpinBox_mesh_angleTolerance->value()*M_PI/180.0,
@@ -1766,29 +1801,52 @@ bool ExportCloudsDialog::getExportedClouds(
std::map<int, CameraModel> cameraModels;
for(std::map<int, Transform>::iterator jter=cameras.begin(); jter!=cameras.end(); ++jter)
{
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
bool cacheHasCompressedImage = false;
if(cachedSignatures.contains(jter->first))
{
const Signature & s = cachedSignatures.value(jter->first);
CameraModel model;
if(s.sensorData().stereoCameraModel().isValidForProjection())
{
model = s.sensorData().stereoCameraModel().left();
}
else if(s.sensorData().cameraModels().size() == 1 && s.sensorData().cameraModels()[0].isValidForProjection())
{
model = s.sensorData().cameraModels()[0];
}
if(!jter->second.isNull() && model.isValidForProjection() && !s.sensorData().imageCompressed().empty())
{
if(model.imageWidth() == 0 || model.imageHeight() == 0)
{
// we are using an old database format (image size not saved in calibrations), we should
// uncompress images to get their size
cv::Mat img;
s.sensorData().uncompressDataConst(&img, 0);
model.setImageSize(img.size());
}
const SensorData & data = cachedSignatures.find(jter->first)->sensorData();
models = data.cameraModels();
stereoModel = data.stereoCameraModel();
cacheHasCompressedImage = !data.imageCompressed().empty();
}
else if(_dbDriver)
{
_dbDriver->getCalibration(jter->first, models, stereoModel);
}
CameraModel model;
if(stereoModel.isValidForProjection())
{
model = stereoModel.left();
}
else if(models.size() == 1 && models[0].isValidForProjection())
{
model = models[0];
}
if(!jter->second.isNull() && model.isValidForProjection())
{
if(model.imageWidth() == 0 || model.imageHeight() == 0)
{
// we are using an old database format (image size not saved in calibrations), we should
// uncompress images to get their size
cv::Mat img;
if(cacheHasCompressedImage)
{
cachedSignatures.find(jter->first)->sensorData().uncompressDataConst(&img, 0);
}
else if(_dbDriver)
{
SensorData data;
_dbDriver->getNodeData(jter->first, data, true, false, false, false);
data.uncompressDataConst(&img, 0);
}
model.setImageSize(img.size());
}
if(model.imageWidth() != 0 && model.imageHeight() != 0)
{
cameraPoses.insert(std::make_pair(jter->first, jter->second));
cameraModels.insert(std::make_pair(jter->first, model));
}
@@ -2096,118 +2154,125 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
pcl::IndicesPtr indices(new std::vector<int>);
if(_ui->checkBox_regenerate->isChecked())
{
SensorData data;
cv::Mat image, depth;
if(cachedSignatures.contains(iter->first))
{
const Signature & s = cachedSignatures.find(iter->first).value();
SensorData d = s.sensorData();
cv::Mat image, depth;
d.uncompressData(&image, &depth, 0);
if(!image.empty() && !depth.empty())
data = s.sensorData();
data.uncompressData(&image, &depth, 0);
}
else if(_dbDriver)
{
_dbDriver->getNodeData(iter->first, data, true, false, false, false);
data.uncompressData(&image, &depth, 0);
}
if(!image.empty() && !depth.empty())
{
if(_ui->spinBox_fillDepthHoles->value() > 0)
{
if(_ui->spinBox_fillDepthHoles->value() > 0)
{
depth = util2d::fillDepthHoles(depth, _ui->spinBox_fillDepthHoles->value(), float(_ui->spinBox_fillDepthHolesError->value())/100.f);
}
depth = util2d::fillDepthHoles(depth, _ui->spinBox_fillDepthHoles->value(), float(_ui->spinBox_fillDepthHolesError->value())/100.f);
}
if(!_ui->lineEdit_distortionModel->text().isEmpty() &&
QFileInfo(_ui->lineEdit_distortionModel->text()).exists())
{
clams::DiscreteDepthDistortionModel model;
model.load(_ui->lineEdit_distortionModel->text().toStdString());
depth = depth.clone();// make sure we are not modifying data in cached signatures.
model.undistort(depth);
d.setDepthOrRightRaw(depth);
}
if(!_ui->lineEdit_distortionModel->text().isEmpty() &&
QFileInfo(_ui->lineEdit_distortionModel->text()).exists())
{
clams::DiscreteDepthDistortionModel model;
model.load(_ui->lineEdit_distortionModel->text().toStdString());
depth = depth.clone();// make sure we are not modifying data in cached signatures.
model.undistort(depth);
data.setDepthOrRightRaw(depth);
}
// bilateral filtering
if(_ui->checkBox_bilateral->isChecked())
{
depth = util2d::fastBilateralFiltering(depth,
_ui->doubleSpinBox_bilateral_sigmaS->value(),
_ui->doubleSpinBox_bilateral_sigmaR->value());
d.setDepthOrRightRaw(depth);
}
// bilateral filtering
if(_ui->checkBox_bilateral->isChecked())
{
depth = util2d::fastBilateralFiltering(depth,
_ui->doubleSpinBox_bilateral_sigmaS->value(),
_ui->doubleSpinBox_bilateral_sigmaR->value());
data.setDepthOrRightRaw(depth);
}
UASSERT(iter->first == d.id());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithoutNormals;
std::vector<float> roiRatios;
if(!_ui->lineEdit_roiRatios->text().isEmpty())
UASSERT(iter->first == data.id());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudWithoutNormals;
std::vector<float> roiRatios;
if(!_ui->lineEdit_roiRatios->text().isEmpty())
{
QStringList values = _ui->lineEdit_roiRatios->text().split(' ');
if(values.size() == 4)
{
QStringList values = _ui->lineEdit_roiRatios->text().split(' ');
if(values.size() == 4)
roiRatios.resize(4);
for(int i=0; i<values.size(); ++i)
{
roiRatios.resize(4);
for(int i=0; i<values.size(); ++i)
{
roiRatios[i] = uStr2Float(values[i].toStdString().c_str());
}
roiRatios[i] = uStr2Float(values[i].toStdString().c_str());
}
}
cloudWithoutNormals = util3d::cloudRGBFromSensorData(
d,
_ui->spinBox_decimation->value() == 0?1:_ui->spinBox_decimation->value(),
_ui->doubleSpinBox_maxDepth->value(),
_ui->doubleSpinBox_minDepth->value(),
indices.get(),
parameters,
roiRatios);
}
cloudWithoutNormals = util3d::cloudRGBFromSensorData(
data,
_ui->spinBox_decimation->value() == 0?1:_ui->spinBox_decimation->value(),
_ui->doubleSpinBox_maxDepth->value(),
_ui->doubleSpinBox_minDepth->value(),
indices.get(),
parameters,
roiRatios);
if(cloudWithoutNormals->size())
if(cloudWithoutNormals->size())
{
// Don't voxelize if we create organized mesh
if(!(_ui->comboBox_pipeline->currentIndex()==0 && _ui->checkBox_meshing->isChecked()) && _ui->doubleSpinBox_voxelSize_assembled->value()>0.0)
{
// Don't voxelize if we create organized mesh
if(!(_ui->comboBox_pipeline->currentIndex()==0 && _ui->checkBox_meshing->isChecked()) && _ui->doubleSpinBox_voxelSize_assembled->value()>0.0)
cloudWithoutNormals = util3d::voxelize(cloudWithoutNormals, indices, _ui->doubleSpinBox_voxelSize_assembled->value());
indices->resize(cloudWithoutNormals->size());
for(unsigned int i=0; i<indices->size(); ++i)
{
cloudWithoutNormals = util3d::voxelize(cloudWithoutNormals, indices, _ui->doubleSpinBox_voxelSize_assembled->value());
indices->resize(cloudWithoutNormals->size());
for(unsigned int i=0; i<indices->size(); ++i)
{
indices->at(i) = i;
}
indices->at(i) = i;
}
}
// view point
Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
if(d.cameraModels().size() && !d.cameraModels()[0].localTransform().isNull())
{
viewPoint[0] = d.cameraModels()[0].localTransform().x();
viewPoint[1] = d.cameraModels()[0].localTransform().y();
viewPoint[2] = d.cameraModels()[0].localTransform().z();
}
else if(!d.stereoCameraModel().localTransform().isNull())
{
viewPoint[0] = d.stereoCameraModel().localTransform().x();
viewPoint[1] = d.stereoCameraModel().localTransform().y();
viewPoint[2] = d.stereoCameraModel().localTransform().z();
}
// view point
Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
{
viewPoint[0] = data.cameraModels()[0].localTransform().x();
viewPoint[1] = data.cameraModels()[0].localTransform().y();
viewPoint[2] = data.cameraModels()[0].localTransform().z();
}
else if(!data.stereoCameraModel().localTransform().isNull())
{
viewPoint[0] = data.stereoCameraModel().localTransform().x();
viewPoint[1] = data.stereoCameraModel().localTransform().y();
viewPoint[2] = data.stereoCameraModel().localTransform().z();
}
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), viewPoint);
pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud);
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), viewPoint);
pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud);
if(_ui->checkBox_subtraction->isChecked() &&
_ui->doubleSpinBox_subtractPointFilteringRadius->value() > 0.0)
if(_ui->checkBox_subtraction->isChecked() &&
_ui->doubleSpinBox_subtractPointFilteringRadius->value() > 0.0)
{
pcl::IndicesPtr beforeSubtractionIndices = indices;
if( cloud->size() &&
previousCloud.get() != 0 &&
previousIndices.get() != 0 &&
previousIndices->size() &&
!previousPose.isNull())
{
pcl::IndicesPtr beforeSubtractionIndices = indices;
if( cloud->size() &&
previousCloud.get() != 0 &&
previousIndices.get() != 0 &&
previousIndices->size() &&
!previousPose.isNull())
{
rtabmap::Transform t = iter->second.inverse() * previousPose;
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(previousCloud, t);
indices = rtabmap::util3d::subtractFiltering(
cloud,
indices,
transformedCloud,
previousIndices,
_ui->doubleSpinBox_subtractPointFilteringRadius->value(),
_ui->doubleSpinBox_subtractPointFilteringAngle->value(),
_ui->spinBox_subtractFilteringMinPts->value());
}
previousCloud = cloud;
previousIndices = beforeSubtractionIndices;
previousPose = iter->second;
rtabmap::Transform t = iter->second.inverse() * previousPose;
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(previousCloud, t);
indices = rtabmap::util3d::subtractFiltering(
cloud,
indices,
transformedCloud,
previousIndices,
_ui->doubleSpinBox_subtractPointFilteringRadius->value(),
_ui->doubleSpinBox_subtractPointFilteringAngle->value(),
_ui->spinBox_subtractFilteringMinPts->value());
}
previousCloud = cloud;
previousIndices = beforeSubtractionIndices;
previousPose = iter->second;
}
}
}
@@ -2242,22 +2307,30 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
// view point
Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
if(cachedSignatures.contains(iter->first))
{
const Signature & s = cachedSignatures.find(iter->first).value();
SensorData d = s.sensorData();
if(d.cameraModels().size() && !d.cameraModels()[0].localTransform().isNull())
{
viewPoint[0] = d.cameraModels()[0].localTransform().x();
viewPoint[1] = d.cameraModels()[0].localTransform().y();
viewPoint[2] = d.cameraModels()[0].localTransform().z();
}
else if(!d.stereoCameraModel().localTransform().isNull())
{
viewPoint[0] = d.stereoCameraModel().localTransform().x();
viewPoint[1] = d.stereoCameraModel().localTransform().y();
viewPoint[2] = d.stereoCameraModel().localTransform().z();
}
models = s.sensorData().cameraModels();
stereoModel = s.sensorData().stereoCameraModel();
}
else if(_dbDriver)
{
_dbDriver->getCalibration(iter->first, models, stereoModel);
}
if(models.size() && !models[0].localTransform().isNull())
{
viewPoint[0] = models[0].localTransform().x();
viewPoint[1] = models[0].localTransform().y();
viewPoint[2] = models[0].localTransform().z();
}
else if(!stereoModel.localTransform().isNull())
{
viewPoint[0] = stereoModel.localTransform().x();
viewPoint[1] = stereoModel.localTransform().y();
viewPoint[2] = stereoModel.localTransform().z();
}
else
{
@@ -2605,7 +2678,7 @@ cv::Mat ExportCloudsDialog::mergeTextures(pcl::TextureMesh & mesh, const QMap<in
{
std::vector<int> textures(mesh.tex_materials.size(), -1);
cv::Size imageSize;
int imageType=CV_8UC1;
const int imageType=CV_8UC3;
UDEBUG("");
bool mergeTextures = true;
for(unsigned int i=0; i<mesh.tex_materials.size(); ++i)
@@ -2617,42 +2690,57 @@ cv::Mat ExportCloudsDialog::mergeTextures(pcl::TextureMesh & mesh, const QMap<in
int textureId = uStr2Int(mesh.tex_materials[i].tex_file);
textures[i] = textureId;
QMap<int, Signature>::const_iterator iter = cachedSignatures.find(textureId);
UASSERT(iter!=cachedSignatures.end() && !iter->sensorData().imageCompressed().empty());
cv::Size tmpImageSize;
if(iter->sensorData().cameraModels().size()==1 &&
iter->sensorData().cameraModels()[0].imageHeight()>0 &&
iter->sensorData().cameraModels()[0].imageWidth()>0)
if(imageSize.width == 0 || imageSize.height == 0)
{
tmpImageSize = iter->sensorData().cameraModels()[0].imageSize();
if(imageSize.height == 0 && imageSize.width == 0)
if(cachedSignatures.find(textureId)!=cachedSignatures.end() && !cachedSignatures.find(textureId)->sensorData().imageCompressed().empty())
{
// just for the first image, get the type, assuming all others have the same type
cv::Mat image;
iter->sensorData().uncompressDataConst(&image, 0);
UASSERT(!image.empty());
imageType = image.type();
SensorData data = cachedSignatures.find(textureId).value().sensorData();
if(data.cameraModels().size()==1 &&
data.cameraModels()[0].imageHeight()>0 &&
data.cameraModels()[0].imageWidth()>0)
{
imageSize = data.cameraModels()[0].imageSize();
}
else if(data.stereoCameraModel().left().imageHeight() > 0 &&
data.stereoCameraModel().left().imageWidth() > 0)
{
imageSize = data.stereoCameraModel().left().imageSize();
}
else // backward compatibility for image size not set in CameraModel
{
cv::Mat image;
data.uncompressDataConst(&image, 0);
UASSERT(!image.empty());
imageSize = image.size();
}
}
else if(_dbDriver)
{
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
_dbDriver->getCalibration(textureId, models, stereoModel);
if(models.size()==1 &&
models[0].imageHeight()>0 &&
models[0].imageWidth()>0)
{
imageSize = models[0].imageSize();
}
else if(stereoModel.left().imageHeight() > 0 &&
stereoModel.left().imageWidth() > 0)
{
imageSize = stereoModel.left().imageSize();
}
else // backward compatibility for image size not set in CameraModel
{
SensorData data;
_dbDriver->getNodeData(textureId, data, true, false, false, false);
cv::Mat image;
data.uncompressDataConst(&image, 0);
UASSERT(!image.empty());
imageSize = image.size();
}
}
}
else // backward compatibility for image size not set in CameraModel
{
cv::Mat image;
iter->sensorData().uncompressDataConst(&image, 0);
UASSERT(!image.empty());
tmpImageSize = image.size();
if(imageSize.height == 0 && imageSize.width == 0)
{
imageType = image.type();
}
}
if(imageSize.width>0 && imageSize.height>0 && imageSize.width != tmpImageSize.width)
{
UWARN("All images should have the same dimensions to merge the textures!");
mergeTextures = false;
break;
}
imageSize = tmpImageSize;
}
}
if(mergeTextures && textures.size() && imageSize.height>0 && imageSize.width>0)
@@ -2680,13 +2768,30 @@ cv::Mat ExportCloudsDialog::mergeTextures(pcl::TextureMesh & mesh, const QMap<in
UASSERT(v < textureSize-emptyImage.rows);
if(textures[t]>=0)
{
QMap<int, Signature>::const_iterator iter = cachedSignatures.find(textures[t]);
UASSERT(iter!=cachedSignatures.end() && !iter->sensorData().imageCompressed().empty());
cv::Mat image;
iter->sensorData().uncompressDataConst(&image, 0);
if(cachedSignatures.find(textures[t]) != cachedSignatures.end() &&
!cachedSignatures.find(textures[t])->sensorData().imageCompressed().empty())
{
cachedSignatures.find(textures[t])->sensorData().uncompressDataConst(&image, 0);
}
else if(_dbDriver)
{
SensorData data;
_dbDriver->getNodeData(textures[t], data, true, false, false, false);
data.uncompressDataConst(&image, 0);
}
UASSERT(!image.empty());
cv::Mat resizedImage;
cv::resize(image, resizedImage, emptyImage.size(), 0.0f, 0.0f, cv::INTER_AREA);
UASSERT(resizedImage.type() == CV_8UC1 || resizedImage.type() == CV_8UC3);
if(resizedImage.type() == CV_8UC1)
{
cv::Mat resizedImageColor;
cv::cvtColor(resizedImage, resizedImageColor, CV_GRAY2BGR);
resizedImage = resizedImageColor;
}
if(_ui->checkBox_gainCompensation->isChecked() && _compensator && _compensator->getIndex(textures[t]) >= 0)
{
_compensator->apply(textures[t], resizedImage);
@@ -2766,9 +2871,17 @@ void ExportCloudsDialog::saveTextureMeshes(
if(uIsInteger(mesh->tex_materials[i].tex_file, false))
{
int textureId = uStr2Int(mesh->tex_materials[i].tex_file);
UASSERT(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty());
cv::Mat image;
cachedSignatures.value(textureId).sensorData().uncompressDataConst(&image, 0);
if(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty())
{
cachedSignatures.value(textureId).sensorData().uncompressDataConst(&image, 0);
}
else if(_dbDriver)
{
SensorData data;
_dbDriver->getNodeData(textureId, data, true, false, false, false);
data.uncompressDataConst(&image, 0);
}
UASSERT(!image.empty());
imageSize = image.size();
if(_ui->checkBox_gainCompensation->isChecked() && _compensator && _compensator->getIndex(textureId) >= 0)
@@ -2886,9 +2999,18 @@ void ExportCloudsDialog::saveTextureMeshes(
if(uIsInteger(mesh->tex_materials[i].tex_file, false))
{
int textureId = uStr2Int(mesh->tex_materials[i].tex_file);
UASSERT(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty());
cv::Mat image;
cachedSignatures.value(textureId).sensorData().uncompressDataConst(&image, 0);
if(cachedSignatures.contains(textureId) && !cachedSignatures.value(textureId).sensorData().imageCompressed().empty())
{
cachedSignatures.value(textureId).sensorData().uncompressDataConst(&image, 0);
}
else if(_dbDriver)
{
SensorData data;
_dbDriver->getNodeData(textureId, data, true, false, false, false);
data.uncompressDataConst(&image, 0);
}
UASSERT(!image.empty());
imageSize = image.size();
if(_ui->checkBox_gainCompensation->isChecked() && _compensator && _compensator->getIndex(textureId) >= 0)

4
guilib/src/ExportCloudsDialog.h
View File

@@ -47,6 +47,7 @@ class QAbstractButton;
namespace rtabmap {
class ProgressDialog;
class GainCompensator;
class DBDriver;
class ExportCloudsDialog : public QDialog
{
@@ -60,6 +61,8 @@ public:
void saveSettings(QSettings & settings, const QString & group = "") const;
void loadSettings(QSettings & settings, const QString & group = "");
void setDBDriver(const DBDriver * dbDriver) {_dbDriver = dbDriver;}
void exportClouds(
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
@@ -124,6 +127,7 @@ private:
QString _workingDirectory;
bool _canceled;
GainCompensator * _compensator;
const DBDriver * _dbDriver;
};
}

122
guilib/src/ui/DatabaseViewer.ui
View File

@@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>1285</width>
<width>1392</width>
<height>965</height>
</rect>
</property>
@@ -52,7 +52,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>253</width>
<width>339</width>
<height>208</height>
</rect>
</property>
@@ -210,7 +210,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>253</width>
<width>339</width>
<height>208</height>
</rect>
</property>
@@ -483,7 +483,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>1285</width>
<width>1392</width>
<height>22</height>
</rect>
</property>
@@ -987,7 +987,7 @@
<item>
<widget class="QToolBox" name="toolBox">
<property name="currentIndex">
<number>3</number>
<number>1</number>
</property>
<widget class="QWidget" name="page_3">
<property name="geometry">
@@ -1131,7 +1131,7 @@
<x>0</x>
<y>0</y>
<width>280</width>
<height>584</height>
<height>608</height>
</rect>
</property>
<attribute name="label">
@@ -1174,7 +1174,7 @@
</property>
</widget>
</item>
<item row="2" column="1">
<item row="3" column="1">
<widget class="QLabel" name="label_51">
<property name="text">
<string>Gain compensation radius (Constraints view)</string>
@@ -1184,7 +1184,7 @@
</property>
</widget>
</item>
<item row="2" column="0">
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_gainCompensationRadius">
<property name="suffix">
<string> m</string>
@@ -1206,6 +1206,20 @@
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_53">
<property name="text">
<string>OctoMap</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="checkBox_octomap">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
<item>
@@ -1547,8 +1561,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>333</width>
<height>256</height>
<width>186</width>
<height>496</height>
</rect>
</property>
<attribute name="label">
@@ -1911,6 +1925,94 @@
</layout>
</widget>
</widget>
<widget class="QDockWidget" name="dockWidget_occupancyGridView">
<property name="windowTitle">
<string>Occupancy Grid</string>
</property>
<attribute name="dockWidgetArea">
<number>4</number>
</attribute>
<widget class="QWidget" name="dockWidgetContents_occupancyGrid">
<layout class="QVBoxLayout" name="verticalLayout_14" stretch="1,0">
<property name="spacing">
<number>0</number>
</property>
<property name="margin">
<number>0</number>
</property>
<item>
<layout class="QVBoxLayout" name="layout_occupancyGridView">
<property name="spacing">
<number>0</number>
</property>
</layout>
</item>
<item>
<layout class="QHBoxLayout" name="horizontalLayout_8">
<item>
<widget class="QCheckBox" name="checkBox_grid_2d">
<property name="text">
<string>2d Map</string>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="checkBox_grid_cubes">
<property name="text">
<string>Cubes Rendering</string>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="checkBox_grid_empty">
<property name="text">
<string>Empty Space</string>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="spinBox_grid_depth">
<property name="prefix">
<string>Depth </string>
</property>
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>16</number>
</property>
<property name="value">
<number>16</number>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_5">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
</layout>
</widget>
</widget>
<action name="actionOpen_database">
<property name="text">
<string>Open database</string>