Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 href="group__mrpt__pbmap__grp.html" >mrpt-pbmap</a>
13  */
14 #include "pbmap-precomp.h" // Precompiled headers
15 
16 
17 #if MRPT_HAS_PCL
18 
22 #include <mrpt/utils/CConfigFile.h>
23 #include <mrpt/system/threads.h>
24 #include <pcl/io/pcd_io.h>
25 #include <pcl/common/time.h>
26 #include <fstream>
27 #include <iostream>
28 
29 using namespace std;
30 using namespace mrpt::pbmap;
31 
32 double time1, time2;
33 
34 PbMapLocaliser::PbMapLocaliser(PbMap &mPbM, const string &config_file) :
35  mPbMap(mPbM),
36  m_pbMapLocaliser_must_stop(false),
37  m_pbMapLocaliser_finished(false)
38 {
39  matcher.configLocaliser.load_params(config_file);
40 std::cout << "PbMapLocaliser::PbMapLocaliser min_planes_recognition " << matcher.configLocaliser.min_planes_recognition << " color thresholds " << matcher.configLocaliser.color_threshold << " " << matcher.configLocaliser.intensity_threshold << " " << matcher.configLocaliser.hue_threshold << endl;
41 
42  LoadPreviousPbMaps("/home/edu/newPbMaps/PbMaps.txt");
43 
45 }
46 
47 
48 // Load Previous PbMaps
50 {
51 cout << "PbMapLocaliser::LoadPreviousPbMaps(...)\n";
52  std::ifstream configFile;
53  configFile.open(fileMaps.c_str());
54 
55 
56  string mapFile, filepath = "/home/edu/newPbMaps/";
57  totalPrevPlanes = 0;
58  while( !configFile.eof() )
59  {
60 // floorPlane = -1;
61  getline(configFile, mapFile);
62  if(mapFile == "")
63  break;
64 
65  if(mapFile.at(0) == '%')
66  continue;
67  cout << "Load map: " << filepath << mapFile << endl;
68 
69  PbMap previousPbMap;
70  previousPbMap.loadPbMap(filepath + mapFile);
71 
72  previousPbMapNames.push_back(mapFile);
73  previousPbMaps.push_back(previousPbMap);
74 
75 // prevPbMap.vPlaness.push_back(vPlanesTemp);
76 // previousPbMapNames.push_back(mapFile);
77 // vFloors.push_back(floorPlane);
78  totalPrevPlanes += previousPbMap.vPlanes.size();
79 // cout << "PbMap loaded has " << previousPbMap.vPlanes.size() << " 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
93 {
94 cout << "PbMapLocaliser::compareSubgraphNeighbors\n";
95  PbMap &matchedPbMap = previousPbMaps[bestMap];
96 
97  for(map<unsigned, unsigned>::iterator it = bestMatch.begin(); it != bestMatch.end(); it++)
98  {
99  // TODO: use the graph to grow
100  if(matcher.configLocaliser.graph_mode == 0) // Nearby neighbors - Check the 2nd order neighbors of the matched planes
101  for(set<unsigned>::iterator it1 = mPbMap.vPlanes[it->first].nearbyPlanes.begin(); it1 != mPbMap.vPlanes[it->first].nearbyPlanes.end(); it1++)
102  {
103  if(matcher.subgraphSrc->subgraphPlanesIdx.count(*it1) )
104  continue;
105 
106  for(set<unsigned>::iterator it2 = matchedPbMap.vPlanes[it->second].nearbyPlanes.begin(); it2 != matchedPbMap.vPlanes[it->second].nearbyPlanes.end(); it2++)
107  {
108  if(matcher.subgraphTrg->subgraphPlanesIdx.count(*it2) )
109  continue;
110 
111  if( !matcher.evalUnaryConstraints(mPbMap.vPlanes[*it1], matchedPbMap.vPlanes[*it2], matchedPbMap, false ) )//(FloorPlane != -1 && FloorPlaneMap != -1) ? true : false ) )
112  continue;
113  for(map<unsigned, unsigned>::iterator it_matched = bestMatch.begin(); it_matched != bestMatch.end(); it_matched++)
114  if( !matcher.evalBinaryConstraints(mPbMap.vPlanes[*it1], mPbMap.vPlanes[it_matched->first], matchedPbMap.vPlanes[*it2], matchedPbMap.vPlanes[it_matched->second]) )
115  continue;
116 
117  // Asign new neighbor
118  bestMatch[*it1] = *it2;
119  }
120  }
121  else //if(matcher.configLocaliser.graph_mode == 1)// Co-visible neighbors - Check the 2nd order neighbors of the matched planes
122  for(map<unsigned, unsigned>::iterator it1 = mPbMap.vPlanes[it->first].neighborPlanes.begin(); it1 != mPbMap.vPlanes[it->first].neighborPlanes.end(); it1++)
123  {
124  if(matcher.subgraphSrc->subgraphPlanesIdx.count(it1->first) )
125  continue;
126 
127  for(map<unsigned, unsigned>::iterator it2 = matchedPbMap.vPlanes[it->second].neighborPlanes.begin(); it2 != matchedPbMap.vPlanes[it->second].neighborPlanes.end(); it2++)
128  {
129  if(matcher.subgraphTrg->subgraphPlanesIdx.count(it2->first) )
130  continue;
131 
132  if( !matcher.evalUnaryConstraints(mPbMap.vPlanes[it1->first], matchedPbMap.vPlanes[it2->first], matchedPbMap, false ) )//(FloorPlane != -1 && FloorPlaneMap != -1) ? true : false ) )
133  continue;
134  for(map<unsigned, unsigned>::iterator it_matched = bestMatch.begin(); it_matched != bestMatch.end(); it_matched++)
135  if( !matcher.evalBinaryConstraints(mPbMap.vPlanes[it1->first], mPbMap.vPlanes[it_matched->first], matchedPbMap.vPlanes[it2->first], matchedPbMap.vPlanes[it_matched->second]) )
136  continue;
137 
138  // Asign new neighbor
139  bestMatch[it1->first] = it2->first;
140  }
141  }
142 
143  }
144 }
145 
147 {
148  double area = 0.0;
149  for(map<unsigned, unsigned>::iterator it = bestMatch.begin(); it != bestMatch.end(); it++)
150  area += mPbMap.vPlanes[it->first].areaVoxels;
151 
152  #ifdef _VERBOSE
153  cout << "getAreaMatch " << area << endl;
154  #endif
155 
156  return area;
157 }
158 
159 
160 /**!
161  * Searches the input plane in the rest of planes of the map taking into account the neighboring relations
162 */
164 {
165  #ifdef _VERBOSE
166  cout << "\nSearching plane P" << searchPlane.id << " numNeigs " << searchPlane.neighborPlanes.size()
167  << " in map composed of " << totalPrevPlanes << " planes\n";
168  #endif
169 
170  // REVISAR: Esto cambia el codigo anterior (antes la vecindad actual contenia todos los vecinos de los planos observados
171  Subgraph currentSubgraph(&mPbMap, searchPlane.id);
172 // matcher.setSourceSubgraph(currentSubgraph);
173 
174  for(size_t mapId=0; mapId < previousPbMaps.size(); mapId++)
175  {
176  PbMap &prevPbMap = previousPbMaps[mapId];
177 
178  for(size_t i=0; i < prevPbMap.vPlanes.size(); i++)
179  {
180  Plane &targetPlane = prevPbMap.vPlanes[i];
181 
182  // Do not consider evaluating planes which do not have more than min_planes_recognition neighbor planes (too much uncertainty)
184  {
185  if( targetPlane.nearbyPlanes.size() < matcher.configLocaliser.min_planes_recognition ){
186  continue;}
187  }
188  else // matcher.configLocaliser.graph_mode = 1
189  {
191  continue;}
192  }
193 
194 
195  Subgraph targetSubgraph(&prevPbMap, targetPlane.id);
196 // matcher.setTargetSubgraph(targetSubgraph);
197 
198 // cout << "\nsource size " << currentSubgraph.subgraphPlanesIdx.size() << endl;
199 // cout << "target size " << targetSubgraph.subgraphPlanesIdx.size() << endl;
200  std::map<unsigned, unsigned> resultingMatch = matcher.compareSubgraphs(currentSubgraph, targetSubgraph);
201 
202 // cout << "ResultingMatch size " << resultingMatch.size() << endl;
203  if( resultingMatch.size() > bestMatch.size())
204  {
205  bestMap = mapId;
206  bestMatch = resultingMatch;
207  }
208 
209  // Evaluate Score of the matched places
210 // if( score > maxScore)
211 // {
212 // max2ndScore = maxScore;
213 // maxScore = score;
214 // winnerPlane = targetPlane.label;
215 // }
216  // score_results[i] = comparePlanes(searchPlane, targetPlane);
217  }
218  }
219 
220  #ifdef _VERBOSE
221  cout << "bestMatch size " << bestMatch.size() << " min " << matcher.configLocaliser.min_planes_recognition << endl;
222  #endif
223 
224  if( bestMatch.size() >= matcher.configLocaliser.min_planes_recognition ) // Assign label of previous map plane
225  {
226  #ifdef _VERBOSE
227  cout << "Context matched\n";
228  #endif
229 
230 // compareSubgraphNeighbors(matcher);
231 
232  PbMap &winnerPbMap = previousPbMaps[bestMap];
233 
234  // We require that the sum of the areas of the matched planes is above a minimum
235  if( getAreaMatch() < 3.0 )
236  return false;
237 
238 
239  pcl::PointXYZ placePos(0,0,0);
240  for(map<unsigned, unsigned>::iterator it = bestMatch.begin(); it != bestMatch.end(); it++)
241  {
242  if(mPbMap.vPlanes[it->first].label != winnerPbMap.vPlanes[it->second].label)
243  {
244  placePos.x += mPbMap.vPlanes[it->first].v3center[0];
245  placePos.y += mPbMap.vPlanes[it->first].v3center[1];
246  placePos.z += mPbMap.vPlanes[it->first].v3center[2];
247  if(planeRecognitionLUT.count(winnerPbMap.vPlanes[it->second].label) )
248  {
249  cout << "Re-assign plane label\n";
250  if( bestMatch.size() >= planeRecognitionLUT[winnerPbMap.vPlanes[it->second].label].second )
251  {
252  mPbMap.vPlanes[planeRecognitionLUT[winnerPbMap.vPlanes[it->second].label].first].label = ""; // Delete previous label asignment
253  mPbMap.vPlanes[it->first].label = winnerPbMap.vPlanes[it->second].label;
254  planeRecognitionLUT[winnerPbMap.vPlanes[it->second].label] = pair<int,double>(mPbMap.vPlanes[it->first].id, bestMatch.size());
255  }
256  else
257  mPbMap.vPlanes[it->first].label = "";
258  }
259  else
260  {
261  #ifdef _VERBOSE
262  cout << "Assign plane label\n";
263  #endif
264 
265  planeRecognitionLUT[winnerPbMap.vPlanes[it->second].label] = pair<int,double>(mPbMap.vPlanes[it->first].id, bestMatch.size());
266  mPbMap.vPlanes[it->first].label = winnerPbMap.vPlanes[it->second].label;
267  }
268  }
269  }
270 
271  placePos.x /= bestMatch.size();
272  placePos.y /= bestMatch.size();
273  placePos.z /= bestMatch.size();
275 
276  // Check previous assignments. TODO. change the external label based system
277 // if(searchPlane.label != winnerPlane)
278 // {
279 // if(planeRecognitionLUT.count(winnerPlane) )
280 // {
281 // if( bestMatch.size() >= planeRecognitionLUT[winnerPlane].second )
282 // {
283 // mPbMap.vPlanes[planeRecognitionLUT[winnerPlane].first].label = ""; // Delete previous label asignment
284 // searchPlane.label = winnerPlane;
285 // planeRecognitionLUT[winnerPlane] = pair<int,double>(searchPlane.id, bestMatch.size());
286 // }
287 // else
288 // searchPlane.label = "";
289 // }
290 // else
291 // {
292 // planeRecognitionLUT[winnerPlane] = pair<int,double>(searchPlane.id, bestMatch.size());
293 // searchPlane.label = winnerPlane;
294 // }
295 // }
296 
297  // Superimpose model
298  Eigen::Matrix4f rigidTransf; // Pose of map as from current model
299  Eigen::Matrix4f rigidTransfInv; // Pose of model as from current map
300  ConsistencyTest fitModel(mPbMap, winnerPbMap);
301  rigidTransf = fitModel.getRTwithModel(bestMatch);
302  rigidTransfInv = inverse(rigidTransf);
303 
304  // Read in the cloud data
305  pcl::PCDReader reader;
306  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr model (new pcl::PointCloud<pcl::PointXYZRGBA>);
307  string filename = "/home/edu/Projects/PbMaps/" + previousPbMapNames[bestMap] + "/MapPlanes.pcd";
308  reader.read (filename, *model);
309  alignedModelPtr.reset(new pcl::PointCloud<pcl::PointXYZRGBA>);
310  pcl::transformPointCloud(*model,*alignedModelPtr,rigidTransfInv);
311 
312  return true;
313  }
314 
315  // TODO Has this place been matched before? Check it and if so, decide which is the best match
316  return false;
317 }
318 
320 {
321 //cout << "run PbMapLocaliser\n";
322  bool placeFound = false;
323 
324  std::map<unsigned,vector<double> > timeLocalizer;
325  std::map<unsigned,vector<int> > nCheckLocalizer;
326  time1 = 0.0;
327  time2 = 0.0;
328 
329  while(!m_pbMapLocaliser_must_stop && !placeFound)
330  {
331 // cout << "while...\n";
332  if(vQueueObservedPlanes.empty()) //if(sQueueObservedPlanes.empty())
334 
335  else
336  {
337 
338  while( !vQueueObservedPlanes.empty() )
339  {
340  // Do not consider searching planes which do not have more than 2 neighbor planes (too much uncertainty)
341  if(matcher.configLocaliser.graph_mode == 0) // Nearby neighbors
342  {
343 // cout << "vecinos1 " << mPbMap.vPlanes[vQueueObservedPlanes[0]].nearbyPlanes.size() << endl;
345  {
347  continue;
348  }
349  }
350  else //matcher.configLocaliser.graph_mode = 1
351  {
352 // cout << "vecinos2 " << mPbMap.vPlanes[vQueueObservedPlanes[0]].neighborPlanes.size() << endl;
354  {
356  continue;
357  }
358  }
359 
360 // cout << "Search context\n";
362 // #ifdef _VERBOSE
363  double search_plane_start = pcl::getTime ();
364 // #endif
365 
367  {
368  placeFound = true;
369 // #ifdef _VERBOSE
370  double search_plane_end = pcl::getTime ();
371  std::cout << "PLACE FOUND. Search took " << double (search_plane_end - search_plane_start) << " s\n";
372 // #endif
373  break;
374  }
375 
377 
378 // cout << "nChecks " << matcher.nCheckConditions << endl;
379  double search_plane_end = pcl::getTime ();
380  if(timeLocalizer.count(mPbMap.vPlanes[vQueueObservedPlanes[0]].neighborPlanes.size()) == 0)
381  {
382  timeLocalizer[mPbMap.vPlanes[vQueueObservedPlanes[0]].neighborPlanes.size()].push_back( double(search_plane_end - search_plane_start) );
383  nCheckLocalizer[mPbMap.vPlanes[vQueueObservedPlanes[0]].neighborPlanes.size()].push_back(matcher.nCheckConditions);
384  }
385  else
386  {
387  vector<double> firstElement(1,double(search_plane_end - search_plane_start));
388  vector<int> firstElement_(1,matcher.nCheckConditions);
389  timeLocalizer[mPbMap.vPlanes[vQueueObservedPlanes[0]].neighborPlanes.size()] = firstElement;
390  nCheckLocalizer[mPbMap.vPlanes[vQueueObservedPlanes[0]].neighborPlanes.size()] = firstElement_;
391  }
392 
393  #ifdef _VERBOSE
394  double search_plane_end = pcl::getTime ();
395  std::cout << "Search a plane took " << double (search_plane_end - search_plane_start) << std::endl; //<< " or " << clock.Tac() << std::endl;
396  #endif
397  }
398  }
399  }
401 
402 cout << "Print TIME PbLocalizer\n";
403  cout << "Tiempo1 " << time1 << " tiempo2 " << time2 << endl;;
404  std::map<unsigned,vector<int> >::iterator itChecks = nCheckLocalizer.begin();
405  for(std::map<unsigned,vector<double> >::iterator it=timeLocalizer.begin(); it != timeLocalizer.end(); it++)
406  {
407  double sum_times = 0;
408  double sum_nChecks = 0;
409  for(unsigned j=0; j < it->second.size(); j++)
410  {
411  sum_times += it->second[j];
412  sum_nChecks += itChecks->second[j];
413  }
414  itChecks++;
415 
416  cout << "Size " << it->first << " time " << sum_times/it->second.size() << endl;
417  cout << "... nChecks " << sum_nChecks/it->second.size() << endl;
418  }
419 
420 }
421 
423 {
427  cout << "Waiting for PbMapLocaliser thread to die.." << endl;
428 
431 
432  return true;
433 }
434 
436 {
438 }
439 
440 #endif
void loadPbMap(std::string PbMapFile)
Definition: PbMap.cpp:117
double time1
void load_params(const std::string &config_file_name)
std::vector< std::string > previousPbMapNames
std::map< std::string, pcl::PointXYZ > foundPlaces
A class used to store a planar feature (Plane for short).
Definition: Plane.h:48
std::map< std::string, std::pair< int, double > > planeRecognitionLUT
Scalar * iterator
Definition: eigen_plugins.h:23
STL namespace.
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr alignedModelPtr
_u8 model
Definition: rplidar_cmd.h:21
void BASE_IMPEXP sleep(int time_ms) MRPT_NO_THROWS
An OS-independent method for sending the current thread to "sleep" for a given period of time...
Definition: threads.cpp:57
std::set< unsigned > nearbyPlanes
Definition: Plane.h:120
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:74
void clear()
Mark the handle as invalid.
Definition: threads.h:38
GLsizei const GLchar ** string
Definition: glext.h:3919
std::map< unsigned, unsigned > neighborPlanes
Definition: Plane.h:121
std::vector< unsigned > vQueueObservedPlanes
void BASE_IMPEXP joinThread(TThreadHandle &threadHandle)
Waits until the given thread ends.
Definition: threads.cpp:74
std::map< unsigned, unsigned > bestMatch
TThreadHandle createThreadFromObjectMethod(CLASS *obj, void(CLASS::*func)(PARAM), PARAM param)
Creates a new thread running a non-static method (so it will have access to "this") from another meth...
Definition: threads.h:121
double time2
bool searchPlaneContext(Plane &searchPlane)
! Searches the input plane in the rest of planes of the map taking into account the neighboring relat...
void compareSubgraphNeighbors(SubgraphMatcher &matcher)
config_heuristics configLocaliser
std::map< unsigned, unsigned > compareSubgraphs(Subgraph &subgraphSource, Subgraph &subgraphTarget, const int option=0)
unsigned id
! Parameters to allow the plane-based representation of the map by a graph
Definition: Plane.h:117
mrpt::system::TThreadHandle pbMapLocaliser_hd
bool evalUnaryConstraints(Plane &plane1, Plane &plane2, PbMap &trgPbMap, bool useStructure=false)
! Check if the two input planes could be the same
bool evalBinaryConstraints(Plane &plane1, Plane &plane2, Plane &planeA, Plane &planeB)
! Compares the relation between Ref-neigRef with the relation between Check-neigCheck.
void LoadPreviousPbMaps(std::string fileMaps)
std::set< unsigned > subgraphPlanesIdx
Definition: Subgraph.h:62
std::vector< Plane > vPlanes
Definition: pbmap/PbMap.h:55
std::vector< PbMap > previousPbMaps
A class used to store a Plane-based Map (PbMap).
Definition: pbmap/PbMap.h:45
Eigen::Matrix4f getRTwithModel(std::map< unsigned, unsigned > &matched_planes)



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019