Main MRPT website > C++ reference for MRPT 1.9.9
PbMapLocaliser.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 /* Plane-based Map (PbMap) library
11  * Construction of plane-based maps and localization in it from RGBD Images.
12  * Writen by Eduardo Fernandez-Moral. See docs for <a
13  * href="group__mrpt__pbmap__grp.html" >mrpt-pbmap</a>
14  */
15 #include "pbmap-precomp.h" // Precompiled headers
16 
17 #if MRPT_HAS_PCL
18 
19 #include <mrpt/pbmap/PbMapLocaliser.h>
20 #include <mrpt/pbmap/SubgraphMatcher.h>
21 #include <mrpt/pbmap/heuristicParams.h>
22 #include <mrpt/config/CConfigFile.h>
23 #include <pcl/io/pcd_io.h>
24 #include <pcl/common/time.h>
25 #include <fstream>
26 #include <iostream>
27 
28 using namespace std;
29 using namespace mrpt::pbmap;
30 
31 double time1, time2;
32 
33 PbMapLocaliser::PbMapLocaliser(PbMap& mPbM, const string& config_file)
34  : mPbMap(mPbM),
35  m_pbMapLocaliser_must_stop(false),
36  m_pbMapLocaliser_finished(false)
37 {
38  matcher.configLocaliser.load_params(config_file);
39  std::cout << "PbMapLocaliser::PbMapLocaliser min_planes_recognition "
40  << matcher.configLocaliser.min_planes_recognition
41  << " color thresholds " << matcher.configLocaliser.color_threshold
42  << " " << matcher.configLocaliser.intensity_threshold << " "
43  << matcher.configLocaliser.hue_threshold << endl;
44 
45  LoadPreviousPbMaps("/home/edu/newPbMaps/PbMaps.txt");
46 
47  pbMapLocaliser_hd = std::thread(&PbMapLocaliser::run, this);
48 }
49 
50 // Load Previous PbMaps
51 void PbMapLocaliser::LoadPreviousPbMaps(std::string fileMaps)
52 {
53  cout << "PbMapLocaliser::LoadPreviousPbMaps(...)\n";
54  std::ifstream configFile;
55  configFile.open(fileMaps.c_str());
56 
57  string mapFile, filepath = "/home/edu/newPbMaps/";
58  totalPrevPlanes = 0;
59  while (!configFile.eof())
60  {
61  // floorPlane = -1;
62  getline(configFile, mapFile);
63  if (mapFile == "") break;
64 
65  if (mapFile.at(0) == '%') continue;
66  cout << "Load map: " << filepath << mapFile << endl;
67 
68  PbMap previousPbMap;
69  previousPbMap.loadPbMap(filepath + mapFile);
70 
71  previousPbMapNames.push_back(mapFile);
72  previousPbMaps.push_back(previousPbMap);
73 
74  // prevPbMap.vPlaness.push_back(vPlanesTemp);
75  // previousPbMapNames.push_back(mapFile);
76  // vFloors.push_back(floorPlane);
77  totalPrevPlanes += previousPbMap.vPlanes.size();
78  // cout << "PbMap loaded has " << previousPbMap.vPlanes.size() << "
79  // planes" << endl;
80  }
81  // cout << "Size: " << totalPrevPlanes << " " << vFloors.size() << endl;
82  // cout << "Floors: ";
83  // for(unsigned i=0; i< vFloors.size(); i++)
84  // cout << vFloors[i] << " ";
85  // cout << endl;
86 
87  configFile.close();
88  cout << "PbMapLocaliser:: previous PbMaps loaded\n";
89 }
90 
91 // Check 2nd order neighbors of the reference plane
92 void PbMapLocaliser::compareSubgraphNeighbors(SubgraphMatcher& matcher)
93 {
94  cout << "PbMapLocaliser::compareSubgraphNeighbors\n";
95  PbMap& matchedPbMap = previousPbMaps[bestMap];
96 
97  for (map<unsigned, unsigned>::iterator it = bestMatch.begin();
98  it != bestMatch.end(); it++)
99  {
100  // TODO: use the graph to grow
101  if (matcher.configLocaliser.graph_mode == 0) // Nearby neighbors -
102  // Check the 2nd order
103  // neighbors of the
104  // matched planes
105  for (set<unsigned>::iterator it1 =
106  mPbMap.vPlanes[it->first].nearbyPlanes.begin();
107  it1 != mPbMap.vPlanes[it->first].nearbyPlanes.end(); it1++)
108  {
109  if (matcher.subgraphSrc->subgraphPlanesIdx.count(*it1))
110  continue;
111 
112  for (set<unsigned>::iterator it2 =
113  matchedPbMap.vPlanes[it->second].nearbyPlanes.begin();
114  it2 != matchedPbMap.vPlanes[it->second].nearbyPlanes.end();
115  it2++)
116  {
117  if (matcher.subgraphTrg->subgraphPlanesIdx.count(*it2))
118  continue;
119 
120  if (!matcher.evalUnaryConstraints(
121  mPbMap.vPlanes[*it1], matchedPbMap.vPlanes[*it2],
122  matchedPbMap, false)) //(FloorPlane != -1 &&
123  // FloorPlaneMap != -1) ? true
124  //: false ) )
125  continue;
126  for (map<unsigned, unsigned>::iterator it_matched =
127  bestMatch.begin();
128  it_matched != bestMatch.end(); it_matched++)
129  if (!matcher.evalBinaryConstraints(
130  mPbMap.vPlanes[*it1],
131  mPbMap.vPlanes[it_matched->first],
132  matchedPbMap.vPlanes[*it2],
133  matchedPbMap.vPlanes[it_matched->second]))
134  continue;
135 
136  // Asign new neighbor
137  bestMatch[*it1] = *it2;
138  }
139  }
140  else // if(matcher.configLocaliser.graph_mode == 1)// Co-visible
141  // neighbors - Check the 2nd order neighbors of the matched planes
142  for (map<unsigned, unsigned>::iterator it1 =
143  mPbMap.vPlanes[it->first].neighborPlanes.begin();
144  it1 != mPbMap.vPlanes[it->first].neighborPlanes.end(); it1++)
145  {
146  if (matcher.subgraphSrc->subgraphPlanesIdx.count(it1->first))
147  continue;
148 
149  for (map<unsigned, unsigned>::iterator it2 =
150  matchedPbMap.vPlanes[it->second]
151  .neighborPlanes.begin();
152  it2 !=
153  matchedPbMap.vPlanes[it->second].neighborPlanes.end();
154  it2++)
155  {
156  if (matcher.subgraphTrg->subgraphPlanesIdx.count(
157  it2->first))
158  continue;
159 
160  if (!matcher.evalUnaryConstraints(
161  mPbMap.vPlanes[it1->first],
162  matchedPbMap.vPlanes[it2->first], matchedPbMap,
163  false)) //(FloorPlane != -1 && FloorPlaneMap != -1)
164  //? true : false ) )
165  continue;
166  for (map<unsigned, unsigned>::iterator it_matched =
167  bestMatch.begin();
168  it_matched != bestMatch.end(); it_matched++)
169  if (!matcher.evalBinaryConstraints(
170  mPbMap.vPlanes[it1->first],
171  mPbMap.vPlanes[it_matched->first],
172  matchedPbMap.vPlanes[it2->first],
173  matchedPbMap.vPlanes[it_matched->second]))
174  continue;
175 
176  // Asign new neighbor
177  bestMatch[it1->first] = it2->first;
178  }
179  }
180  }
181 }
182 
183 double PbMapLocaliser::getAreaMatch()
184 {
185  double area = 0.0;
186  for (map<unsigned, unsigned>::iterator it = bestMatch.begin();
187  it != bestMatch.end(); it++)
188  area += mPbMap.vPlanes[it->first].areaVoxels;
189 
190 #ifdef _VERBOSE
191  cout << "getAreaMatch " << area << endl;
192 #endif
193 
194  return area;
195 }
196 
197 /**!
198  * Searches the input plane in the rest of planes of the map taking into account
199  * the neighboring relations
200 */
201 bool PbMapLocaliser::searchPlaneContext(Plane& searchPlane)
202 {
203 #ifdef _VERBOSE
204  cout << "\nSearching plane P" << searchPlane.id << " numNeigs "
205  << searchPlane.neighborPlanes.size() << " in map composed of "
206  << totalPrevPlanes << " planes\n";
207 #endif
208 
209  // REVISAR: Esto cambia el codigo anterior (antes la vecindad actual
210  // contenia todos los vecinos de los planos observados
211  Subgraph currentSubgraph(&mPbMap, searchPlane.id);
212  // matcher.setSourceSubgraph(currentSubgraph);
213 
214  for (size_t mapId = 0; mapId < previousPbMaps.size(); mapId++)
215  {
216  PbMap& prevPbMap = previousPbMaps[mapId];
217 
218  for (size_t i = 0; i < prevPbMap.vPlanes.size(); i++)
219  {
220  Plane& targetPlane = prevPbMap.vPlanes[i];
221 
222  // Do not consider evaluating planes which do not have more than
223  // min_planes_recognition neighbor planes (too much uncertainty)
224  if (matcher.configLocaliser.graph_mode == 0)
225  {
226  if (targetPlane.nearbyPlanes.size() <
227  matcher.configLocaliser.min_planes_recognition)
228  {
229  continue;
230  }
231  }
232  else // matcher.configLocaliser.graph_mode = 1
233  {
234  if (targetPlane.neighborPlanes.size() <
235  matcher.configLocaliser.min_planes_recognition)
236  {
237  continue;
238  }
239  }
240 
241  Subgraph targetSubgraph(&prevPbMap, targetPlane.id);
242  // matcher.setTargetSubgraph(targetSubgraph);
243 
244  // cout << "\nsource size " <<
245  // currentSubgraph.subgraphPlanesIdx.size() << endl;
246  // cout << "target size " <<
247  // targetSubgraph.subgraphPlanesIdx.size() << endl;
248  std::map<unsigned, unsigned> resultingMatch =
249  matcher.compareSubgraphs(currentSubgraph, targetSubgraph);
250 
251  // cout << "ResultingMatch size " << resultingMatch.size() <<
252  // endl;
253  if (resultingMatch.size() > bestMatch.size())
254  {
255  bestMap = mapId;
256  bestMatch = resultingMatch;
257  }
258 
259  // Evaluate Score of the matched places
260  // if( score > maxScore)
261  // {
262  // max2ndScore = maxScore;
263  // maxScore = score;
264  // winnerPlane = targetPlane.label;
265  // }
266  // score_results[i] = comparePlanes(searchPlane, targetPlane);
267  }
268  }
269 
270 #ifdef _VERBOSE
271  cout << "bestMatch size " << bestMatch.size() << " min "
272  << matcher.configLocaliser.min_planes_recognition << endl;
273 #endif
274 
275  if (bestMatch.size() >=
276  matcher.configLocaliser
277  .min_planes_recognition) // Assign label of previous map plane
278  {
279 #ifdef _VERBOSE
280  cout << "Context matched\n";
281 #endif
282 
283  // compareSubgraphNeighbors(matcher);
284 
285  PbMap& winnerPbMap = previousPbMaps[bestMap];
286 
287  // We require that the sum of the areas of the matched planes is above a
288  // minimum
289  if (getAreaMatch() < 3.0) return false;
290 
291  pcl::PointXYZ placePos(0, 0, 0);
292  for (map<unsigned, unsigned>::iterator it = bestMatch.begin();
293  it != bestMatch.end(); it++)
294  {
295  if (mPbMap.vPlanes[it->first].label !=
296  winnerPbMap.vPlanes[it->second].label)
297  {
298  placePos.x += mPbMap.vPlanes[it->first].v3center[0];
299  placePos.y += mPbMap.vPlanes[it->first].v3center[1];
300  placePos.z += mPbMap.vPlanes[it->first].v3center[2];
301  if (planeRecognitionLUT.count(
302  winnerPbMap.vPlanes[it->second].label))
303  {
304  cout << "Re-assign plane label\n";
305  if (bestMatch.size() >=
306  planeRecognitionLUT[winnerPbMap.vPlanes[it->second]
307  .label]
308  .second)
309  {
310  mPbMap
311  .vPlanes[planeRecognitionLUT
312  [winnerPbMap.vPlanes[it->second].label]
313  .first]
314  .label = ""; // Delete previous label asignment
315  mPbMap.vPlanes[it->first].label =
316  winnerPbMap.vPlanes[it->second].label;
317  planeRecognitionLUT[winnerPbMap.vPlanes[it->second]
318  .label] =
319  pair<int, double>(
320  mPbMap.vPlanes[it->first].id, bestMatch.size());
321  }
322  else
323  mPbMap.vPlanes[it->first].label = "";
324  }
325  else
326  {
327 #ifdef _VERBOSE
328  cout << "Assign plane label\n";
329 #endif
330 
331  planeRecognitionLUT[winnerPbMap.vPlanes[it->second].label] =
332  pair<int, double>(
333  mPbMap.vPlanes[it->first].id, bestMatch.size());
334  mPbMap.vPlanes[it->first].label =
335  winnerPbMap.vPlanes[it->second].label;
336  }
337  }
338  }
339 
340  placePos.x /= bestMatch.size();
341  placePos.y /= bestMatch.size();
342  placePos.z /= bestMatch.size();
343  foundPlaces[previousPbMapNames[bestMap]] = placePos;
344 
345  // Check previous assignments. TODO. change the external label based
346  // system
347  // if(searchPlane.label != winnerPlane)
348  // {
349  // if(planeRecognitionLUT.count(winnerPlane) )
350  // {
351  // if( bestMatch.size() >=
352  // planeRecognitionLUT[winnerPlane].second )
353  // {
354  // mPbMap.vPlanes[planeRecognitionLUT[winnerPlane].first].label
355  // = ""; // Delete previous label asignment
356  // searchPlane.label = winnerPlane;
357  // planeRecognitionLUT[winnerPlane] =
358  // pair<int,double>(searchPlane.id, bestMatch.size());
359  // }
360  // else
361  // searchPlane.label = "";
362  // }
363  // else
364  // {
365  // planeRecognitionLUT[winnerPlane] =
366  // pair<int,double>(searchPlane.id, bestMatch.size());
367  // searchPlane.label = winnerPlane;
368  // }
369  // }
370 
371  // Superimpose model
372  Eigen::Matrix4f rigidTransf; // Pose of map as from current model
373  Eigen::Matrix4f rigidTransfInv; // Pose of model as from current map
374  ConsistencyTest fitModel(mPbMap, winnerPbMap);
375  rigidTransf = fitModel.getRTwithModel(bestMatch);
376  rigidTransfInv = inverse(rigidTransf);
377 
378  // Read in the cloud data
379  pcl::PCDReader reader;
380  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr model(
381  new pcl::PointCloud<pcl::PointXYZRGBA>);
382  string filename = "/home/edu/Projects/PbMaps/" +
383  previousPbMapNames[bestMap] + "/MapPlanes.pcd";
384  reader.read(filename, *model);
385  alignedModelPtr.reset(new pcl::PointCloud<pcl::PointXYZRGBA>);
386  pcl::transformPointCloud(*model, *alignedModelPtr, rigidTransfInv);
387 
388  return true;
389  }
390 
391  // TODO Has this place been matched before? Check it and if so, decide which
392  // is the best match
393  return false;
394 }
395 
396 void PbMapLocaliser::run()
397 {
398  // cout << "run PbMapLocaliser\n";
399  bool placeFound = false;
400 
401  std::map<unsigned, vector<double>> timeLocalizer;
402  std::map<unsigned, vector<int>> nCheckLocalizer;
403  time1 = 0.0;
404  time2 = 0.0;
405 
406  while (!m_pbMapLocaliser_must_stop && !placeFound)
407  {
408  // cout << "while...\n";
409  if (vQueueObservedPlanes.empty()) // if(sQueueObservedPlanes.empty())
410  std::this_thread::sleep_for(50ms);
411 
412  else
413  {
414  while (!vQueueObservedPlanes.empty())
415  {
416  // Do not consider searching planes which do not have more than
417  // 2 neighbor planes (too much uncertainty)
418  if (matcher.configLocaliser.graph_mode ==
419  0) // Nearby neighbors
420  {
421  // cout << "vecinos1 " <<
422  // mPbMap.vPlanes[vQueueObservedPlanes[0]].nearbyPlanes.size()
423  // << endl;
424  if (mPbMap.vPlanes[vQueueObservedPlanes[0]]
425  .nearbyPlanes.size() <
426  matcher.configLocaliser.min_planes_recognition)
427  {
428  vQueueObservedPlanes.erase(
429  vQueueObservedPlanes.begin());
430  continue;
431  }
432  }
433  else // matcher.configLocaliser.graph_mode = 1
434  {
435  // cout << "vecinos2 " <<
436  // mPbMap.vPlanes[vQueueObservedPlanes[0]].neighborPlanes.size()
437  // << endl;
438  if (mPbMap.vPlanes[vQueueObservedPlanes[0]]
439  .neighborPlanes.size() <
440  matcher.configLocaliser.min_planes_recognition)
441  {
442  vQueueObservedPlanes.erase(
443  vQueueObservedPlanes.begin());
444  continue;
445  }
446  }
447 
448  // cout << "Search context\n";
449  matcher.nCheckConditions = 0;
450  // #ifdef _VERBOSE
451  double search_plane_start = pcl::getTime();
452  // #endif
453 
454  if (searchPlaneContext(mPbMap.vPlanes[vQueueObservedPlanes[0]]))
455  {
456  placeFound = true;
457  // #ifdef _VERBOSE
458  double search_plane_end = pcl::getTime();
459  std::cout << "PLACE FOUND. Search took "
460  << double(search_plane_end - search_plane_start)
461  << " s\n";
462  // #endif
463  break;
464  }
465 
466  vQueueObservedPlanes.erase(vQueueObservedPlanes.begin());
467 
468  // cout << "nChecks " << matcher.nCheckConditions << endl;
469  double search_plane_end = pcl::getTime();
470  if (timeLocalizer.count(
471  mPbMap.vPlanes[vQueueObservedPlanes[0]]
472  .neighborPlanes.size()) == 0)
473  {
474  timeLocalizer[mPbMap.vPlanes[vQueueObservedPlanes[0]]
475  .neighborPlanes.size()]
476  .push_back(
477  double(search_plane_end - search_plane_start));
478  nCheckLocalizer[mPbMap.vPlanes[vQueueObservedPlanes[0]]
479  .neighborPlanes.size()]
480  .push_back(matcher.nCheckConditions);
481  }
482  else
483  {
484  vector<double> firstElement(
485  1, double(search_plane_end - search_plane_start));
486  vector<int> firstElement_(1, matcher.nCheckConditions);
487  timeLocalizer[mPbMap.vPlanes[vQueueObservedPlanes[0]]
488  .neighborPlanes.size()] = firstElement;
489  nCheckLocalizer[mPbMap.vPlanes[vQueueObservedPlanes[0]]
490  .neighborPlanes.size()] = firstElement_;
491  }
492 
493 #ifdef _VERBOSE
494  double search_plane_end = pcl::getTime();
495  std::cout
496  << "Search a plane took "
497  << double(search_plane_end - search_plane_start)
498  << std::endl; //<< " or " << clock.Tac() << std::endl;
499 #endif
500  }
501  }
502  }
503  m_pbMapLocaliser_finished = true;
504 
505  cout << "Print TIME PbLocalizer\n";
506  cout << "Tiempo1 " << time1 << " tiempo2 " << time2 << endl;
507  ;
508  std::map<unsigned, vector<int>>::iterator itChecks =
509  nCheckLocalizer.begin();
510  for (std::map<unsigned, vector<double>>::iterator it =
511  timeLocalizer.begin();
512  it != timeLocalizer.end(); it++)
513  {
514  double sum_times = 0;
515  double sum_nChecks = 0;
516  for (unsigned j = 0; j < it->second.size(); j++)
517  {
518  sum_times += it->second[j];
519  sum_nChecks += itChecks->second[j];
520  }
521  itChecks++;
522 
523  cout << "Size " << it->first << " time "
524  << sum_times / it->second.size() << endl;
525  cout << "... nChecks " << sum_nChecks / it->second.size() << endl;
526  }
527 }
528 
529 bool PbMapLocaliser::stop_pbMapLocaliser()
530 {
531  m_pbMapLocaliser_must_stop = true;
532  while (!m_pbMapLocaliser_finished) std::this_thread::sleep_for(1ms);
533  cout << "Waiting for PbMapLocaliser thread to die.." << endl;
534 
535  pbMapLocaliser_hd.join();
536 
537  return true;
538 }
539 
540 PbMapLocaliser::~PbMapLocaliser() { stop_pbMapLocaliser(); }
541 #endif
mrpt::pbmap::PbMapLocaliser::LoadPreviousPbMaps
void LoadPreviousPbMaps(std::string fileMaps)
Definition: PbMapLocaliser.cpp:51
mrpt::pbmap::PbMapLocaliser::m_pbMapLocaliser_finished
bool m_pbMapLocaliser_finished
Definition: PbMapLocaliser.h:116
mrpt::pbmap::PbMap::vPlanes
std::vector< Plane > vPlanes
Definition: pbmap/PbMap.h:57
time1
double time1
Definition: PbMapLocaliser.cpp:31
mrpt::pbmap::SubgraphMatcher::configLocaliser
config_heuristics configLocaliser
Definition: SubgraphMatcher.h:103
mrpt::pbmap::SubgraphMatcher::subgraphSrc
Subgraph * subgraphSrc
Definition: SubgraphMatcher.h:88
mrpt::pbmap::PbMapLocaliser::searchPlaneContext
bool searchPlaneContext(Plane &searchPlane)
! Searches the input plane in the rest of planes of the map taking into account the neighboring relat...
Definition: PbMapLocaliser.cpp:201
mrpt::pbmap::SubgraphMatcher::evalBinaryConstraints
bool evalBinaryConstraints(Plane &plane1, Plane &plane2, Plane &planeA, Plane &planeB)
! Compares the relation between Ref-neigRef with the relation between Check-neigCheck.
Definition: SubgraphMatcher.cpp:778
mrpt::pbmap::SubgraphMatcher::evalUnaryConstraints
bool evalUnaryConstraints(Plane &plane1, Plane &plane2, PbMap &trgPbMap, bool useStructure=false)
! Check if the two input planes could be the same
Definition: SubgraphMatcher.cpp:48
mrpt::pbmap::config_heuristics::intensity_threshold
float intensity_threshold
Definition: heuristicParams.h:55
mrpt::pbmap::config_heuristics::hue_threshold
float hue_threshold
Definition: heuristicParams.h:56
mrpt::pbmap::Subgraph
Definition: Subgraph.h:34
CConfigFile.h
mrpt::pbmap::PbMapLocaliser::totalPrevPlanes
size_t totalPrevPlanes
Definition: PbMapLocaliser.h:59
mrpt::pbmap::PbMapLocaliser::mPbMap
PbMap & mPbMap
Definition: PbMapLocaliser.h:74
mrpt::pbmap::SubgraphMatcher::nCheckConditions
int nCheckConditions
Definition: SubgraphMatcher.h:93
mrpt::pbmap::SubgraphMatcher::compareSubgraphs
std::map< unsigned, unsigned > compareSubgraphs(Subgraph &subgraphSource, Subgraph &subgraphTarget, const int option=0)
Definition: SubgraphMatcher.cpp:1403
mrpt::pbmap::PbMapLocaliser::previousPbMaps
std::vector< PbMap > previousPbMaps
Definition: PbMapLocaliser.h:51
pbmap-precomp.h
mrpt::pbmap::PbMapLocaliser::planeRecognitionLUT
std::map< std::string, std::pair< int, double > > planeRecognitionLUT
Definition: PbMapLocaliser.h:83
mrpt::pbmap::PbMapLocaliser::m_pbMapLocaliser_must_stop
bool m_pbMapLocaliser_must_stop
Definition: PbMapLocaliser.h:113
mrpt::pbmap::PbMapLocaliser::bestMatch
std::map< unsigned, unsigned > bestMatch
Definition: PbMapLocaliser.h:97
mrpt::pbmap::Subgraph::subgraphPlanesIdx
std::set< unsigned > subgraphPlanesIdx
Definition: Subgraph.h:67
heuristicParams.h
mrpt::pbmap::PbMapLocaliser::getAreaMatch
double getAreaMatch()
Definition: PbMapLocaliser.cpp:183
mrpt::pbmap::config_heuristics::load_params
void load_params(const std::string &config_file_name)
Definition: heuristicParams.h:83
mrpt::pbmap::ConsistencyTest::getRTwithModel
Eigen::Matrix4f getRTwithModel(std::map< unsigned, unsigned > &matched_planes)
Definition: ConsistencyTest.cpp:483
mrpt::pbmap::PbMapLocaliser::previousPbMapNames
std::vector< std::string > previousPbMapNames
Definition: PbMapLocaliser.h:48
mrpt::pbmap::SubgraphMatcher
Definition: SubgraphMatcher.h:37
mrpt::pbmap::config_heuristics::min_planes_recognition
unsigned min_planes_recognition
Definition: heuristicParams.h:41
mrpt::pbmap::Plane
A class used to store a planar feature (Plane for short).
Definition: Plane.h:46
mrpt::pbmap::PbMap::loadPbMap
void loadPbMap(std::string PbMapFile)
Definition: PbMap.cpp:96
mrpt::pbmap::config_heuristics::color_threshold
float color_threshold
Definition: heuristicParams.h:54
mrpt::pbmap::PbMapLocaliser::foundPlaces
std::map< std::string, pcl::PointXYZ > foundPlaces
Definition: PbMapLocaliser.h:65
mrpt::pbmap::PbMap
A class used to store a Plane-based Map (PbMap).
Definition: pbmap/PbMap.h:47
mrpt::pbmap::inverse
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:82
mrpt::pbmap::PbMapLocaliser::run
void run()
Definition: PbMapLocaliser.cpp:396
mrpt::pbmap::PbMapLocaliser::matcher
SubgraphMatcher matcher
Definition: PbMapLocaliser.h:77
SubgraphMatcher.h
time2
double time2
Definition: PbMapLocaliser.cpp:31
mrpt::pbmap
Definition: ConsistencyTest.h:27
mrpt::pbmap::SubgraphMatcher::subgraphTrg
Subgraph * subgraphTrg
Definition: SubgraphMatcher.h:91
mrpt::pbmap::PbMapLocaliser::pbMapLocaliser_hd
std::thread pbMapLocaliser_hd
Definition: PbMapLocaliser.h:107
mrpt::pbmap::config_heuristics::graph_mode
int graph_mode
Definition: heuristicParams.h:64
PbMapLocaliser.h
mrpt::pbmap::Plane::neighborPlanes
std::map< unsigned, unsigned > neighborPlanes
Definition: Plane.h:131
mrpt::pbmap::Plane::id
unsigned id
! Parameters to allow the plane-based representation of the map by a graph
Definition: Plane.h:127
string
GLsizei const GLchar ** string
Definition: glext.h:4101
iterator
Scalar * iterator
Definition: eigen_plugins.h:26
mrpt::pbmap::PbMapLocaliser::~PbMapLocaliser
~PbMapLocaliser()
Definition: PbMapLocaliser.cpp:540
mrpt::pbmap::ConsistencyTest
Definition: ConsistencyTest.h:35
mrpt::pbmap::PbMapLocaliser::bestMap
unsigned bestMap
Definition: PbMapLocaliser.h:94
mrpt::pbmap::PbMapLocaliser::stop_pbMapLocaliser
bool stop_pbMapLocaliser()
Definition: PbMapLocaliser.cpp:529
mrpt::pbmap::Plane::nearbyPlanes
std::set< unsigned > nearbyPlanes
Definition: Plane.h:130
mrpt::pbmap::PbMapLocaliser::compareSubgraphNeighbors
void compareSubgraphNeighbors(SubgraphMatcher &matcher)
Definition: PbMapLocaliser.cpp:92
mrpt::pbmap::PbMapLocaliser::vQueueObservedPlanes
std::vector< unsigned > vQueueObservedPlanes
Definition: PbMapLocaliser.h:62
model
_u8 model
Definition: rplidar_cmd.h:2
mrpt::pbmap::PbMapLocaliser::alignedModelPtr
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr alignedModelPtr
Definition: PbMapLocaliser.h:68


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST