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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:43:41