/* * Copyright (c) 2009-2010, Oracle and/or its affiliates. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: * * * Redistributions of source code must retain the above copyright notice, * this list of conditions and the following disclaimer. * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. * * * Neither the name of Oracle nor the names of its contributors * may be used to endorse or promote products derived from this software without * specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF * THE POSSIBILITY OF SUCH DAMAGE. */ #include "KittiForm.h" //#include //#include #include #include #include #include #include #include KittiForm::KittiForm() { widget.setupUi(this); widget.imgFrame->setScene(&m_frameQT); PATTERN_DEM = string("/media/DATOS/Videos/DEM/%s_%s_%010d.bin"); PATTERN_CRFLOG = string("/media/DATOS/Videos/CRF/%s_%s_%010d.log"); PATTERN_CRF = string("/media/DATOS/Videos/CRF/%s_%s_%010d_p%03d_l%03d_t%02d.bin"); PATTERN_CRF_NAME = string("%s_%s_%010d_"); } KittiForm::~KittiForm() { } void KittiForm::updateSpinValue() { widget.spinFrame->setValue(widget.sliderFrame->value()); } void KittiForm::loadDispImage() { char imgName[500]; char imgLeftName[500]; char imgRightName[500]; cv::Mat dispImg; showFrame(); printf("loadDispImage\n"); if (widget.chkDispImg->isChecked() || widget.chkComputeStereo->isChecked()) { sprintf(imgName, "%06d.png", widget.spinFrame->value());//change here sprintf(imgLeftName, "%s/%s/%s/image_00/data/%s", widget.txtDataSetDir->text().toStdString().data(), getSelectedSceneTxt().toStdString().data(), widget.listSequences->currentItem()->text().toStdString().data(), imgName); sprintf(imgRightName, "%s/%s/%s/image_01/data/%s", widget.txtDataSetDir->text().toStdString().data(), getSelectedSceneTxt().toStdString().data(), widget.listSequences->currentItem()->text().toStdString().data(), imgName); cout<<"left image: "<value(), widget.spinSAD->value(), widget.spinPreFiltSize->value(), widget.spinPreFiltCap->value(), widget.spinMinDisp->value(), widget.spinTextureTh->value(), widget.spinUnique->value(), widget.spinSpeckWind->value(), widget.spinSpeckRange->value(), widget.spinDisp12MaxDiff->value()); cv::Mat auxShow(dispImg.rows, dispImg.cols, CV_8UC1); cv::Mat auxDisp(dispImg.rows, dispImg.cols, CV_8UC1); dispImg.convertTo(auxDisp, CV_8UC1); cv::normalize(auxDisp, auxShow, 0, 255, CV_MINMAX); cv::imshow("DISPARITY_BM", auxShow); m_stereo.sgbmKitti(string(imgLeftName), string(imgRightName), &dispImg, widget.spinMinDisp->value(), widget.spinDisp->value(), widget.spinSAD->value(), widget.spinSGMp1->value(), widget.spinSGMp2->value(), widget.spinDisp12MaxDiff->value(), widget.spinPreFiltCap->value(), widget.spinUnique->value(), widget.spinSpeckWind->value(), widget.spinSpeckRange->value()); dispImg.convertTo(auxDisp, CV_8UC1); cv::normalize(auxDisp, auxShow, 0, 255, CV_MINMAX); cv::imshow("DISPARITY_SGBM", auxShow); } else { if (widget.chkSGM->isChecked()) { m_stereo.sgbmKitti(string(imgLeftName), string(imgRightName), &dispImg, widget.spinMinDisp->value(), widget.spinDisp->value(), widget.spinSAD->value(), widget.spinSGMp1->value(), widget.spinSGMp2->value(), widget.spinDisp12MaxDiff->value(), widget.spinPreFiltCap->value(), widget.spinUnique->value(), widget.spinSpeckWind->value(), widget.spinSpeckRange->value()); } else { m_stereo.exampleOpencvBaseKitti(string(imgLeftName), string(imgRightName), &dispImg, 7, 9, 5, 63, -35, 100, 5, 196, 128, 90); // widget.spinDisp->value(), // widget.spinSAD->value(), // widget.spinPreFiltSize->value(), // widget.spinPreFiltCap->value(), // widget.spinMinDisp->value(), // widget.spinTextureTh->value(), // widget.spinUnique->value(), // widget.spinSpeckWind->value(), // widget.spinSpeckRange->value(), // widget.spinDisp12MaxDiff->value()); } } printf("Frame leido (%d)\n", widget.spinFrame->value()); if (widget.chkDispImg->isChecked()) { //show disp image cv::Mat auxShow(dispImg.rows, dispImg.cols, CV_8UC1); cv::Mat auxDisp(dispImg.rows, dispImg.cols, CV_8UC1); dispImg.convertTo(auxDisp, CV_8UC1); cv::normalize(auxDisp, auxShow, 0, 255, CV_MINMAX); cv::imshow("DISPARITY", auxShow); } if (widget.chkComputeStereo->isChecked()) { //compute point cloud sprintf(imgName, "%010d.pcd", widget.spinFrame->value()); sprintf(imgLeftName, "/tmp/%s", imgName); // printf("%s\n",widget.txtDataSetDir->text().toStdString().data()); // printf("%s\n",getSelectedSceneTxt().toStdString().data()); // printf("%s\n",widget.listSequences->currentItem()->text().toStdString().data()); // printf("%s\n",imgName); // printf("PCD: \"%s\"\n",imgLeftName); m_stereo.compute3dKitti(string(imgLeftName), string("/tmp/img25.png"), widget.chkSaveStPcd->isChecked(), widget.spinPitch->value(), widget.spinRoll->value(), widget.spinYaw->value(), widget.spinX->value(), widget.spinY->value(), widget.spinZ->value(), widget.chkAngles->isChecked()); //show cloud m_ptCloud.readCloudFromFile(string(imgLeftName)/*string("/tmp/cloudTemp.pcd")*/, m_ptCloud.original); this->rotateCloud(); //m_ptCloud.setCloud(m_stereo.getCloud(),0); //m_ptCloud.showPointCloud(m_ptCloud.original); } } algorithm_01(); widget.sliderFrame->setValue(widget.spinFrame->value()); } void KittiForm::rotateCloud() { // computeDEM(); if (widget.chkAngles->isChecked()) { m_ptCloud.rotTransCloud(m_ptCloud.original, m_ptCloud.triangulation, pcl::deg2rad(widget.spinPitch->value()), pcl::deg2rad(widget.spinRoll->value()), pcl::deg2rad(widget.spinYaw->value()), widget.spinX->value(), widget.spinY->value(), widget.spinZ->value()); m_ptCloud.showPointCloud(m_ptCloud.triangulation); } else { pcl::PointIndices ind; m_ptCloud.conditionalRemoval(0, 10, -4, 4, -5, 5, m_ptCloud.original, ind, m_ptCloud.cond_removal); pcl::ModelCoefficients coeff; pcl::PointIndices inliers; this->m_ptCloud.detectOnePlane(m_ptCloud.cond_removal, ind, &inliers, 0.03, &coeff); double pitch, roll, yaw; m_ptCloud.getPlaneRadiansInclination(coeff, &pitch, &roll, &yaw); m_ptCloud.rotTransCloud(m_ptCloud.original, m_ptCloud.triangulation, -pitch, -roll, 0.0, 0.0, 0.0, 1.65); m_ptCloud.showPointCloud(m_ptCloud.triangulation); m_ptCloud.getPlaneDegreeInclination(coeff, &pitch, &roll, &yaw); printf("Rotated: X(%.3lf) Y(%.3lf) Z(%.3lf)\n", -pitch, -roll, 0.0); } } void KittiForm::calcMainPlane() { pcl::PointIndices ind; m_ptCloud.conditionalRemoval(0, 20, -4, 4, -5, 5, m_ptCloud.triangulation, ind, m_ptCloud.cond_removal); pcl::ModelCoefficients coeff; pcl::PointIndices inliers; this->m_ptCloud.detectOnePlane(m_ptCloud.cond_removal, ind, &inliers, 0.03, &coeff); double pitch, roll, yaw; m_ptCloud.getPlaneDegreeInclination(coeff, &pitch, &roll, &yaw); printf("MainPlane: X(%.3lf) Y(%.3lf) Z(%.3lf)\n", pitch, roll, yaw); } void KittiForm::showFrame() { char imgName[50]; char fullName[500]; sprintf(imgName, "%06d.png", widget.spinFrame->value());//change here sprintf(fullName, "%s/%s/%s/image_0%d/data/%s", widget.txtDataSetDir->text().toStdString().data(), getSelectedSceneTxt().toStdString().data(), widget.listSequences->currentItem()->text().toStdString().data(), getSelectedCam(), imgName); cv::Mat imgLeft = cv::imread(fullName, CV_LOAD_IMAGE_GRAYSCALE); m_stereo.setUndistLeft(imgLeft); QImage qImage = QImage(fullName); if (getSelectedCam() > 1) {//color cameras m_pixmap = QPixmap::fromImage(qImage, Qt::ColorOnly); } else { m_pixmap = QPixmap::fromImage(qImage, Qt::MonoOnly); } m_pixmap = m_pixmap.scaled(widget.imgFrame->width(), widget.imgFrame->height(), Qt::KeepAspectRatio); m_frameQT.clear(); m_frameQT.addPixmap(m_pixmap); } void KittiForm::openSequence() { printf("openSequence\n"); char imgName[500]; if (widget.listSequences->currentItem() != NULL) { sprintf(imgName, "%s/%s/%s/image_00/data", widget.txtDataSetDir->text().toStdString().data(), getSelectedSceneTxt().toStdString().data(), widget.listSequences->currentItem()->text().toStdString().data()); QDirIterator it(QString(imgName), QDirIterator::NoIteratorFlags); int fileCount = 0; while (it.hasNext()) { it.next(); if (it.fileInfo().isFile() && it.fileName().compare(".") != 0 && it.fileName().compare("..") != 0) { fileCount++; } } widget.sliderFrame->setEnabled(true); widget.sliderFrame->setMaximum(fileCount); widget.spinFrame->setEnabled(true); widget.spinFrame->setMaximum(fileCount); widget.sliderFrame->setValue(0); //init stereo kitti //if (widget.chkComputeStereo->isChecked() || widget.chkDispImg->isChecked()) { string calibDate = widget.listSequences->currentItem()->text().toStdString().substr(0, 11).append("calib/calib_cam_to_cam.txt"); string calibName = widget.txtDataSetDir->text().toStdString().append("/").append(getSelectedSceneTxt().toStdString()).append("/").append(calibDate); cout<<"calib name: "<text().toStdString().data(), getSelectedSceneTxt().toStdString().data(), widget.listSequences->currentItem()->text().toStdString().data()); char imgName2[500]; sprintf(imgName2, "%s/%s/%s/image_01/data", widget.txtDataSetDir->text().toStdString().data(), getSelectedSceneTxt().toStdString().data(), widget.listSequences->currentItem()->text().toStdString().data()); char imgName3[500]; sprintf(imgName3, "%s/%s/%s/pcdStereo", widget.txtDataSetDir->text().toStdString().data(), getSelectedSceneTxt().toStdString().data(), widget.listSequences->currentItem()->text().toStdString().data()); m_stereo.initKittiStereo(calibName, string(imgName), string(imgName2), string(imgName3)); m_ptCloud.startPclVisualizerThread("POINT_CLOUD"); //} loadDispImage(); this->calculateCRFcount(); } else { printf("OpenSequence.currentItem == NULL\n"); } } //int KittiForm::createPcdFilesFromStereo(string leftCamDir, string rightCamDir, string pcdDir){ // QString dirName = widget.txtDataSetDir->text(); // dirName.append(getSelectedSceneTxt()); // widget.listSequences->clear(); // QDirIterator it(dirName, QDirIterator::NoIteratorFlags); // while (it.hasNext()) { // // if (it.fileInfo().isDir() && it.fileName().compare(".") != 0 && it.fileName().compare("..") != 0) { // widget.listSequences->addItem(it.fileName()); // } // it.next(); // } //} void KittiForm::updateSequencesList() { printf("updateSequencesList\n"); QString dirName = widget.txtDataSetDir->text(); dirName.append("/").append(getSelectedSceneTxt()); widget.listSequences->clear(); QDirIterator it(dirName, QDirIterator::NoIteratorFlags); while (it.hasNext()) { it.next(); if (it.fileInfo().isDir() && it.fileName().compare(".") != 0 && it.fileName().compare("..") != 0 && it.fileName().contains(QString("drive"), Qt::CaseSensitive)) { widget.listSequences->addItem(it.fileName()); } } } void KittiForm::algorithm_01() { char cloudName[500]; char fullName[500]; const double minX = widget.spinAlgMinX->value(); const double maxX = 1.00; const double minY = widget.spinAlgMinY->value(); const double maxY = 1.00; const int colorCnt = 6; const int colors[][3] = { {0, 255, 0}, //green {255, 255, 0}, //yellow {255, 0, 0}, //red {0, 255, 255}, //light blue {138, 43, 226}, //violet {160, 82, 45}, //brown {255, 0, 255}//pink }; string colorsName[] = {string("green"), string("yellow"), string("red"), string("light blue"), string("violet"), string("brown"), string("pink")}; QPalette qtPalette[colorCnt]; for (int i = 0; i < colorCnt; i++) { QPalette myPalette(widget.spinAlgPlane_1->palette()); myPalette.setColor(QPalette::All, QPalette::Base, QColor(colors[i][0], colors[i][1], colors[i][2])); qtPalette[i] = myPalette; } widget.spinAlgPlane_1->setPalette(qtPalette[0]); widget.spinAlgPlane_2->setPalette(qtPalette[1]); widget.spinAlgPlane_3->setPalette(qtPalette[2]); widget.spinAlgPlane_4->setPalette(qtPalette[3]); widget.spinAlgPlane_5->setPalette(qtPalette[4]); widget.spinAlgPlane_6->setPalette(qtPalette[5]); //different thresholds for each plane double planeThres[] = {widget.spinAlgPlane_1->value(), widget.spinAlgPlane_2->value(), widget.spinAlgPlane_3->value(), widget.spinAlgPlane_4->value(), widget.spinAlgPlane_5->value(), widget.spinAlgPlane_6->value()}; if (widget.chkAlgCompute->isChecked()) { if (widget.chkAlgReadCloud->isChecked()) { // m_ptCloud.removeAllObjects(); //read conditional removal cloud sprintf(cloudName, "%010d_01.pcd", widget.spinFrame->value()); sprintf(fullName, "%s/%s/%s/pcdStereo/%s", widget.txtDataSetDir->text().toStdString().data(), getSelectedSceneTxt().toStdString().data(), widget.listSequences->currentItem()->text().toStdString().data(), cloudName); this->m_ptCloud.readCloudFromFile(string(fullName), m_ptCloud.original); //read normal cloud sprintf(cloudName, "%010d_01_normals_r10.pcd", widget.spinFrame->value()); sprintf(fullName, "%s/%s/%s/pcdStereo/%s", widget.txtDataSetDir->text().toStdString().data(), getSelectedSceneTxt().toStdString().data(), widget.listSequences->currentItem()->text().toStdString().data(), cloudName); this->m_ptCloud.readCloudNormalFromFile(string(fullName), m_ptCloud.original); pcl::PointIndices indices[10]; int indicesCnt = 0; pcl::PointIndices rem, avail, aux, totalUsed; //remove obstacles this->m_ptCloud.removeObstacles(m_ptCloud.original, m_ptCloud.mls, minX, maxX, minY, maxY, &rem, widget.chkAlgObst->isChecked()); totalUsed.indices.insert(totalUsed.indices.end(), rem.indices.begin(), rem.indices.end()); m_ptCloud.removeIndices(m_ptCloud.original, totalUsed, &avail, m_ptCloud.tmp); // printf("[obst] originalCloud(%d) - totalUsed(%d) obst(%d) = available(%d)\n", // m_ptCloud.getPointCount(m_ptCloud.original), // totalUsed.indices.size(), rem.indices.size(), // avail.indices.size()); if (widget.chkAlgCloser->isChecked()) { //select closer points to calculate main plane. this->m_ptCloud.conditionalRemoval(0.0, 10.0, -2.5, 2.5, -5.0, 5.0, m_ptCloud.original, avail, &aux); //"aux" store removed point list --> pcl::PointIndices totUsedTmp = totalUsed; totUsedTmp.indices.insert(totUsedTmp.indices.end(), aux.indices.begin(), aux.indices.end()); m_ptCloud.removeIndices(m_ptCloud.original, totUsedTmp, &avail, m_ptCloud.tmp); // printf("[cond] originalCloud(%d) - totalUsed(%d) cond(%d) = available(%d)\n", // m_ptCloud.getPointCount(m_ptCloud.original), // totUsedTmp.indices.size(), aux.indices.size(), // avail.indices.size()); } //extract main plane pcl::ModelCoefficients coeff; m_ptCloud.detectOnePlane(m_ptCloud.original, avail, &rem, planeThres[0], &coeff); indices[indicesCnt] = rem; indicesCnt++; //remove used indices from cloud totalUsed.indices.insert(totalUsed.indices.end(), rem.indices.begin(), rem.indices.end()); m_ptCloud.removeIndices(m_ptCloud.original, totalUsed, &avail, m_ptCloud.tmp); double xDeg, yDeg, zDeg; Eigen::Vector4f centroid; m_ptCloud.getPlaneDegreeInclination(coeff, &xDeg, &yDeg, &zDeg); m_ptCloud.calcCentroid(m_ptCloud.original, rem, ¢roid); printf("[%d](%10s) X(%06.2f) Y(%06.2f) Z(%06.2f) plane(%06d) x(%06.2f) y(%06.2f) z(%06.2f) available(%06d)\n", 0, colorsName[0].data(), xDeg, yDeg, zDeg, rem.indices.size(), centroid[0], centroid[1], centroid[2], avail.indices.size()); // //add plane to view // char name[15]; // sprintf(name, "plane%d", 0); // m_ptCloud.addPlaneToView(coeff, string(name)); //detect other planes for (int i = 1; i < widget.spinAlgRanges->value(); i++) { m_ptCloud.detectOnePlane(m_ptCloud.original, avail, &rem, planeThres[i], &coeff); indices[indicesCnt] = rem; indicesCnt++; totalUsed.indices.insert(totalUsed.indices.end(), rem.indices.begin(), rem.indices.end()); m_ptCloud.removeIndices(m_ptCloud.original, totalUsed, &avail, m_ptCloud.tmp); m_ptCloud.getPlaneDegreeInclination(coeff, &xDeg, &yDeg, &zDeg); m_ptCloud.calcCentroid(m_ptCloud.original, rem, ¢roid); printf("[%d](%10s) X(%06.2f) Y(%06.2f) Z(%06.2f) plane(%06d) x(%06.2f) y(%06.2f) z(%06.2f) available(%06d)\n", i, colorsName[i].data(), xDeg, yDeg, zDeg, rem.indices.size(), centroid[0], centroid[1], centroid[2], avail.indices.size()); // //add plane to view // char name[15]; // sprintf(name, "plane%d", i); // m_ptCloud.addPlaneToView(coeff, string(name)); } //paint points for (int i = 0; i < indicesCnt; i++) { for (int j = 0; j < indices[i].indices.size(); j++) { int idx = indices[i].indices.at(j); m_ptCloud.setPointColor(m_ptCloud.original, idx, colors[i][0], colors[i][1], colors[i][2]); } } //for each point, set color depending on distance: //[0.00 .. planeThres] green //[planeThres .. planeThres+0.05] yellow //[planeThres+0.05 .. planeThres+0.10] red //[> planeThres+0.10] ignored // const double ranges[] = {0.00, widget.spinAlgPlane->value(), // widget.spinAlgPlane->value() + 0.05, // widget.spinAlgPlane->value() + 0.10}; // for (int i = 0; i < m_ptCloud.getPointCount(m_ptCloud.original); i++) { // double dist = m_ptCloud.distPtPlane(m_ptCloud.getPoint(m_ptCloud.original, i), coeff); // bool found = false; // for (int j = 0; j < widget.spinAlgRanges->value() && !found; j++) { // if (dist >= ranges[j] && dist < ranges[j + 1]) { // m_ptCloud.setPointColor(m_ptCloud.original, i, colors[j][0], colors[j][1], colors[j][2]); // found = true; // } // } // } m_ptCloud.showPointCloud(m_ptCloud.original); } } } void KittiForm::computeStereoToPointCloud() { char currentDir[500]; PclPointCloud myCloud; //compute point clouds for all the scene if (widget.radioStCurrScene->isChecked()) { sprintf(currentDir, "%s/%s", widget.txtDataSetDir->text().toStdString().data(), this->getSelectedSceneTxt().toStdString().data()); } else if (widget.radioStCurrFrame->isChecked()) { sprintf(currentDir, "%s/%s/%s", widget.txtDataSetDir->text().toStdString().data(), this->getSelectedSceneTxt().toStdString().data(), widget.listSequences->currentItem()->text().toStdString().data()); } QDirIterator it(QString(currentDir), QDirIterator::Subdirectories); int fileCount = 0; time_t time_end, time_st; char logFileName[500]; sprintf(logFileName, "%s/stereoToCloud.log", currentDir); FILE *fd_log = fopen(logFileName, "w"); while (it.hasNext()) { it.next(); if (it.fileInfo().isFile() && it.filePath().contains("image_00", Qt::CaseSensitive) && it.filePath().contains(".png", Qt::CaseSensitive)) { // fileCount++; // } time_st = time(NULL); fileCount++; string left = it.filePath().toStdString(); int pos = left.find("image_00", 0); string right = left; right.replace(pos, 8, "image_01"); string cloud = left.substr(0, pos); cloud.append("pcdStereo/").append(it.fileName().toStdString()); cloud.replace(cloud.length() - 3, 3, "pcd"); string img25 = left.substr(0, pos); img25.append("img25/").append(it.fileName().toStdString()); img25.replace(img25.length() - 3, 3, "yml"); cv::Mat aux; if (widget.chkSGM->isChecked()) { m_stereo.sgbmKitti(left, right, &aux, widget.spinMinDisp->value(), widget.spinDisp->value(), widget.spinSAD->value(), widget.spinSGMp1->value(), widget.spinSGMp2->value(), widget.spinDisp12MaxDiff->value(), widget.spinPreFiltCap->value(), widget.spinUnique->value(), widget.spinSpeckWind->value(), widget.spinSpeckRange->value()); } else { m_stereo.exampleOpencvBaseKitti(left, right, &aux, widget.spinDisp->value(), widget.spinSAD->value(), widget.spinPreFiltSize->value(), widget.spinPreFiltCap->value(), widget.spinMinDisp->value(), widget.spinTextureTh->value(), widget.spinUnique->value(), widget.spinSpeckWind->value(), widget.spinSpeckRange->value(), widget.spinDisp12MaxDiff->value()); } m_stereo.compute3dKitti(cloud, img25, true, widget.spinPitch->value(), widget.spinRoll->value(), widget.spinYaw->value(), widget.spinX->value(), widget.spinY->value(), widget.spinZ->value(), widget.chkAngles->isChecked()); printf("Cloud: %s\n", cloud.data()); string condCloud = cloud; condCloud.insert(condCloud.length() - 4, "_01"); double curvRad = 0.10; int kNN = 30; // myCloud.readCloudFromFile(cloud.data(), myCloud.original); pcl::PointIndices ind; myCloud.readCloudFromFile(cloud.data(), myCloud.original); myCloud.conditionalRemoval(0, 50, -50, 50, -10, 10, myCloud.original, ind, myCloud.cond_removal); myCloud.writeCloudToFile(condCloud, myCloud.cond_removal); char logName[300]; sprintf(logName, "%s/principalCurvatures.log", it.path().toStdString().data()); myCloud.principalCurvatures(myCloud.cond_removal, condCloud, string(""), string(logName), curvRad, curvRad); // myCloud.principalCurvatures(myCloud.cond_removal, condCloud, string(""), string(logName), kNN); time_end = time(NULL); fprintf(fd_log, "%010d\t%04d\t%s\n", fileCount, time_end - time_st, cloud.data()); } else { if (it.fileInfo().isDir() && it.fileName().contains("drive", Qt::CaseSensitive)) { string pcdDir = it.filePath().append("/pcdStereo").toStdString(); string img25Dir = it.filePath().append("/img25").toStdString(); string cmd = string("mkdir ").append(pcdDir); string cmd2 = string("mkdir ").append(img25Dir); string cmd3 = string("rm ").append(img25Dir).append("/*.png"); system(cmd.data()); system(cmd2.data()); system(cmd3.data()); // char calibName[500]; // sprintf(calibName, "%s"); string aux = it.filePath().toStdString(); string calibDate = aux.substr(0, aux.length() - 10).append("calib/calib_cam_to_cam.txt"); //printf("CalibrationFile: %s\n",calibDate.data()); m_stereo.initKittiStereo(calibDate, string(""), string(""), string("")); } } } fclose(fd_log); } void KittiForm::openDir() { // cv::FileStorage fs(string("/media/BBDD_KITI/KITTI_rawdata/city/2011_09_26_drive_0001/img25/0000000000.yml"), // cv::FileStorage::READ); // // cv::Mat img25; // fs["img25"] >> img25; // fs.release(); if (widget.chkOpenDir->isChecked()) { QString dirName = QFileDialog::getExistingDirectory(this, tr("Open KITTI Directory"), "", QFileDialog::ShowDirsOnly); widget.txtDataSetDir->setText(dirName); } updateSequencesList(); } int KittiForm::getSelectedScene() { int ret = -1; if (widget.radioCampus->isChecked()) ret = CAMPUS; else if (widget.radioCity->isChecked()) ret = CITY; else if (widget.radioPerson->isChecked()) ret = PERSON; else if (widget.radioResidential->isChecked()) ret = RESIDENTIAL; else if (widget.radioRoad->isChecked()) ret = ROAD; return ret; } int KittiForm::getSelectedCam() { int ret = -1; if (widget.radioLBW->isChecked()) ret = LEFT_BW; else if (widget.radioRBW->isChecked()) ret = RIGHT_BW; else if (widget.radioLC->isChecked()) ret = LEFT_C; else if (widget.radioRC->isChecked()) ret = RIGHT_C; return ret; } QString KittiForm::getSelectedSceneTxt() { QString ret = ""; if (widget.radioCampus->isChecked()) ret = "campus"; else if (widget.radioCity->isChecked()) ret = "city"; else if (widget.radioPerson->isChecked()) ret = "person"; else if (widget.radioResidential->isChecked()) ret = "residential"; else if (widget.radioRoad->isChecked()) ret = "road"; return ret; } void KittiForm::computeNormals() { this->m_ptCloud.readColorScale(string("/home/carlos/MyThesisCode/mat/colorJet1001.txt")); this->m_ptCloud.principalCurvatures(m_ptCloud.cond_removal, 0.25); pcl::PointCloud::Ptr aux = pcl::PointCloud::Ptr(new pcl::PointCloud); m_ptCloud.getCloud(aux,m_ptCloud.cond_removal); m_ptCloud.setCloud(aux,m_ptCloud.triangulation); this->m_ptCloud.showCurvatures(m_ptCloud.triangulation, 7, this->widget.spinNormalTh->value()); } void KittiForm::pointNormal(){ pcl::PointCloud::Ptr aux = pcl::PointCloud::Ptr(new pcl::PointCloud); m_ptCloud.getCloud(aux,m_ptCloud.cond_removal); m_ptCloud.setCloud(aux,m_ptCloud.triangulation); m_ptCloud.computeRegionNormals(m_ptCloud.triangulation, widget.spinRadNormal->value(), widget.spinNN_X->value(), widget.spinNN_Y->value(), widget.spinNN_Z->value()); // m_ptCloud.showPointCloud(m_ptCloud.triangulation); } void KittiForm::alternate() { static bool change = false; // if (change) // this->m_ptCloud.showCurvatures(m_ptCloud.cond_removal, 3, this->widget.spinNormalTh->value()); // else{ // this->m_ptCloud.showCurvatures(m_ptCloud.cond_removal, 7, this->widget.spinNormalTh->value()); // // } pcl::PointCloud::Ptr curv = pcl::PointCloud::Ptr(new pcl::PointCloud); pcl::PointCloud::Ptr cloudxyz = pcl::PointCloud::Ptr(new pcl::PointCloud); m_ptCloud.getCurvaturesCloud(curv); m_ptCloud.getCloud(cloudxyz,m_ptCloud.cond_removal); m_stereo.showCurvImg(curv, cloudxyz, m_stereo.getUndistLeft(), (float)this->widget.spinNormalTh->value(), 255, 0, 0); m_ptCloud.setCloud(cloudxyz,m_ptCloud.ror); m_ptCloud.showPointCloud(m_ptCloud.ror); // change = !change; } void KittiForm::computeDEM() { cv::Mat imgDem; // char pcdName[500]; // char pcdFullName[500]; // // // sprintf(pcdName, "%010d_01.pcd", widget.spinFrame->value()); // sprintf(pcdFullName, "%s%s/%s/pcdStereo/%s", // widget.txtDataSetDir->text().toStdString().data(), // getSelectedSceneTxt().toStdString().data(), // widget.listSequences->currentItem()->text().toStdString().data(), // pcdName); // pcl::PointCloud::Ptr demCloud = pcl::PointCloud::Ptr(new pcl::PointCloud); demCloud->points.clear(); m_ptCloud.getCloud(demCloud, m_ptCloud.triangulation); // if (pcl::io::loadPCDFile (string(pcdFullName), *(demCloud)) == -1) { // printf("Error Reading File: %s\n", pcdFullName); // } // else { cv::Mat dem; printf("*** DEM computation *** cloud = %d pts\n", demCloud->points.size()); this->m_stereo.computeDEM_maxHistEfficientBinary(this->m_stereo.getUndistLeft(), this->m_stereo.getImg3D(), demCloud, widget.spinCellWidth->value(), widget.spinCellHeight->value(), widget.spinBeansSize->value(), 0, &imgDem, &dem); m_ptCloud.setCloud(demCloud, m_ptCloud.cond_removal); m_ptCloud.showPointCloud(m_ptCloud.cond_removal); m_stereo.showDEM(dem, m_stereo.getUndistLeft(), string("initialDEM")); //save DEM to xml file if (widget.chkSaveDEM->isChecked()) { char demFileName[500]; sprintf(demFileName, PATTERN_DEM.data(), this->getSelectedSceneTxt().toStdString().data(), widget.listSequences->currentItem()->text().toStdString().data(), widget.spinFrame->value()); m_stereo.saveDemToBinFile(dem, string(demFileName)); m_ptCloud.writeCloudToFile(string("/tmp/cloud.pcd"),m_ptCloud.cond_removal); } m_dem = dem; } void KittiForm::readDEM() { char demFileName[500]; sprintf(demFileName, PATTERN_DEM.data(), this->getSelectedSceneTxt().toStdString().data(), widget.listSequences->currentItem()->text().toStdString().data(), widget.spinFrame->value()); m_stereo.readDemFromBinFile(string(demFileName), &m_dem); m_stereo.showDEM(m_dem, m_stereo.getUndistLeft(), string(demFileName)); } void KittiForm::readCRF() { ///media/DATOS/Videos/CRF/%s_%s_%010d_p%03d_l%03d_t%02d.bin char demFileName[500]; sprintf(demFileName, PATTERN_CRF_NAME.data(), this->getSelectedSceneTxt().toStdString().data(), widget.listSequences->currentItem()->text().toStdString().data(), widget.spinFrame->value()); QString dirName = QString("/media/DATOS/Videos/CRF"); QDirIterator it(dirName, QDirIterator::NoIteratorFlags); while (it.hasNext()) { it.next(); if (it.fileName().compare(".") != 0 && it.fileName().compare("..") != 0 && it.fileName().contains(QString(demFileName), Qt::CaseInsensitive)) { m_stereo.readDemFromBinFile(it.filePath().toStdString(), &m_dem); m_stereo.showDEM(m_dem, m_stereo.getUndistLeft(), it.fileName().toStdString()); } } // char demFileName[500]; // sprintf(demFileName, PATTERN_CRF_NAME.data(), // this->getSelectedSceneTxt().toStdString().data(), // widget.listSequences->currentItem()->text().toStdString().data(), // widget.spinFrame->value()); // // m_stereo.readDemFromBinFile(string(demFileName), &m_dem); // m_stereo.showDEM(m_dem, m_stereo.getUndistLeft(), string(demFileName)); } void KittiForm::calculateCRFcount() { double cnt = (widget.spinCRFmax->value() - widget.spinCRFmin->value()) / widget.spinCRFres->value(); char count[100]; sprintf(count, "%d", (int) cnt); widget.txtCRFlabel->setText(QString(count)); } void KittiForm::computeCRF() { // CpuTime cpuTime; // //read DEM from file // char demFileName[500]; // sprintf(demFileName, PATTERN_DEM.data(), // this->getSelectedSceneTxt().toStdString().data(), // widget.listSequences->currentItem()->text().toStdString().data(), // widget.spinFrame->value()); // m_stereo.readDemFromBinFile(string(demFileName), &m_dem); // // //log file // sprintf(demFileName, PATTERN_CRFLOG.data(), // this->getSelectedSceneTxt().toStdString().data(), // widget.listSequences->currentItem()->text().toStdString().data(), // widget.spinFrame->value()); // // //init CRF // //int neighbor, int gridRows, int gridCols, int priorSatVal, int likeSatValue, string logFile, int labelCnt // CRF crf = CRF(4, m_dem.rows, m_dem.cols, // widget.spinCRFprior->value(), widget.spinCRFlike->value(), // string(demFileName), 100); // // vector channels; // cv::split(m_dem, channels); // cv::Mat dem = channels.at(4); // // crf.updateMeasurements(dem); // // //preCompute prior values // cpuTime.start(); // crf.computePriors(); // cpuTime.stop(); // printf("computePriors = %s\n", cpuTime.getText().data()); // // cpuTime.start(); // crf.computeLikelihoods(); // cpuTime.stop(); // printf("computeLikelihood = %s\n", cpuTime.getText().data()); // // //compute CRF // const int loopCount = 4; // for (int i = 0; i < loopCount; i++) { // // cpuTime.start(); // crf.computeMessages(); // crf.computeBeliefs(); // // cv::Mat result = crf.getResult(); // channels.at(4) = result; // cv::merge(channels, m_dem); // // //results xml file // sprintf(demFileName, PATTERN_CRF.data(), // this->getSelectedSceneTxt().toStdString().data(), // widget.listSequences->currentItem()->text().toStdString().data(), // widget.spinFrame->value(), widget.spinCRFprior->value(), widget.spinCRFlike->value(), i); // m_stereo.saveDemToBinFile(m_dem, string(demFileName)); // // cpuTime.stop(); // printf("loop %d finished. %s\n", i, cpuTime.getText().data()); // } } void KittiForm::calcGrid() { }