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

DepthCalibration: support depth images smaller than RGB images

Browse Source
This commit is contained in:
matlabbe
2017-07-24 13:06:30 -04:00
parent 0a1694dd78
commit 57dc0cd53e
1 changed files with 112 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files

188
guilib/src/DepthCalibrationDialog.cpp
View File

@@ -227,25 +227,54 @@ void DepthCalibrationDialog::calibrate(
_ui->label_width->setText("NA");
_ui->label_height->setText("NA");
_imageSize = cv::Size();
CameraModel model;
if(cachedSignatures.size())
{
const Signature & s = cachedSignatures.begin().value();
const SensorData & data = s.sensorData();
if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForReprojection())
cv::Mat depth;
data.uncompressDataConst(0, &depth);
if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection() && !depth.empty())
{
_imageSize = data.cameraModels()[0].imageSize();
_ui->label_width->setNum(data.cameraModels()[0].imageWidth());
_ui->label_height->setNum(data.cameraModels()[0].imageHeight());
// use depth image size
_imageSize = depth.size();
_ui->label_width->setNum(_imageSize.width);
_ui->label_height->setNum(_imageSize.height);
if(data.cameraModels()[0].imageWidth() % _ui->spinBox_bin_width->value() != 0 ||
data.cameraModels()[0].imageHeight() % _ui->spinBox_bin_height->value() != 0)
if(_imageSize.width % _ui->spinBox_bin_width->value() != 0 ||
_imageSize.height % _ui->spinBox_bin_height->value() != 0)
{
size_t bin_width, bin_height;
clams::DiscreteDepthDistortionModel::getBinSize(data.cameraModels()[0].imageWidth(), data.cameraModels()[0].imageHeight(), bin_width, bin_height);
clams::DiscreteDepthDistortionModel::getBinSize(_imageSize.width, _imageSize.height, bin_width, bin_height);
_ui->spinBox_bin_width->setValue(bin_width);
_ui->spinBox_bin_height->setValue(bin_height);
}
}
else if(data.cameraModels().size() > 1)
{
QMessageBox::warning(this, tr("Depth Calibration"),tr("Multi-camera not supported!"));
return;
}
else if(data.cameraModels().size() != 1)
{
QMessageBox::warning(this, tr("Depth Calibration"), tr("Camera model not found."));
return;
}
else if(data.cameraModels().size() == 1 && !data.cameraModels()[0].isValidForProjection())
{
QMessageBox::warning(this, tr("Depth Calibration"), tr("Camera model %1 not valid for projection.").arg(s.id()));
return;
}
else
{
QMessageBox::warning(this, tr("Depth Calibration"), tr("Depth image cannot be found in the cache, make sure to update cache before doing calibration."));
return;
}
}
else
{
QMessageBox::warning(this, tr("Depth Calibration"), tr("No signatures detected! Map is empty!?"));
return;
}
if(this->exec() == QDialog::Accepted)
@@ -285,65 +314,63 @@ void DepthCalibrationDialog::calibrate(
{
const Signature & s = cachedSignatures.find(iter->first).value();
SensorData data = s.sensorData();
if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForReprojection())
cv::Mat depth, laserScan;
data.uncompressData(0, &depth, _ui->checkBox_laserScan->isChecked()?&laserScan:0);
if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection() && !depth.empty())
{
cv::Mat image, depth, laserScan;
data.uncompressData(&image, &depth, _ui->checkBox_laserScan->isChecked()?&laserScan:0);
if(!image.empty() && !depth.empty())
UASSERT(iter->first == data.id());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
if(_ui->checkBox_laserScan->isChecked())
{
UASSERT(iter->first == data.id());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
if(_ui->checkBox_laserScan->isChecked())
cloud = util3d::laserScanToPointCloud(laserScan);
indices->resize(cloud->size());
for(unsigned int i=0; i<indices->size(); ++i)
{
cloud = util3d::laserScanToPointCloud(laserScan);
indices->resize(cloud->size());
for(unsigned int i=0; i<indices->size(); ++i)
{
indices->at(i) = i;
}
indices->at(i) = i;
}
else
}
else
{
cloud = util3d::cloudFromSensorData(
data,
_ui->spinBox_decimation->value(),
_ui->doubleSpinBox_maxDepth->value(),
_ui->doubleSpinBox_minDepth->value(),
indices.get(),
parameters);
}
if(indices->size())
{
if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
{
cloud = util3d::cloudFromSensorData(
data,
_ui->spinBox_decimation->value(),
_ui->doubleSpinBox_maxDepth->value(),
_ui->doubleSpinBox_minDepth->value(),
indices.get(),
parameters);
cloud = util3d::voxelize(cloud, indices, _ui->doubleSpinBox_voxelSize->value());
}
if(indices->size())
cloud = util3d::transformPointCloud(cloud, iter->second);
points+=cloud->size();
*map += *cloud;
sequence.insert(std::make_pair(iter->first, data));
cv::Size size = depth.size();
if(_model &&
(_model->getWidth()!=size.width ||
_model->getHeight()!=size.height))
{
if(_ui->doubleSpinBox_voxelSize->value() > 0.0)
{
cloud = util3d::voxelize(cloud, indices, _ui->doubleSpinBox_voxelSize->value());
}
cloud = util3d::transformPointCloud(cloud, iter->second);
points+=cloud->size();
*map += *cloud;
sequence.insert(std::make_pair(iter->first, data));
cv::Size size = data.cameraModels()[0].imageSize();
if(_model &&
(_model->getWidth()!=size.width ||
_model->getHeight()!=size.height))
{
QString msg = tr("Depth images (%1x%2) in the map don't have the "
"same size then in the current model (%3x%4). You may want "
"to check \"Reset previous model\" before trying again.")
.arg(size.width).arg(size.height)
.arg(_model->getWidth()).arg(_model->getHeight());
QMessageBox::warning(this, tr("Depth Calibration"), msg);
_progressDialog->appendText(msg, Qt::darkRed);
_progressDialog->setAutoClose(false);
return;
}
QString msg = tr("Depth images (%1x%2) in the map don't have the "
"same size then in the current model (%3x%4). You may want "
"to check \"Reset previous model\" before trying again.")
.arg(size.width).arg(size.height)
.arg(_model->getWidth()).arg(_model->getHeight());
QMessageBox::warning(this, tr("Depth Calibration"), msg);
_progressDialog->appendText(msg, Qt::darkRed);
_progressDialog->setAutoClose(false);
return;
}
}
}
@@ -434,7 +461,7 @@ void DepthCalibrationDialog::calibrate(
QDialog * dialog = new QDialog(this->parentWidget()?this->parentWidget():this, Qt::Window);
dialog->setAttribute(Qt::WA_DeleteOnClose, true);
dialog->setWindowTitle(tr("Original/Map"));
dialog->setMinimumWidth(sequence.begin()->second.cameraModels()[0].imageWidth());
dialog->setMinimumWidth(_imageSize.width);
ImageView * imageView1 = new ImageView(dialog);
imageView1->setMinimumSize(320, 240);
ImageView * imageView2 = new ImageView(dialog);
@@ -450,7 +477,7 @@ void DepthCalibrationDialog::calibrate(
}
//clams::DiscreteDepthDistortionModel model = clams::calibrate(sequence, poses, map);
const cv::Size & imageSize = sequence.begin()->second.cameraModels()[0].imageSize();
const cv::Size & imageSize = _imageSize;
if(_model == 0)
{
size_t bin_width = _ui->spinBox_bin_width->value();
@@ -487,24 +514,33 @@ void DepthCalibrationDialog::calibrate(
cv::Mat depthImage;
ster->second.uncompressDataConst(0, &depthImage);
cv::Mat mapDepth;
clams::FrameProjector projector(ster->second.cameraModels()[0]);
mapDepth = projector.estimateMapDepth(
map,
iter->second.inverse(),
depthImage,
_ui->doubleSpinBox_coneRadius->value(),
_ui->doubleSpinBox_coneStdDevThresh->value());
if(ULogger::level() == ULogger::kDebug)
if(ster->second.cameraModels().size() == 1 && ster->second.cameraModels()[0].isValidForProjection() && !depthImage.empty())
{
imageView1->setImage(uCvMat2QImage(depthImage));
imageView2->setImage(uCvMat2QImage(mapDepth));
}
cv::Mat mapDepth;
CameraModel model = ster->second.cameraModels()[0];
if(model.imageWidth() != depthImage.cols)
{
UASSERT_MSG(model.imageHeight() % depthImage.rows == 0, uFormat("rgb=%d depth=%d", model.imageHeight(), depthImage.rows).c_str());
model = model.scaled(double(depthImage.rows) / double(model.imageHeight()));
}
clams::FrameProjector projector(model);
mapDepth = projector.estimateMapDepth(
map,
iter->second.inverse(),
depthImage,
_ui->doubleSpinBox_coneRadius->value(),
_ui->doubleSpinBox_coneStdDevThresh->value());
counts = _model->accumulate(mapDepth, depthImage);
_progressDialog->appendText(tr("Added %1 training examples from node %2 (%3/%4).").arg(counts).arg(iter->first).arg(++index).arg(sequence.size()));
}
if(ULogger::level() == ULogger::kDebug)
{
imageView1->setImage(uCvMat2QImage(depthImage));
imageView2->setImage(uCvMat2QImage(mapDepth));
}
counts = _model->accumulate(mapDepth, depthImage);
_progressDialog->appendText(tr("Added %1 training examples from node %2 (%3/%4).").arg(counts).arg(iter->first).arg(++index).arg(sequence.size()));
}
}
_progressDialog->incrementStep();
QApplication::processEvents();
}