MultiSessionLocWidget.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
30 #include "rtabmap/gui/ImageView.h"
31 #include "rtabmap/utilite/UStl.h"
32 #include "rtabmap/core/Graph.h"
33 
34 #include <QHBoxLayout>
35 #include <QPushButton>
36 #include <QProgressBar>
37 
38 namespace rtabmap {
39 
41  const QMap<int, Signature> * cache,
42  const std::map<int, int> * mapIds,
43  QWidget * parent) :
44  QWidget(parent),
45  cache_(cache),
46  mapIds_(mapIds),
47  totalFrames_(0),
48  totalLoops_(0)
49 {
50  UASSERT(cache != 0);
51  UASSERT(mapIds != 0);
52  imageView_ = new ImageView(this);
53  imageView_->setObjectName("multisession_imageview");
54  totalLocProgressBar_ = new QProgressBar(this);
55  resetbutton_ = new QPushButton(this);
56  resetbutton_->setText("Reset");
57 
58  // setup layout
59  this->setLayout(new QHBoxLayout());
60  QVBoxLayout * vLayout = new QVBoxLayout();
61  vLayout->addWidget(imageView_, 1);
62  QHBoxLayout * hLayout = new QHBoxLayout();
63  hLayout->addWidget(resetbutton_, 0);
64  hLayout->addWidget(totalLocProgressBar_, 1);
65  vLayout->addLayout(hLayout, 0);
66  ((QHBoxLayout*)this->layout())->addLayout(vLayout, 1);
67 
68  connect(resetbutton_, SIGNAL(clicked()), this, SLOT(clear()));
69 }
71 
73  const Signature & lastSignature,
74  const Statistics & stats)
75 {
76  ++totalFrames_;
77  std::multimap<int, Link> loopLinks = graph::filterLinks(lastSignature.getLinks(), Link::kGlobalClosure, true);
78  std::multimap<int, Link> localLinks = graph::filterLinks(lastSignature.getLinks(), Link::kLocalSpaceClosure, true);
79  loopLinks.insert(localLinks.begin(), localLinks.end());
80 
81  if(!loopLinks.empty())
82  {
83  ++totalLoops_;
84  totalLocProgressBar_->setValue(float(totalLoops_)/float(totalFrames_) * 100);
85  }
86 
87  if(!lastSignature.sensorData().imageRaw().empty() ||
88  !lastSignature.sensorData().imageCompressed().empty())
89  {
90  cv::Mat image;
91  lastSignature.sensorData().uncompressDataConst(&image, 0);
92  if(!image.empty())
93  {
96  imageView_->setBackgroundColor(Qt::black);
97  }
98  else
99  {
100  imageView_->clear();
101  }
102  }
103  else
104  {
105  imageView_->clear();
106  }
107 
108  std::map<int, std::pair<int, float> > top;
109  const std::map<int, float> & likelihood = stats.posterior();
110  for(std::map<int, float>::const_iterator iter=likelihood.begin(); iter!=likelihood.end(); ++iter)
111  {
112  if(mapIds_->find(iter->first) != mapIds_->end())
113  {
114  int mapId = mapIds_->at(iter->first);
115  if(subViews_.find(mapId)==subViews_.end())
116  {
117  MultiSessionLocSubView * subView = new MultiSessionLocSubView(imageView_, mapId, this);
118  ((QHBoxLayout*)this->layout())->addWidget(subView, 1);
119  subViews_.insert(std::make_pair(mapId, std::make_pair(subView, 0)));
120  }
121 
122  if(top.find(mapId) == top.end() || top.at(mapId).second < iter->second)
123  {
124  uInsert(top, std::make_pair(mapId, std::make_pair(iter->first, iter->second)));
125  }
126  }
127  }
128 
129  // update highest loop closure hypotheses per session
130  for(std::map<int, std::pair<MultiSessionLocSubView*, int> >::iterator iter=subViews_.begin(); iter!=subViews_.end(); ++iter)
131  {
132 
133  for(std::multimap<int, Link>::iterator jter=loopLinks.begin(); jter!=loopLinks.end(); ++jter)
134  {
135  if(mapIds_->find(jter->first)!= mapIds_->end())
136  {
137  int mapId = mapIds_->at(jter->first);
138  iter->second.second += mapId==iter->first?1:0;
139  }
140  }
141 
142  if(uContains(top, iter->first))
143  {
144  int nodeId = top.at(iter->first).first;
145  if(cache_->contains(nodeId))
146  {
147  cv::Mat image;
148  const Signature & s = (*cache_)[nodeId];
149 
150  Link link = loopLinks.find(nodeId) != loopLinks.end()?loopLinks.find(nodeId)->second:Link();
151  std::multimap<int, cv::KeyPoint> keypoints;
152  for(std::multimap<int, int>::const_iterator jter=s.getWords().begin(); jter!=s.getWords().end(); ++jter)
153  {
154  if(jter->first>0 && lastSignature.getWords().find(jter->first) != lastSignature.getWords().end())
155  {
156  keypoints.insert(std::make_pair(jter->first, s.getWordsKpts()[jter->second]));
157  }
158  }
159 
160  if(!keypoints.empty())
161  {
162  s.sensorData().uncompressDataConst(&image, 0);
163  if(!image.empty())
164  {
165  iter->second.first->updateView(
166  nodeId,
167  uCvMat2QImage(image),
168  keypoints,
169  float(iter->second.second)/float(totalFrames_),
170  link.type() == Link::kLocalSpaceClosure?Qt::yellow:
171  link.type() == Link::kGlobalClosure?Qt::green:Qt::gray);
172  }
173  else
174  {
175  iter->second.first->clear();
176  }
177  }
178  else
179  {
180  iter->second.first->clear();
181  }
182  }
183  else
184  {
185  iter->second.first->clear();
186  }
187  }
188  else
189  {
190  iter->second.first->clear();
191  }
192  }
193 }
195 {
196  for(std::map<int, std::pair<MultiSessionLocSubView*, int> >::iterator iter=subViews_.begin(); iter!=subViews_.end(); ++iter)
197  {
198  delete iter->second.first;
199  }
200  subViews_.clear();
201  imageView_->clear();
202  totalFrames_ = 0;
203  totalLoops_ = 0;
204  totalLocProgressBar_->setValue(0);
205 }
206 
207 } /* namespace rtabmap */
rtabmap::MultiSessionLocWidget::imageView_
ImageView * imageView_
Definition: MultiSessionLocWidget.h:68
ImageView.h
rtabmap::Statistics
Definition: Statistics.h:53
rtabmap::SensorData::uncompressDataConst
void uncompressDataConst(cv::Mat *imageRaw, cv::Mat *depthOrRightRaw, LaserScan *laserScanRaw=0, cv::Mat *userDataRaw=0, cv::Mat *groundCellsRaw=0, cv::Mat *obstacleCellsRaw=0, cv::Mat *emptyCellsRaw=0) const
Definition: SensorData.cpp:623
rtabmap::Signature::getLinks
const std::multimap< int, Link > & getLinks() const
Definition: Signature.h:101
rtabmap::MultiSessionLocWidget::totalLoops_
int totalLoops_
Definition: MultiSessionLocWidget.h:72
s
RealScalar s
MultiSessionLocSubView.h
rtabmap::Signature::getWords
const std::multimap< int, int > & getWords() const
Definition: Signature.h:112
rtabmap::MultiSessionLocWidget::mapIds_
const std::map< int, int > * mapIds_
Definition: MultiSessionLocWidget.h:66
rtabmap::Signature::getWordsKpts
const std::vector< cv::KeyPoint > & getWordsKpts() const
Definition: Signature.h:113
rtabmap::SensorData::imageCompressed
const cv::Mat & imageCompressed() const
Definition: SensorData.h:179
rtabmap::MultiSessionLocWidget::MultiSessionLocWidget
MultiSessionLocWidget(const QMap< int, Signature > *cache, const std::map< int, int > *mapIds, QWidget *parent=0)
Definition: MultiSessionLocWidget.cpp:40
iterator
rtabmap::MultiSessionLocWidget::~MultiSessionLocWidget
virtual ~MultiSessionLocWidget()
Definition: MultiSessionLocWidget.cpp:70
rtabmap::ImageView::setBackgroundColor
void setBackgroundColor(const QColor &color)
Definition: ImageView.cpp:705
rtabmap::MultiSessionLocWidget::totalLocProgressBar_
QProgressBar * totalLocProgressBar_
Definition: MultiSessionLocWidget.h:69
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:441
rtabmap::MultiSessionLocWidget::subViews_
std::map< int, std::pair< MultiSessionLocSubView *, int > > subViews_
Definition: MultiSessionLocWidget.h:67
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:407
rtabmap::MultiSessionLocWidget::clear
void clear()
Definition: MultiSessionLocWidget.cpp:194
rtabmap::ImageView::setImage
void setImage(const QImage &image, const std::vector< CameraModel > &models=std::vector< CameraModel >(), const Transform &pose=Transform())
Definition: ImageView.cpp:1235
rtabmap::MultiSessionLocWidget::updateView
void updateView(const Signature &lastSignature, const Statistics &stats)
Definition: MultiSessionLocWidget.cpp:72
stats
bool stats
rtabmap::ImageView::getDefaultMatchingFeatureColor
const QColor & getDefaultMatchingFeatureColor() const
Definition: ImageView.cpp:394
UASSERT
#define UASSERT(condition)
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
Graph.h
iter
iterator iter(handle obj)
rtabmap::graph::filterLinks
std::multimap< int, Link > RTABMAP_CORE_EXPORT filterLinks(const std::multimap< int, Link > &links, Link::Type filteredType, bool inverted=false)
Definition: Graph.cpp:1252
rtabmap::MultiSessionLocSubView
Definition: MultiSessionLocSubView.h:41
UStl.h
Wrappers of STL for convenient functions.
rtabmap::ImageView
Definition: ImageView.h:49
rtabmap::MultiSessionLocWidget::cache_
const QMap< int, Signature > * cache_
Definition: MultiSessionLocWidget.h:65
uCvMat2QImage
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack, float depthMin=0, float depthMax=0)
Definition: UCv2Qt.h:47
rtabmap::MultiSessionLocWidget::resetbutton_
QPushButton * resetbutton_
Definition: MultiSessionLocWidget.h:70
rtabmap::ImageView::setFeatures
void setFeatures(const std::multimap< int, cv::KeyPoint > &refWords, const cv::Mat &depth=cv::Mat(), const QColor &color=Qt::yellow)
Definition: ImageView.cpp:1137
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:138
rtabmap::ImageView::clear
void clear()
Definition: ImageView.cpp:1451
rtabmap
Definition: CameraARCore.cpp:35
MultiSessionLocWidget.h
rtabmap::MultiSessionLocWidget::totalFrames_
int totalFrames_
Definition: MultiSessionLocWidget.h:71
rtabmap::Signature
Definition: Signature.h:48


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:13