LoopClosureViewer.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include "rtabmap/gui/LoopClosureViewer.h"
00029 #include "ui_loopClosureViewer.h"
00030 
00031 #include "rtabmap/core/Memory.h"
00032 #include "rtabmap/core/util3d_filtering.h"
00033 #include "rtabmap/core/util3d_transforms.h"
00034 #include "rtabmap/core/util3d.h"
00035 #include "rtabmap/core/Signature.h"
00036 #include "rtabmap/utilite/ULogger.h"
00037 #include "rtabmap/utilite/UStl.h"
00038 
00039 #include <QtCore/QTimer>
00040 
00041 namespace rtabmap {
00042 
00043 LoopClosureViewer::LoopClosureViewer(QWidget * parent) :
00044         QWidget(parent),
00045         decimation_(1),
00046         maxDepth_(0),
00047         minDepth_(0)
00048 {
00049         ui_ = new Ui_loopClosureViewer();
00050         ui_->setupUi(this);
00051         ui_->cloudViewerTransform->setCameraLockZ(false);
00052 
00053         connect(ui_->checkBox_rawCloud, SIGNAL(clicked()), this, SLOT(updateView()));
00054 }
00055 
00056 LoopClosureViewer::~LoopClosureViewer() {
00057         delete ui_;
00058 }
00059 
00060 void LoopClosureViewer::setData(const Signature & sA, const Signature & sB)
00061 {
00062         sA_ = sA;
00063         sB_ = sB;
00064         if(sA_.id()>0 && sB_.id()>0)
00065         {
00066                 ui_->label_idA->setText(QString("[%1-%2]").arg(sA.id()).arg(sB.id()));
00067         }
00068 }
00069 
00070 void LoopClosureViewer::updateView(const Transform & transform, const ParametersMap & parameters)
00071 {
00072         if(sA_.id()>0 && sB_.id()>0)
00073         {
00074                 int decimation = 1;
00075                 float maxDepth = 0;
00076                 float minDepth = 0;
00077 
00078                 if(!ui_->checkBox_rawCloud->isChecked())
00079                 {
00080                         decimation = decimation_;
00081                         maxDepth = maxDepth_;
00082                         minDepth = minDepth_;
00083                 }
00084 
00085                 UDEBUG("decimation = %d", decimation);
00086                 UDEBUG("maxDepth = %f", maxDepth);
00087                 UDEBUG("minDepth = %d", minDepth);
00088 
00089                 Transform t;
00090                 if(!transform.isNull())
00091                 {
00092                         transform_ = transform;
00093                         t = transform;
00094                 }
00095                 else if(!transform_.isNull())
00096                 {
00097                         t = transform_;
00098                 }
00099                 else
00100                 {
00101                         t = sB_.getPose();
00102                 }
00103 
00104                 UDEBUG("t= %s", t.prettyPrint().c_str());
00105                 ui_->label_transform->setText(QString("(%1)").arg(t.prettyPrint().c_str()));
00106                 if(!t.isNull())
00107                 {
00108                         //cloud 3d
00109                         pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA, cloudB;
00110                         cloudA = util3d::cloudRGBFromSensorData(sA_.sensorData(), decimation, maxDepth, minDepth, 0, parameters);
00111                         cloudB = util3d::cloudRGBFromSensorData(sB_.sensorData(), decimation, maxDepth, minDepth, 0, parameters);
00112 
00113                         //cloud 2d
00114                         pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
00115                         scanA = util3d::laserScanToPointCloud(sA_.sensorData().laserScanRaw());
00116                         scanB = util3d::laserScanToPointCloud(sB_.sensorData().laserScanRaw());
00117                         scanB = util3d::transformPointCloud(scanB, t);
00118 
00119                         ui_->label_idA->setText(QString("[%1 (%2) -> %3 (%4)]").arg(sB_.id()).arg(cloudB->size()).arg(sA_.id()).arg(cloudA->size()));
00120 
00121                         if(cloudA->size())
00122                         {
00123                                 ui_->cloudViewerTransform->addCloud("cloud0", cloudA);
00124                         }
00125                         if(cloudB->size())
00126                         {
00127                                 cloudB = util3d::transformPointCloud(cloudB, t);
00128                                 ui_->cloudViewerTransform->addCloud("cloud1", cloudB);
00129                         }
00130                         if(scanA->size())
00131                         {
00132                                 ui_->cloudViewerTransform->addCloud("scan0", scanA);
00133                         }
00134                         if(scanB->size())
00135                         {
00136                                 ui_->cloudViewerTransform->addCloud("scan1", scanB);
00137                         }
00138                 }
00139                 else
00140                 {
00141                         UERROR("loop transform is null !?!?");
00142                         ui_->cloudViewerTransform->removeAllClouds();
00143                 }
00144                 ui_->cloudViewerTransform->update();
00145         }
00146 }
00147 
00148 
00149 void LoopClosureViewer::showEvent(QShowEvent * event)
00150 {
00151         QWidget::showEvent( event );
00152         QTimer::singleShot(500, this, SLOT(updateView())); // make sure the QVTKWidget is shown!
00153 }
00154 
00155 } /* namespace rtabmap */


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:20