Skip to content

Commit

Permalink
DepthCalibration: support depth images smaller than RGB images
Browse files Browse the repository at this point in the history
  • Loading branch information
@matlabbe
matlabbe committed Jul 24, 2017
1 parent 0a1694d commit 57dc0cd
Showing 1 changed file with 112 additions and 76 deletions.
188 changes: 112 additions & 76 deletions guilib/src/DepthCalibrationDialog.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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;
UASSERT(iter->first == data.id());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;

if(_ui->checkBox_laserScan->isChecked())
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;
}
}
}
Expand Down Expand Up @@ -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);
Expand All @@ -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();
Expand Down Expand Up @@ -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());

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()));
}

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();
}
Expand Down

0 comments on commit 57dc0cd

Please sign in to comment.