util3d_correspondences.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 
28 #include "rtabmap/core/util3d_correspondences.h"
29 #include "rtabmap/core/util3d.h"
30 
31 #include <rtabmap/utilite/UStl.h>
32 #include <rtabmap/core/EpipolarGeometry.h>
33 #include <opencv2/calib3d/calib3d.hpp>
34 #include <pcl/search/kdtree.h>
35 #include <pcl/common/point_tests.h>
36 
37 namespace rtabmap
38 {
39 
40 namespace util3d
41 {
42 
43 
44 void extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,
45  const std::multimap<int, pcl::PointXYZ> & words2,
46  pcl::PointCloud<pcl::PointXYZ> & cloud1,
47  pcl::PointCloud<pcl::PointXYZ> & cloud2)
48 {
49  const std::list<int> & ids = uUniqueKeys(words1);
50  for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
51  {
52  if(words1.count(*i) == 1 && words2.count(*i) == 1)
53  {
54  const pcl::PointXYZ & pt1 = words1.find(*i)->second;
55  const pcl::PointXYZ & pt2 = words2.find(*i)->second;
56  if(pcl::isFinite(pt1) && pcl::isFinite(pt2))
57  {
58  cloud1.push_back(pt1);
59  cloud2.push_back(pt2);
60  }
61  }
62  }
63 }
64 
65 void extractXYZCorrespondencesRANSAC(const std::multimap<int, pcl::PointXYZ> & words1,
66  const std::multimap<int, pcl::PointXYZ> & words2,
67  pcl::PointCloud<pcl::PointXYZ> & cloud1,
68  pcl::PointCloud<pcl::PointXYZ> & cloud2)
69 {
70  std::list<std::pair<pcl::PointXYZ, pcl::PointXYZ> > pairs;
71  const std::list<int> & ids = uUniqueKeys(words1);
72  for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
73  {
74  if(words1.count(*i) == 1 && words2.count(*i) == 1)
75  {
76  const pcl::PointXYZ & pt1 = words1.find(*i)->second;
77  const pcl::PointXYZ & pt2 = words2.find(*i)->second;
78  if(pcl::isFinite(pt1) && pcl::isFinite(pt2))
79  {
80  pairs.push_back(std::pair<pcl::PointXYZ, pcl::PointXYZ>(pt1, pt2));
81  }
82  }
83  }
84 
85  if(pairs.size() > 7)
86  {
87  // Remove outliers using fundamental matrix RANSAC
88  std::vector<uchar> status(pairs.size(), 0);
89  //Convert Keypoints to a structure that OpenCV understands
90  //3 dimensions (Homogeneous vectors)
91  cv::Mat points1(1, (int)pairs.size(), CV_32FC2);
92  cv::Mat points2(1, (int)pairs.size(), CV_32FC2);
93 
94  float * points1data = points1.ptr<float>(0);
95  float * points2data = points2.ptr<float>(0);
96 
97  // Fill the points here ...
98  int i=0;
99  for(std::list<std::pair<pcl::PointXYZ, pcl::PointXYZ> >::const_iterator iter = pairs.begin();
100  iter != pairs.end();
101  ++iter )
102  {
103  points1data[i*2] = (*iter).first.x;
104  points1data[i*2+1] = (*iter).first.y;
105 
106  points2data[i*2] = (*iter).second.x;
107  points2data[i*2+1] = (*iter).second.y;
108 
109  ++i;
110  }
111 
112  // Find the fundamental matrix
113  cv::Mat fundamentalMatrix = cv::findFundamentalMat(
114  points1,
115  points2,
116  status,
117  cv::FM_RANSAC,
118  3.0,
119  0.99);
120 
121  if(!fundamentalMatrix.empty())
122  {
123  int i = 0;
124  for(std::list<std::pair<pcl::PointXYZ, pcl::PointXYZ> >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
125  {
126  if(status[i])
127  {
128  cloud1.push_back(iter->first);
129  cloud2.push_back(iter->second);
130  }
131  ++i;
132  }
133  }
134  }
135 }
136 
137 void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
138  const cv::Mat & depthImage1,
139  const cv::Mat & depthImage2,
140  float cx, float cy,
141  float fx, float fy,
142  float maxDepth,
143  pcl::PointCloud<pcl::PointXYZ> & cloud1,
144  pcl::PointCloud<pcl::PointXYZ> & cloud2)
145 {
146  cloud1.resize(correspondences.size());
147  cloud2.resize(correspondences.size());
148  int oi=0;
149  for(std::list<std::pair<cv::Point2f, cv::Point2f> >::const_iterator iter = correspondences.begin();
150  iter!=correspondences.end();
151  ++iter)
152  {
153  pcl::PointXYZ pt1 = projectDepthTo3D(depthImage1, iter->first.x, iter->first.y, cx, cy, fx, fy, true);
154  pcl::PointXYZ pt2 = projectDepthTo3D(depthImage2, iter->second.x, iter->second.y, cx, cy, fx, fy, true);
155  if(pcl::isFinite(pt1) && pcl::isFinite(pt2) &&
156  (maxDepth <= 0 || (pt1.z <= maxDepth && pt2.z<=maxDepth)))
157  {
158  cloud1[oi] = pt1;
159  cloud2[oi] = pt2;
160  ++oi;
161  }
162  }
163  cloud1.resize(oi);
164  cloud2.resize(oi);
165 }
166 
167 template<typename PointT>
168 inline void extractXYZCorrespondencesImpl(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
169  const pcl::PointCloud<PointT> & cloud1,
170  const pcl::PointCloud<PointT> & cloud2,
171  pcl::PointCloud<pcl::PointXYZ> & inliers1,
172  pcl::PointCloud<pcl::PointXYZ> & inliers2,
173  char depthAxis)
174 {
175  for(std::list<std::pair<cv::Point2f, cv::Point2f> >::const_iterator iter = correspondences.begin();
176  iter!=correspondences.end();
177  ++iter)
178  {
179  PointT pt1 = cloud1.at(int(iter->first.y+0.5f) * cloud1.width + int(iter->first.x+0.5f));
180  PointT pt2 = cloud2.at(int(iter->second.y+0.5f) * cloud2.width + int(iter->second.x+0.5f));
181  if(pcl::isFinite(pt1) &&
182  pcl::isFinite(pt2))
183  {
184  inliers1.push_back(pcl::PointXYZ(pt1.x, pt1.y, pt1.z));
185  inliers2.push_back(pcl::PointXYZ(pt2.x, pt2.y, pt2.z));
186  }
187  }
188 }
189 
190 void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
191  const pcl::PointCloud<pcl::PointXYZ> & cloud1,
192  const pcl::PointCloud<pcl::PointXYZ> & cloud2,
193  pcl::PointCloud<pcl::PointXYZ> & inliers1,
194  pcl::PointCloud<pcl::PointXYZ> & inliers2,
195  char depthAxis)
196 {
197  extractXYZCorrespondencesImpl(correspondences, cloud1, cloud2, inliers1, inliers2, depthAxis);
198 }
199 void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
200  const pcl::PointCloud<pcl::PointXYZRGB> & cloud1,
201  const pcl::PointCloud<pcl::PointXYZRGB> & cloud2,
202  pcl::PointCloud<pcl::PointXYZ> & inliers1,
203  pcl::PointCloud<pcl::PointXYZ> & inliers2,
204  char depthAxis)
205 {
206  extractXYZCorrespondencesImpl(correspondences, cloud1, cloud2, inliers1, inliers2, depthAxis);
207 }
208 
209 int countUniquePairs(const std::multimap<int, pcl::PointXYZ> & wordsA,
210  const std::multimap<int, pcl::PointXYZ> & wordsB)
211 {
212  const std::list<int> & ids = uUniqueKeys(wordsA);
213  int pairs = 0;
214  for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
215  {
216  std::list<pcl::PointXYZ> ptsA = uValues(wordsA, *i);
217  std::list<pcl::PointXYZ> ptsB = uValues(wordsB, *i);
218  if(ptsA.size() == 1 && ptsB.size() == 1)
219  {
220  ++pairs;
221  }
222  }
223  return pairs;
224 }
225 
226 void filterMaxDepth(pcl::PointCloud<pcl::PointXYZ> & inliers1,
227  pcl::PointCloud<pcl::PointXYZ> & inliers2,
228  float maxDepth,
229  char depthAxis,
230  bool removeDuplicates)
231 {
232  std::list<pcl::PointXYZ> addedPts;
233  if(maxDepth > 0.0f && inliers1.size() && inliers1.size() == inliers2.size())
234  {
235  pcl::PointCloud<pcl::PointXYZ> tmpInliers1;
236  pcl::PointCloud<pcl::PointXYZ> tmpInliers2;
237  for(unsigned int i=0; i<inliers1.size(); ++i)
238  {
239  if((depthAxis == 'x' && inliers1.at(i).x < maxDepth && inliers2.at(i).x < maxDepth) ||
240  (depthAxis == 'y' && inliers1.at(i).y < maxDepth && inliers2.at(i).y < maxDepth) ||
241  (depthAxis == 'z' && inliers1.at(i).z < maxDepth && inliers2.at(i).z < maxDepth))
242  {
243  bool dup = false;
244  if(removeDuplicates)
245  {
246  for(std::list<pcl::PointXYZ>::iterator iter = addedPts.begin(); iter!=addedPts.end(); ++iter)
247  {
248  if(iter->x == inliers1.at(i).x &&
249  iter->y == inliers1.at(i).y &&
250  iter->z == inliers1.at(i).z)
251  {
252  dup = true;
253  }
254  }
255  if(!dup)
256  {
257  addedPts.push_back(inliers1.at(i));
258  }
259  }
260 
261  if(!dup)
262  {
263  tmpInliers1.push_back(inliers1.at(i));
264  tmpInliers2.push_back(inliers2.at(i));
265  }
266  }
267  }
268  inliers1 = tmpInliers1;
269  inliers2 = tmpInliers2;
270  }
271 }
272 
273 
274 // a kdtree is constructed with cloud_target, then nearest neighbor
275 // is computed for each cloud_source points.
276 int getCorrespondencesCount(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
277  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
278  float maxDistance)
279 {
280  pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
281  kdTree->setInputCloud(cloud_target);
282  int count = 0;
283  float sqrdMaxDistance = maxDistance * maxDistance;
284  for(unsigned int i=0; i<cloud_source->size(); ++i)
285  {
286  std::vector<int> ind(1);
287  std::vector<float> dist(1);
288  if(kdTree->nearestKSearch(cloud_source->at(i), 1, ind, dist) && dist[0] < sqrdMaxDistance)
289  {
290  ++count;
291  }
292  }
293  return count;
294 }
295 
300 void findCorrespondences(
301  const std::multimap<int, cv::KeyPoint> & wordsA,
302  const std::multimap<int, cv::KeyPoint> & wordsB,
303  std::list<std::pair<cv::Point2f, cv::Point2f> > & pairs)
304 {
305  const std::list<int> & ids = uUniqueKeys(wordsA);
306  pairs.clear();
307  for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
308  {
309  std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
310  std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
311  if(ptsA.size() == 1 && ptsB.size() == 1)
312  {
313  pairs.push_back(std::pair<cv::Point2f, cv::Point2f>(ptsA.front().pt, ptsB.front().pt));
314  }
315  }
316 }
317 
318 void findCorrespondences(
319  const std::multimap<int, cv::Point3f> & words1,
320  const std::multimap<int, cv::Point3f> & words2,
321  std::vector<cv::Point3f> & inliers1,
322  std::vector<cv::Point3f> & inliers2,
323  float maxDepth,
324  std::vector<int> * uniqueCorrespondences)
325 {
326  std::list<int> ids = uUniqueKeys(words1);
327  // Find pairs
328  inliers1.resize(ids.size());
329  inliers2.resize(ids.size());
330  if(uniqueCorrespondences)
331  {
332  uniqueCorrespondences->resize(ids.size());
333  }
334 
335  int oi=0;
336  for(std::list<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
337  {
338  if(words1.count(*iter) == 1 && words2.count(*iter) == 1)
339  {
340  inliers1[oi] = words1.find(*iter)->second;
341  inliers2[oi] = words2.find(*iter)->second;
342  if(util3d::isFinite(inliers1[oi]) &&
343  util3d::isFinite(inliers2[oi]) &&
344  (inliers1[oi].x != 0 || inliers1[oi].y != 0 || inliers1[oi].z != 0) &&
345  (inliers2[oi].x != 0 || inliers2[oi].y != 0 || inliers2[oi].z != 0) &&
346  (maxDepth <= 0 || (inliers1[oi].x > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth)))
347  {
348  if(uniqueCorrespondences)
349  {
350  uniqueCorrespondences->at(oi) = *iter;
351  }
352  ++oi;
353  }
354  }
355  }
356  inliers1.resize(oi);
357  inliers2.resize(oi);
358  if(uniqueCorrespondences)
359  {
360  uniqueCorrespondences->resize(oi);
361  }
362 }
363 
364 void findCorrespondences(
365  const std::map<int, cv::Point3f> & words1,
366  const std::map<int, cv::Point3f> & words2,
367  std::vector<cv::Point3f> & inliers1,
368  std::vector<cv::Point3f> & inliers2,
369  float maxDepth,
370  std::vector<int> * correspondences)
371 {
372  std::vector<int> ids = uKeys(words1);
373  // Find pairs
374  inliers1.resize(ids.size());
375  inliers2.resize(ids.size());
376  if(correspondences)
377  {
378  correspondences->resize(ids.size());
379  }
380 
381  int oi=0;
382  for(std::vector<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
383  {
384  if(words2.find(*iter) != words2.end())
385  {
386  inliers1[oi] = words1.find(*iter)->second;
387  inliers2[oi] = words2.find(*iter)->second;
388  if(util3d::isFinite(inliers1[oi]) &&
389  util3d::isFinite(inliers2[oi]) &&
390  (inliers1[oi].x != 0 || inliers1[oi].y != 0 || inliers1[oi].z != 0) &&
391  (inliers2[oi].x != 0 || inliers2[oi].y != 0 || inliers2[oi].z != 0) &&
392  (maxDepth <= 0 || (inliers1[oi].x > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth)))
393  {
394  if(correspondences)
395  {
396  correspondences->at(oi) = *iter;
397  }
398  ++oi;
399  }
400  }
401  }
402  inliers1.resize(oi);
403  inliers2.resize(oi);
404  if(correspondences)
405  {
406  correspondences->resize(oi);
407  }
408 }
409 
410 }
411 
412 }
uUniqueKeys
std::list< K > uUniqueKeys(const std::multimap< K, V > &mm)
Definition: UStl.h:46
rtabmap::util3d::isFinite
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3194
uKeys
std::vector< K > uKeys(const std::multimap< K, V > &mm)
Definition: UStl.h:67
f
f
util3d_correspondences.h
UStl.h
Wrappers of STL for convenient functions.
rtabmap::util3d::projectDepthTo3D
pcl::PointXYZ RTABMAP_EXP projectDepthTo3D(const cv::Mat &depthImage, float x, float y, float cx, float cy, float fx, float fy, bool smoothing, float depthErrorRatio=0.02f)
Definition: util3d.cpp:214
rtabmap::util3d::extractXYZCorrespondencesRANSAC
void RTABMAP_EXP extractXYZCorrespondencesRANSAC(const std::multimap< int, pcl::PointXYZ > &words1, const std::multimap< int, pcl::PointXYZ > &words2, pcl::PointCloud< pcl::PointXYZ > &cloud1, pcl::PointCloud< pcl::PointXYZ > &cloud2)
Definition: util3d_correspondences.cpp:65
rtabmap::util3d::filterMaxDepth
void RTABMAP_EXP filterMaxDepth(pcl::PointCloud< pcl::PointXYZ > &inliers1, pcl::PointCloud< pcl::PointXYZ > &inliers2, float maxDepth, char depthAxis, bool removeDuplicates)
Definition: util3d_correspondences.cpp:226
rtabmap::util3d::extractXYZCorrespondencesImpl
void extractXYZCorrespondencesImpl(const std::list< std::pair< cv::Point2f, cv::Point2f > > &correspondences, const pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2, pcl::PointCloud< pcl::PointXYZ > &inliers1, pcl::PointCloud< pcl::PointXYZ > &inliers2, char depthAxis)
Definition: util3d_correspondences.cpp:168
rtabmap::util3d::getCorrespondencesCount
int RTABMAP_EXP getCorrespondencesCount(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_source, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_target, float maxDistance)
Definition: util3d_correspondences.cpp:276
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
rtabmap::util3d::extractXYZCorrespondences
void RTABMAP_EXP extractXYZCorrespondences(const std::multimap< int, pcl::PointXYZ > &words1, const std::multimap< int, pcl::PointXYZ > &words2, pcl::PointCloud< pcl::PointXYZ > &cloud1, pcl::PointCloud< pcl::PointXYZ > &cloud2)
Definition: util3d_correspondences.cpp:44
dist
dist
rtabmap::util3d::countUniquePairs
int RTABMAP_EXP countUniquePairs(const std::multimap< int, pcl::PointXYZ > &wordsA, const std::multimap< int, pcl::PointXYZ > &wordsB)
Definition: util3d_correspondences.cpp:209
rtabmap::util3d::findCorrespondences
void RTABMAP_EXP findCorrespondences(const std::multimap< int, cv::KeyPoint > &wordsA, const std::multimap< int, cv::KeyPoint > &wordsB, std::list< std::pair< cv::Point2f, cv::Point2f > > &pairs)
Definition: util3d_correspondences.cpp:300

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:38:58