void DataRecorder::showImage(const cv::Mat & image, const cv::Mat & depth)
{
processingImages_ = true;
imageView_->setImage(uCvMat2QImage(image));
imageView_->setImageDepth(uCvMat2QImage(depth));
label_->setText(tr("Images=%1 (~%2 MB)").arg(count_).arg(totalSizeKB_/1000));
processingImages_ = false;
}
void PdfPlotItem::showDescription(bool shown)
{
if(!_text)
{
_text = new QGraphicsTextItem(this);
_text->setVisible(false);
}
if(shown)
{
if(!_img && _signaturesRef)
{
QImage img;
QMap<int, Signature>::const_iterator iter = _signaturesRef->find(int(this->data().x()));
if(iter != _signaturesRef->constEnd() && !iter.value().sensorData().imageCompressed().empty())
{
cv::Mat image;
iter.value().sensorData().uncompressDataConst(&image, 0, 0);
if(!image.empty())
{
img = uCvMat2QImage(image);
QPixmap scaled = QPixmap::fromImage(img).scaledToWidth(128);
_img = new QGraphicsPixmapItem(scaled, this);
_img->setVisible(false);
}
}
}
if(_img)
_text->setPos(this->mapFromScene(4+150,0));
else
_text->setPos(this->mapFromScene(4,0));
if(_childCount >= 0)
{
_text->setPlainText(QString("ID = %1\nValue = %2\nWeight = %3").arg(this->data().x()).arg(this->data().y()).arg(_childCount));
}
else
{
_text->setPlainText(QString("ID = %1\nValue = %2").arg(this->data().x()).arg(this->data().y()));
}
_text->setVisible(true);
if(_img)
{
_img->setPos(this->mapFromScene(4,0));
_img->setVisible(true);
}
}
else
{
_text->setVisible(false);
if(_img)
_img->setVisible(false);
}
UPlotItem::showDescription(shown);
}
void PdfPlotItem::showDescription(bool shown)
{
if(!_text)
{
_text = new QGraphicsTextItem(this);
_text->setVisible(false);
}
if(shown)
{
if(!_img && _imagesRef)
{
QImage img;
QMap<int, std::vector<unsigned char> >::const_iterator iter = _imagesRef->find(int(this->data().x()));
if(iter != _imagesRef->constEnd())
{
img = uCvMat2QImage(util3d::uncompressImage(iter.value()));
QPixmap scaled = QPixmap::fromImage(img).scaledToWidth(128);
_img = new QGraphicsPixmapItem(scaled, this);
_img->setVisible(false);
}
}
if(_img)
_text->setPos(this->mapFromScene(4+150,0));
else
_text->setPos(this->mapFromScene(4,0));
if(_childCount >= 0)
{
_text->setPlainText(QString("ID = %1\nValue = %2\nWeight = %3").arg(this->data().x()).arg(this->data().y()).arg(_childCount));
}
else
{
_text->setPlainText(QString("ID = %1\nValue = %2").arg(this->data().x()).arg(this->data().y()));
}
_text->setVisible(true);
if(_img)
{
_img->setPos(this->mapFromScene(4,0));
_img->setVisible(true);
}
}
else
{
_text->setVisible(false);
if(_img)
_img->setVisible(false);
}
UPlotItem::showDescription(shown);
}
void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat & imageRight, const QString & cameraName)
{
processingData_ = true;
if(cameraName_.isEmpty())
{
cameraName_ = "0000";
if(!cameraName.isEmpty())
{
cameraName_ = cameraName;
}
}
if(ui_->label_serial->text().isEmpty())
{
ui_->label_serial->setText(cameraName_);
}
std::vector<cv::Mat> inputRawImages(2);
if(ui_->checkBox_switchImages->isChecked())
{
inputRawImages[0] = imageRight;
inputRawImages[1] = imageLeft;
}
else
{
inputRawImages[0] = imageLeft;
inputRawImages[1] = imageRight;
}
std::vector<cv::Mat> images(2);
images[0] = inputRawImages[0];
images[1] = inputRawImages[1];
imageSize_[0] = images[0].size();
imageSize_[1] = images[1].size();
bool boardFound[2] = {false};
bool boardAccepted[2] = {false};
bool readyToCalibrate[2] = {false};
std::vector<std::vector<cv::Point2f> > pointBuf(2);
bool depthDetected = false;
for(int id=0; id<(stereo_?2:1); ++id)
{
cv::Mat viewGray;
if(!images[id].empty())
{
if(images[id].type() == CV_16UC1)
{
depthDetected = true;
//assume IR image: convert to gray scaled
const float factor = 255.0f / float((maxIrs_[id] - minIrs_[id]));
viewGray = cv::Mat(images[id].rows, images[id].cols, CV_8UC1);
for(int i=0; i<images[id].rows; ++i)
{
for(int j=0; j<images[id].cols; ++j)
{
viewGray.at<unsigned char>(i, j) = (unsigned char)std::min(float(std::max(images[id].at<unsigned short>(i,j) - minIrs_[id], 0)) * factor, 255.0f);
}
}
cvtColor(viewGray, images[id], cv::COLOR_GRAY2BGR); // convert to show detected points in color
}
else if(images[id].channels() == 3)
{
cvtColor(images[id], viewGray, cv::COLOR_BGR2GRAY);
}
else
{
viewGray = images[id];
cvtColor(viewGray, images[id], cv::COLOR_GRAY2BGR); // convert to show detected points in color
}
}
else
{
UERROR("Image %d is empty!! Should not!", id);
}
minIrs_[id] = 0;
maxIrs_[id] = 0x7FFF;
//Dot it only if not yet calibrated
if(!ui_->pushButton_save->isEnabled())
{
cv::Size boardSize(ui_->spinBox_boardWidth->value(), ui_->spinBox_boardHeight->value());
if(!viewGray.empty())
{
int flags = CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE;
if(!viewGray.empty())
{
int maxScale = viewGray.cols < 640?2:1;
for( int scale = 1; scale <= maxScale; scale++ )
{
cv::Mat timg;
if( scale == 1 )
timg = viewGray;
else
cv::resize(viewGray, timg, cv::Size(), scale, scale, CV_INTER_CUBIC);
boardFound[id] = cv::findChessboardCorners(timg, boardSize, pointBuf[id], flags);
if(boardFound[id])
{
if( scale > 1 )
{
cv::Mat cornersMat(pointBuf[id]);
cornersMat *= 1./scale;
}
break;
}
}
}
}
if(boardFound[id]) // If done with success,
{
// improve the found corners' coordinate accuracy for chessboard
float minSquareDistance = -1.0f;
for(unsigned int i=0; i<pointBuf[id].size()-1; ++i)
{
float d = cv::norm(pointBuf[id][i] - pointBuf[id][i+1]);
if(minSquareDistance == -1.0f || minSquareDistance > d)
{
minSquareDistance = d;
}
}
float radius = minSquareDistance/2.0f +0.5f;
cv::cornerSubPix( viewGray, pointBuf[id], cv::Size(radius, radius), cv::Size(-1,-1),
cv::TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1 ));
// Draw the corners.
cv::drawChessboardCorners(images[id], boardSize, cv::Mat(pointBuf[id]), boardFound[id]);
std::vector<float> params(4,0);
getParams(pointBuf[id], boardSize, imageSize_[id], params[0], params[1], params[2], params[3]);
bool addSample = true;
for(unsigned int i=0; i<imageParams_[id].size(); ++i)
{
if(fabs(params[0] - imageParams_[id][i].at(0)) < 0.1 && // x
fabs(params[1] - imageParams_[id][i].at(1)) < 0.1 && // y
fabs(params[2] - imageParams_[id][i].at(2)) < 0.05 && // size
fabs(params[3] - imageParams_[id][i].at(3)) < 0.1) // skew
{
addSample = false;
}
}
if(addSample)
{
boardAccepted[id] = true;
imagePoints_[id].push_back(pointBuf[id]);
imageParams_[id].push_back(params);
UINFO("[%d] Added board, total=%d. (x=%f, y=%f, size=%f, skew=%f)", id, (int)imagePoints_[id].size(), params[0], params[1], params[2], params[3]);
}
// update statistics
std::vector<float> xRange(2, imageParams_[id][0].at(0));
std::vector<float> yRange(2, imageParams_[id][0].at(1));
std::vector<float> sizeRange(2, imageParams_[id][0].at(2));
std::vector<float> skewRange(2, imageParams_[id][0].at(3));
for(unsigned int i=1; i<imageParams_[id].size(); ++i)
{
xRange[0] = imageParams_[id][i].at(0) < xRange[0] ? imageParams_[id][i].at(0) : xRange[0];
xRange[1] = imageParams_[id][i].at(0) > xRange[1] ? imageParams_[id][i].at(0) : xRange[1];
yRange[0] = imageParams_[id][i].at(1) < yRange[0] ? imageParams_[id][i].at(1) : yRange[0];
yRange[1] = imageParams_[id][i].at(1) > yRange[1] ? imageParams_[id][i].at(1) : yRange[1];
sizeRange[0] = imageParams_[id][i].at(2) < sizeRange[0] ? imageParams_[id][i].at(2) : sizeRange[0];
sizeRange[1] = imageParams_[id][i].at(2) > sizeRange[1] ? imageParams_[id][i].at(2) : sizeRange[1];
skewRange[0] = imageParams_[id][i].at(3) < skewRange[0] ? imageParams_[id][i].at(3) : skewRange[0];
skewRange[1] = imageParams_[id][i].at(3) > skewRange[1] ? imageParams_[id][i].at(3) : skewRange[1];
}
//UINFO("Stats [%d]:", id);
//UINFO(" Count = %d", (int)imagePoints_[id].size());
//UINFO(" x = [%f -> %f]", xRange[0], xRange[1]);
//UINFO(" y = [%f -> %f]", yRange[0], yRange[1]);
//UINFO(" size = [%f -> %f]", sizeRange[0], sizeRange[1]);
//UINFO(" skew = [%f -> %f]", skewRange[0], skewRange[1]);
float xGood = xRange[1] - xRange[0];
float yGood = yRange[1] - yRange[0];
float sizeGood = sizeRange[1] - sizeRange[0];
float skewGood = skewRange[1] - skewRange[0];
if(id == 0)
{
ui_->progressBar_x->setValue(xGood*100);
ui_->progressBar_y->setValue(yGood*100);
ui_->progressBar_size->setValue(sizeGood*100);
ui_->progressBar_skew->setValue(skewGood*100);
if((int)imagePoints_[id].size() > ui_->progressBar_count->maximum())
{
ui_->progressBar_count->setMaximum((int)imagePoints_[id].size());
}
ui_->progressBar_count->setValue((int)imagePoints_[id].size());
}
else
{
ui_->progressBar_x_2->setValue(xGood*100);
ui_->progressBar_y_2->setValue(yGood*100);
ui_->progressBar_size_2->setValue(sizeGood*100);
ui_->progressBar_skew_2->setValue(skewGood*100);
if((int)imagePoints_[id].size() > ui_->progressBar_count_2->maximum())
{
ui_->progressBar_count_2->setMaximum((int)imagePoints_[id].size());
}
ui_->progressBar_count_2->setValue((int)imagePoints_[id].size());
}
if(imagePoints_[id].size() >= COUNT_MIN && xGood > 0.5 && yGood > 0.5 && sizeGood > 0.4 && skewGood > 0.5)
{
readyToCalibrate[id] = true;
}
//update IR values
if(inputRawImages[id].type() == CV_16UC1)
{
//update min max IR if the chessboard was found
minIrs_[id] = 0xFFFF;
maxIrs_[id] = 0;
for(size_t i = 0; i < pointBuf[id].size(); ++i)
{
const cv::Point2f &p = pointBuf[id][i];
cv::Rect roi(std::max(0, (int)p.x - 3), std::max(0, (int)p.y - 3), 6, 6);
roi.width = std::min(roi.width, inputRawImages[id].cols - roi.x);
roi.height = std::min(roi.height, inputRawImages[id].rows - roi.y);
//find minMax in the roi
double min, max;
cv::minMaxLoc(inputRawImages[id](roi), &min, &max);
if(min < minIrs_[id])
{
minIrs_[id] = min;
}
if(max > maxIrs_[id])
{
maxIrs_[id] = max;
}
}
}
}
}
}
ui_->label_baseline->setVisible(!depthDetected);
ui_->label_baseline_name->setVisible(!depthDetected);
if(stereo_ && ((boardAccepted[0] && boardFound[1]) || (boardAccepted[1] && boardFound[0])))
{
stereoImagePoints_[0].push_back(pointBuf[0]);
stereoImagePoints_[1].push_back(pointBuf[1]);
UINFO("Add stereo image points (size=%d)", (int)stereoImagePoints_[0].size());
}
if(!stereo_ && readyToCalibrate[0])
{
ui_->pushButton_calibrate->setEnabled(true);
}
else if(stereo_ && readyToCalibrate[0] && readyToCalibrate[1] && stereoImagePoints_[0].size())
{
ui_->pushButton_calibrate->setEnabled(true);
}
if(ui_->radioButton_rectified->isChecked())
{
if(models_[0].isValid())
{
images[0] = models_[0].rectifyImage(images[0]);
}
if(models_[1].isValid())
{
images[1] = models_[1].rectifyImage(images[1]);
}
}
else if(ui_->radioButton_stereoRectified->isChecked() &&
(stereoModel_.left().isValid() &&
stereoModel_.right().isValid()&&
(!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0)))
{
images[0] = stereoModel_.left().rectifyImage(images[0]);
images[1] = stereoModel_.right().rectifyImage(images[1]);
}
if(ui_->checkBox_showHorizontalLines->isChecked())
{
for(int id=0; id<(stereo_?2:1); ++id)
{
int step = imageSize_[id].height/16;
for(int i=step; i<imageSize_[id].height; i+=step)
{
cv::line(images[id], cv::Point(0, i), cv::Point(imageSize_[id].width, i), CV_RGB(0,255,0));
}
}
}
ui_->label_left->setText(tr("%1x%2").arg(images[0].cols).arg(images[0].rows));
//show frame
ui_->image_view->setImage(uCvMat2QImage(images[0]).mirrored(ui_->checkBox_mirror->isChecked(), false));
if(stereo_)
{
ui_->label_right->setText(tr("%1x%2").arg(images[1].cols).arg(images[1].rows));
ui_->image_view_2->setImage(uCvMat2QImage(images[1]).mirrored(ui_->checkBox_mirror->isChecked(), false));
}
processingData_ = false;
}
void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
{
processingData_ = true;
int quality = odom.info().inliers;
bool lost = false;
bool lostStateChanged = false;
if(odom.pose().isNull())
{
UDEBUG("odom lost"); // use last pose
lostStateChanged = imageView_->getBackgroundColor() != Qt::darkRed;
imageView_->setBackgroundColor(Qt::darkRed);
cloudView_->setBackgroundColor(Qt::darkRed);
lost = true;
}
else if(odom.info().inliers>0 &&
qualityWarningThr_ &&
odom.info().inliers < qualityWarningThr_)
{
UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, qualityWarningThr_);
lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
imageView_->setBackgroundColor(Qt::darkYellow);
cloudView_->setBackgroundColor(Qt::darkYellow);
}
else
{
UDEBUG("odom ok");
lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
imageView_->setBackgroundColor(cloudView_->getDefaultBackgroundColor());
cloudView_->setBackgroundColor(Qt::black);
}
timeLabel_->setText(QString("%1 s").arg(odom.info().timeEstimation));
if(!odom.data().imageRaw().empty() &&
!odom.data().depthOrRightRaw().empty() &&
(odom.data().stereoCameraModel().isValid() || odom.data().cameraModels().size()))
{
UDEBUG("New pose = %s, quality=%d", odom.pose().prettyPrint().c_str(), quality);
if(!odom.data().depthRaw().empty())
{
if(odom.data().imageRaw().cols % decimationSpin_->value() == 0 &&
odom.data().imageRaw().rows % decimationSpin_->value() == 0)
{
validDecimationValue_ = decimationSpin_->value();
}
else
{
UWARN("Decimation (%d) must be a denominator of the width and height of "
"the image (%d/%d). Using last valid decimation value (%d).",
decimationSpin_->value(),
odom.data().imageRaw().cols,
odom.data().imageRaw().rows,
validDecimationValue_);
}
}
else
{
validDecimationValue_ = decimationSpin_->value();
}
// visualization: buffering the clouds
// Create the new cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = util3d::cloudRGBFromSensorData(
odom.data(),
validDecimationValue_,
0.0f,
voxelSpin_->value());
if(cloud->size())
{
if(!odom.pose().isNull())
{
if(cloudView_->getAddedClouds().contains("cloudtmp"))
{
cloudView_->removeCloud("cloudtmp");
}
while(maxCloudsSpin_->value()>0 && (int)addedClouds_.size() > maxCloudsSpin_->value())
{
UASSERT(cloudView_->removeCloud(addedClouds_.first()));
addedClouds_.pop_front();
}
odom.data().id()?id_=odom.data().id():++id_;
std::string cloudName = uFormat("cloud%d", id_);
addedClouds_.push_back(cloudName);
UASSERT(cloudView_->addCloud(cloudName, cloud, odom.pose()));
}
else
{
cloudView_->addOrUpdateCloud("cloudtmp", cloud, lastOdomPose_);
}
}
}
if(!odom.pose().isNull())
{
lastOdomPose_ = odom.pose();
cloudView_->updateCameraTargetPosition(odom.pose());
}
if(odom.info().localMap.size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(odom.info().localMap.size());
int i=0;
for(std::multimap<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
{
(*cloud)[i].x = iter->second.x;
(*cloud)[i].y = iter->second.y;
(*cloud)[i++].z = iter->second.z;
}
cloudView_->addOrUpdateCloud("localmap", cloud);
}
if(!odom.data().imageRaw().empty())
{
if(odom.info().type == 0)
{
imageView_->setFeatures(odom.info().words, odom.data().depthRaw(), Qt::yellow);
}
else if(odom.info().type == 1)
{
std::vector<cv::KeyPoint> kpts;
cv::KeyPoint::convert(odom.info().refCorners, kpts);
imageView_->setFeatures(kpts, odom.data().depthRaw(), Qt::red);
}
imageView_->clearLines();
if(lost)
{
if(lostStateChanged)
{
// save state
odomImageShow_ = imageView_->isImageShown();
odomImageDepthShow_ = imageView_->isImageDepthShown();
}
imageView_->setImageDepth(uCvMat2QImage(odom.data().imageRaw()));
imageView_->setImageShown(true);
imageView_->setImageDepthShown(true);
}
else
{
if(lostStateChanged)
{
// restore state
imageView_->setImageShown(odomImageShow_);
imageView_->setImageDepthShown(odomImageDepthShow_);
}
imageView_->setImage(uCvMat2QImage(odom.data().imageRaw()));
if(imageView_->isImageDepthShown())
{
imageView_->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw()));
}
if(odom.info().type == 0)
{
if(imageView_->isFeaturesShown())
{
for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
{
imageView_->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
}
for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
{
imageView_->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
}
}
}
}
if(odom.info().type == 1 && odom.info().cornerInliers.size())
{
if(imageView_->isFeaturesShown() || imageView_->isLinesShown())
{
//draw lines
UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
for(unsigned int i=0; i<odom.info().cornerInliers.size(); ++i)
{
if(imageView_->isFeaturesShown())
{
imageView_->setFeatureColor(odom.info().cornerInliers[i], Qt::green); // inliers
}
if(imageView_->isLinesShown())
{
imageView_->addLine(
odom.info().refCorners[odom.info().cornerInliers[i]].x,
odom.info().refCorners[odom.info().cornerInliers[i]].y,
odom.info().newCorners[odom.info().cornerInliers[i]].x,
odom.info().newCorners[odom.info().cornerInliers[i]].y,
Qt::blue);
}
}
}
}
if(!odom.data().imageRaw().empty())
{
imageView_->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows));
}
}
imageView_->update();
cloudView_->update();
QApplication::processEvents();
processingData_ = false;
}
void CameraViewer::showImage(const rtabmap::SensorData & data)
{
if(!data.imageRaw().empty() || !data.depthOrRightRaw().empty())
{
if(data.imageRaw().cols % decimationSpin_->value() == 0 &&
data.imageRaw().rows % decimationSpin_->value() == 0 &&
data.depthOrRightRaw().cols % decimationSpin_->value() == 0 &&
data.depthOrRightRaw().rows % decimationSpin_->value() == 0)
{
validDecimationValue_ = decimationSpin_->value();
}
else
{
UWARN("Decimation (%d) must be a denominator of the width and height of "
"the image (color=%d/%d depth=%d/%d). Using last valid decimation value (%d).",
decimationSpin_->value(),
data.imageRaw().cols,
data.imageRaw().rows,
data.depthOrRightRaw().cols,
data.depthOrRightRaw().rows,
validDecimationValue_);
}
}
processingImages_ = true;
if(!data.imageRaw().empty())
{
imageView_->setImage(uCvMat2QImage(util2d::decimate(data.imageRaw(), validDecimationValue_)));
}
if(!data.depthOrRightRaw().empty())
{
imageView_->setImageDepth(util2d::decimate(data.depthOrRightRaw(), validDecimationValue_));
}
if(!data.depthOrRightRaw().empty() &&
(data.stereoCameraModel().isValidForProjection() || (data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection())))
{
if(showCloudCheckbox_->isChecked())
{
if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty())
{
showCloudCheckbox_->setEnabled(true);
cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, validDecimationValue_, 0, 0, 0, parameters_));
}
else if(!data.depthOrRightRaw().empty())
{
showCloudCheckbox_->setEnabled(true);
cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, validDecimationValue_, 0, 0, 0, parameters_));
}
}
}
if(!data.laserScanRaw().isEmpty())
{
showScanCheckbox_->setEnabled(true);
if(showScanCheckbox_->isChecked())
{
cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloud(data.laserScanRaw()), validDecimationValue_), Transform::getIdentity(), Qt::yellow);
}
}
cloudView_->setVisible((showCloudCheckbox_->isEnabled() && showCloudCheckbox_->isChecked()) ||
(showScanCheckbox_->isEnabled() && showScanCheckbox_->isChecked()));
if(cloudView_->isVisible())
{
cloudView_->update();
}
if(cloudView_->getAddedClouds().contains("cloud"))
{
cloudView_->setCloudVisibility("cloud", showCloudCheckbox_->isChecked());
}
if(cloudView_->getAddedClouds().contains("scan"))
{
cloudView_->setCloudVisibility("scan", showScanCheckbox_->isChecked());
}
processingImages_ = false;
}
