i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 08:47:45 +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
guilib/src/DepthCalibrationDialog.cpp
+53 -17
View File
@@ -227,25 +227,54 @@ void DepthCalibrationDialog::calibrate(
_ui->label_width->setText("NA"); _ui->label_width->setText("NA");
_ui->label_height->setText("NA"); _ui->label_height->setText("NA");
_imageSize = cv::Size(); _imageSize = cv::Size();
CameraModel model;
if(cachedSignatures.size()) if(cachedSignatures.size())
{ {
const Signature & s = cachedSignatures.begin().value(); const Signature & s = cachedSignatures.begin().value();
const SensorData & data = s.sensorData(); 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(); // use depth image size
_ui->label_width->setNum(data.cameraModels()[0].imageWidth()); _imageSize = depth.size();
_ui->label_height->setNum(data.cameraModels()[0].imageHeight()); _ui->label_width->setNum(_imageSize.width);
_ui->label_height->setNum(_imageSize.height);
if(data.cameraModels()[0].imageWidth() % _ui->spinBox_bin_width->value() != 0 || if(_imageSize.width % _ui->spinBox_bin_width->value() != 0 ||
data.cameraModels()[0].imageHeight() % _ui->spinBox_bin_height->value() != 0) _imageSize.height % _ui->spinBox_bin_height->value() != 0)
{ {
size_t bin_width, bin_height; 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_width->setValue(bin_width);
_ui->spinBox_bin_height->setValue(bin_height); _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) if(this->exec() == QDialog::Accepted)
@@ -285,11 +314,10 @@ void DepthCalibrationDialog::calibrate(
{ {
const Signature & s = cachedSignatures.find(iter->first).value(); const Signature & s = cachedSignatures.find(iter->first).value();
SensorData data = s.sensorData(); SensorData data = s.sensorData();
if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForReprojection())
{ cv::Mat depth, laserScan;
cv::Mat image, depth, laserScan; data.uncompressData(0, &depth, _ui->checkBox_laserScan->isChecked()?&laserScan:0);
data.uncompressData(&image, &depth, _ui->checkBox_laserScan->isChecked()?&laserScan:0); if(data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection() && !depth.empty())
if(!image.empty() && !depth.empty())
{ {
UASSERT(iter->first == data.id()); UASSERT(iter->first == data.id());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud; pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
@@ -329,7 +357,7 @@ void DepthCalibrationDialog::calibrate(
sequence.insert(std::make_pair(iter->first, data)); sequence.insert(std::make_pair(iter->first, data));
cv::Size size = data.cameraModels()[0].imageSize(); cv::Size size = depth.size();
if(_model && if(_model &&
(_model->getWidth()!=size.width || (_model->getWidth()!=size.width ||
_model->getHeight()!=size.height)) _model->getHeight()!=size.height))
@@ -346,7 +374,6 @@ void DepthCalibrationDialog::calibrate(
} }
} }
} }
}
else else
{ {
_progressDialog->appendText(tr("Not suitable camera model found for node %1, ignoring this node!").arg(iter->first), Qt::darkYellow); _progressDialog->appendText(tr("Not suitable camera model found for node %1, ignoring this node!").arg(iter->first), Qt::darkYellow);
@@ -434,7 +461,7 @@ void DepthCalibrationDialog::calibrate(
QDialog * dialog = new QDialog(this->parentWidget()?this->parentWidget():this, Qt::Window); QDialog * dialog = new QDialog(this->parentWidget()?this->parentWidget():this, Qt::Window);
dialog->setAttribute(Qt::WA_DeleteOnClose, true); dialog->setAttribute(Qt::WA_DeleteOnClose, true);
dialog->setWindowTitle(tr("Original/Map")); dialog->setWindowTitle(tr("Original/Map"));
dialog->setMinimumWidth(sequence.begin()->second.cameraModels()[0].imageWidth()); dialog->setMinimumWidth(_imageSize.width);
ImageView * imageView1 = new ImageView(dialog); ImageView * imageView1 = new ImageView(dialog);
imageView1->setMinimumSize(320, 240); imageView1->setMinimumSize(320, 240);
ImageView * imageView2 = new ImageView(dialog); ImageView * imageView2 = new ImageView(dialog);
@@ -450,7 +477,7 @@ void DepthCalibrationDialog::calibrate(
} }
//clams::DiscreteDepthDistortionModel model = clams::calibrate(sequence, poses, map); //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) if(_model == 0)
{ {
size_t bin_width = _ui->spinBox_bin_width->value(); size_t bin_width = _ui->spinBox_bin_width->value();
@@ -487,8 +514,16 @@ void DepthCalibrationDialog::calibrate(
cv::Mat depthImage; cv::Mat depthImage;
ster->second.uncompressDataConst(0, &depthImage); ster->second.uncompressDataConst(0, &depthImage);
if(ster->second.cameraModels().size() == 1 && ster->second.cameraModels()[0].isValidForProjection() && !depthImage.empty())
{
cv::Mat mapDepth; cv::Mat mapDepth;
clams::FrameProjector projector(ster->second.cameraModels()[0]); 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( mapDepth = projector.estimateMapDepth(
map, map,
iter->second.inverse(), iter->second.inverse(),
@@ -505,6 +540,7 @@ void DepthCalibrationDialog::calibrate(
counts = _model->accumulate(mapDepth, depthImage); 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->appendText(tr("Added %1 training examples from node %2 (%3/%4).").arg(counts).arg(iter->first).arg(++index).arg(sequence.size()));
} }
}
_progressDialog->incrementStep(); _progressDialog->incrementStep();
QApplication::processEvents(); QApplication::processEvents();
} }