util3d_correspondences.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include "rtabmap/core/util3d_correspondences.h"
00029 #include "rtabmap/core/util3d.h"
00030 
00031 #include <rtabmap/utilite/UStl.h>
00032 #include <rtabmap/core/EpipolarGeometry.h>
00033 #include <opencv2/calib3d/calib3d.hpp>
00034 #include <pcl/search/kdtree.h>
00035 
00036 namespace rtabmap
00037 {
00038 
00039 namespace util3d
00040 {
00041 
00042 
00043 void extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,
00044                                                                           const std::multimap<int, pcl::PointXYZ> & words2,
00045                                                                           pcl::PointCloud<pcl::PointXYZ> & cloud1,
00046                                                                           pcl::PointCloud<pcl::PointXYZ> & cloud2)
00047 {
00048         const std::list<int> & ids = uUniqueKeys(words1);
00049         for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
00050         {
00051                 if(words1.count(*i) == 1 && words2.count(*i) == 1)
00052                 {
00053                         const pcl::PointXYZ & pt1 = words1.find(*i)->second;
00054                         const pcl::PointXYZ & pt2 = words2.find(*i)->second;
00055                         if(pcl::isFinite(pt1) && pcl::isFinite(pt2))
00056                         {
00057                                 cloud1.push_back(pt1);
00058                                 cloud2.push_back(pt2);
00059                         }
00060                 }
00061         }
00062 }
00063 
00064 void extractXYZCorrespondencesRANSAC(const std::multimap<int, pcl::PointXYZ> & words1,
00065                                                                           const std::multimap<int, pcl::PointXYZ> & words2,
00066                                                                           pcl::PointCloud<pcl::PointXYZ> & cloud1,
00067                                                                           pcl::PointCloud<pcl::PointXYZ> & cloud2)
00068 {
00069         std::list<std::pair<pcl::PointXYZ, pcl::PointXYZ> > pairs;
00070         const std::list<int> & ids = uUniqueKeys(words1);
00071         for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
00072         {
00073                 if(words1.count(*i) == 1 && words2.count(*i) == 1)
00074                 {
00075                         const pcl::PointXYZ & pt1 = words1.find(*i)->second;
00076                         const pcl::PointXYZ & pt2 = words2.find(*i)->second;
00077                         if(pcl::isFinite(pt1) && pcl::isFinite(pt2))
00078                         {
00079                                 pairs.push_back(std::pair<pcl::PointXYZ, pcl::PointXYZ>(pt1, pt2));
00080                         }
00081                 }
00082         }
00083 
00084         if(pairs.size() > 7)
00085         {
00086                 // Remove outliers using fundamental matrix RANSAC
00087                 std::vector<uchar> status(pairs.size(), 0);
00088                 //Convert Keypoints to a structure that OpenCV understands
00089                 //3 dimensions (Homogeneous vectors)
00090                 cv::Mat points1(1, (int)pairs.size(), CV_32FC2);
00091                 cv::Mat points2(1, (int)pairs.size(), CV_32FC2);
00092 
00093                 float * points1data = points1.ptr<float>(0);
00094                 float * points2data = points2.ptr<float>(0);
00095 
00096                 // Fill the points here ...
00097                 int i=0;
00098                 for(std::list<std::pair<pcl::PointXYZ, pcl::PointXYZ> >::const_iterator iter = pairs.begin();
00099                         iter != pairs.end();
00100                         ++iter )
00101                 {
00102                         points1data[i*2] = (*iter).first.x;
00103                         points1data[i*2+1] = (*iter).first.y;
00104 
00105                         points2data[i*2] = (*iter).second.x;
00106                         points2data[i*2+1] = (*iter).second.y;
00107 
00108                         ++i;
00109                 }
00110 
00111                 // Find the fundamental matrix
00112                 cv::Mat fundamentalMatrix = cv::findFundamentalMat(
00113                                         points1,
00114                                         points2,
00115                                         status,
00116                                         cv::FM_RANSAC,
00117                                         3.0,
00118                                         0.99);
00119 
00120                 if(!fundamentalMatrix.empty())
00121                 {
00122                         int i = 0;
00123                         for(std::list<std::pair<pcl::PointXYZ, pcl::PointXYZ> >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
00124                         {
00125                                 if(status[i])
00126                                 {
00127                                         cloud1.push_back(iter->first);
00128                                         cloud2.push_back(iter->second);
00129                                 }
00130                                 ++i;
00131                         }
00132                 }
00133         }
00134 }
00135 
00136 void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
00137                                                                            const cv::Mat & depthImage1,
00138                                                                            const cv::Mat & depthImage2,
00139                                                                            float cx, float cy,
00140                                                                            float fx, float fy,
00141                                                                            float maxDepth,
00142                                                                            pcl::PointCloud<pcl::PointXYZ> & cloud1,
00143                                                                            pcl::PointCloud<pcl::PointXYZ> & cloud2)
00144 {
00145         cloud1.resize(correspondences.size());
00146         cloud2.resize(correspondences.size());
00147         int oi=0;
00148         for(std::list<std::pair<cv::Point2f, cv::Point2f> >::const_iterator iter = correspondences.begin();
00149                 iter!=correspondences.end();
00150                 ++iter)
00151         {
00152                 pcl::PointXYZ pt1 = projectDepthTo3D(depthImage1, iter->first.x, iter->first.y, cx, cy, fx, fy, true);
00153                 pcl::PointXYZ pt2 = projectDepthTo3D(depthImage2, iter->second.x, iter->second.y, cx, cy, fx, fy, true);
00154                 if(pcl::isFinite(pt1) && pcl::isFinite(pt2) &&
00155                    (maxDepth <= 0 || (pt1.z <= maxDepth && pt2.z<=maxDepth)))
00156                 {
00157                         cloud1[oi] = pt1;
00158                         cloud2[oi] = pt2;
00159                         ++oi;
00160                 }
00161         }
00162         cloud1.resize(oi);
00163         cloud2.resize(oi);
00164 }
00165 
00166 template<typename PointT>
00167 inline void extractXYZCorrespondencesImpl(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
00168                                                            const pcl::PointCloud<PointT> & cloud1,
00169                                                            const pcl::PointCloud<PointT> & cloud2,
00170                                                            pcl::PointCloud<pcl::PointXYZ> & inliers1,
00171                                                            pcl::PointCloud<pcl::PointXYZ> & inliers2,
00172                                                            char depthAxis)
00173 {
00174         for(std::list<std::pair<cv::Point2f, cv::Point2f> >::const_iterator iter = correspondences.begin();
00175                 iter!=correspondences.end();
00176                 ++iter)
00177         {
00178                 PointT pt1 = cloud1.at(int(iter->first.y+0.5f) * cloud1.width + int(iter->first.x+0.5f));
00179                 PointT pt2 = cloud2.at(int(iter->second.y+0.5f) * cloud2.width + int(iter->second.x+0.5f));
00180                 if(pcl::isFinite(pt1) &&
00181                    pcl::isFinite(pt2))
00182                 {
00183                         inliers1.push_back(pcl::PointXYZ(pt1.x, pt1.y, pt1.z));
00184                         inliers2.push_back(pcl::PointXYZ(pt2.x, pt2.y, pt2.z));
00185                 }
00186         }
00187 }
00188 
00189 void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
00190                                                            const pcl::PointCloud<pcl::PointXYZ> & cloud1,
00191                                                            const pcl::PointCloud<pcl::PointXYZ> & cloud2,
00192                                                            pcl::PointCloud<pcl::PointXYZ> & inliers1,
00193                                                            pcl::PointCloud<pcl::PointXYZ> & inliers2,
00194                                                            char depthAxis)
00195 {
00196         extractXYZCorrespondencesImpl(correspondences, cloud1, cloud2, inliers1, inliers2, depthAxis);
00197 }
00198 void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
00199                                                            const pcl::PointCloud<pcl::PointXYZRGB> & cloud1,
00200                                                            const pcl::PointCloud<pcl::PointXYZRGB> & cloud2,
00201                                                            pcl::PointCloud<pcl::PointXYZ> & inliers1,
00202                                                            pcl::PointCloud<pcl::PointXYZ> & inliers2,
00203                                                            char depthAxis)
00204 {
00205         extractXYZCorrespondencesImpl(correspondences, cloud1, cloud2, inliers1, inliers2, depthAxis);
00206 }
00207 
00208 int countUniquePairs(const std::multimap<int, pcl::PointXYZ> & wordsA,
00209                                          const std::multimap<int, pcl::PointXYZ> & wordsB)
00210 {
00211         const std::list<int> & ids = uUniqueKeys(wordsA);
00212         int pairs = 0;
00213         for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
00214         {
00215                 std::list<pcl::PointXYZ> ptsA = uValues(wordsA, *i);
00216                 std::list<pcl::PointXYZ> ptsB = uValues(wordsB, *i);
00217                 if(ptsA.size() == 1 && ptsB.size() == 1)
00218                 {
00219                         ++pairs;
00220                 }
00221         }
00222         return pairs;
00223 }
00224 
00225 void filterMaxDepth(pcl::PointCloud<pcl::PointXYZ> & inliers1,
00226                                         pcl::PointCloud<pcl::PointXYZ> & inliers2,
00227                                         float maxDepth,
00228                                         char depthAxis,
00229                                         bool removeDuplicates)
00230 {
00231         std::list<pcl::PointXYZ> addedPts;
00232         if(maxDepth > 0.0f && inliers1.size() && inliers1.size() == inliers2.size())
00233         {
00234                 pcl::PointCloud<pcl::PointXYZ> tmpInliers1;
00235                 pcl::PointCloud<pcl::PointXYZ> tmpInliers2;
00236                 for(unsigned int i=0; i<inliers1.size(); ++i)
00237                 {
00238                         if((depthAxis == 'x' && inliers1.at(i).x < maxDepth && inliers2.at(i).x < maxDepth) ||
00239                            (depthAxis == 'y' && inliers1.at(i).y < maxDepth && inliers2.at(i).y < maxDepth) ||
00240                            (depthAxis == 'z' && inliers1.at(i).z < maxDepth && inliers2.at(i).z < maxDepth))
00241                         {
00242                                 bool dup = false;
00243                                 if(removeDuplicates)
00244                                 {
00245                                         for(std::list<pcl::PointXYZ>::iterator iter = addedPts.begin(); iter!=addedPts.end(); ++iter)
00246                                         {
00247                                                 if(iter->x == inliers1.at(i).x &&
00248                                                    iter->y == inliers1.at(i).y &&
00249                                                    iter->z == inliers1.at(i).z)
00250                                                 {
00251                                                         dup = true;
00252                                                 }
00253                                         }
00254                                         if(!dup)
00255                                         {
00256                                                 addedPts.push_back(inliers1.at(i));
00257                                         }
00258                                 }
00259 
00260                                 if(!dup)
00261                                 {
00262                                         tmpInliers1.push_back(inliers1.at(i));
00263                                         tmpInliers2.push_back(inliers2.at(i));
00264                                 }
00265                         }
00266                 }
00267                 inliers1 = tmpInliers1;
00268                 inliers2 = tmpInliers2;
00269         }
00270 }
00271 
00272 
00273 // a kdtree is constructed with cloud_target, then nearest neighbor
00274 // is computed for each cloud_source points.
00275 int getCorrespondencesCount(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
00276                                                         const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
00277                                                         float maxDistance)
00278 {
00279         pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
00280         kdTree->setInputCloud(cloud_target);
00281         int count = 0;
00282         float sqrdMaxDistance = maxDistance * maxDistance;
00283         for(unsigned int i=0; i<cloud_source->size(); ++i)
00284         {
00285                 std::vector<int> ind(1);
00286                 std::vector<float> dist(1);
00287                 if(kdTree->nearestKSearch(cloud_source->at(i), 1, ind, dist) && dist[0] < sqrdMaxDistance)
00288                 {
00289                         ++count;
00290                 }
00291         }
00292         return count;
00293 }
00294 
00299 void findCorrespondences(
00300                 const std::multimap<int, cv::KeyPoint> & wordsA,
00301                 const std::multimap<int, cv::KeyPoint> & wordsB,
00302                 std::list<std::pair<cv::Point2f, cv::Point2f> > & pairs)
00303 {
00304         const std::list<int> & ids = uUniqueKeys(wordsA);
00305         pairs.clear();
00306         for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
00307         {
00308                 std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
00309                 std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
00310                 if(ptsA.size() == 1 && ptsB.size() == 1)
00311                 {
00312                         pairs.push_back(std::pair<cv::Point2f, cv::Point2f>(ptsA.front().pt, ptsB.front().pt));
00313                 }
00314         }
00315 }
00316 
00317 void findCorrespondences(
00318                 const std::multimap<int, cv::Point3f> & words1,
00319                 const std::multimap<int, cv::Point3f> & words2,
00320                 std::vector<cv::Point3f> & inliers1,
00321                 std::vector<cv::Point3f> & inliers2,
00322                 float maxDepth,
00323                 std::vector<int> * uniqueCorrespondences)
00324 {
00325         std::list<int> ids = uUniqueKeys(words1);
00326         // Find pairs
00327         inliers1.resize(ids.size());
00328         inliers2.resize(ids.size());
00329         if(uniqueCorrespondences)
00330         {
00331                 uniqueCorrespondences->resize(ids.size());
00332         }
00333 
00334         int oi=0;
00335         for(std::list<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
00336         {
00337                 if(words1.count(*iter) == 1 && words2.count(*iter) == 1)
00338                 {
00339                         inliers1[oi] = words1.find(*iter)->second;
00340                         inliers2[oi] = words2.find(*iter)->second;
00341                         if(util3d::isFinite(inliers1[oi]) &&
00342                            util3d::isFinite(inliers2[oi]) &&
00343                            (inliers1[oi].x != 0 || inliers1[oi].y != 0 || inliers1[oi].z != 0) &&
00344                            (inliers2[oi].x != 0 || inliers2[oi].y != 0 || inliers2[oi].z != 0) &&
00345                            (maxDepth <= 0 || (inliers1[oi].x > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth)))
00346                         {
00347                                 if(uniqueCorrespondences)
00348                                 {
00349                                         uniqueCorrespondences->at(oi) = *iter;
00350                                 }
00351                                 ++oi;
00352                         }
00353                 }
00354         }
00355         inliers1.resize(oi);
00356         inliers2.resize(oi);
00357         if(uniqueCorrespondences)
00358         {
00359                 uniqueCorrespondences->resize(oi);
00360         }
00361 }
00362 
00363 void findCorrespondences(
00364                 const std::map<int, cv::Point3f> & words1,
00365                 const std::map<int, cv::Point3f> & words2,
00366                 std::vector<cv::Point3f> & inliers1,
00367                 std::vector<cv::Point3f> & inliers2,
00368                 float maxDepth,
00369                 std::vector<int> * correspondences)
00370 {
00371         std::vector<int> ids = uKeys(words1);
00372         // Find pairs
00373         inliers1.resize(ids.size());
00374         inliers2.resize(ids.size());
00375         if(correspondences)
00376         {
00377                 correspondences->resize(ids.size());
00378         }
00379 
00380         int oi=0;
00381         for(std::vector<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
00382         {
00383                 if(words2.find(*iter) != words2.end())
00384                 {
00385                         inliers1[oi] = words1.find(*iter)->second;
00386                         inliers2[oi] = words2.find(*iter)->second;
00387                         if(util3d::isFinite(inliers1[oi]) &&
00388                            util3d::isFinite(inliers2[oi]) &&
00389                            (inliers1[oi].x != 0 || inliers1[oi].y != 0 || inliers1[oi].z != 0) &&
00390                            (inliers2[oi].x != 0 || inliers2[oi].y != 0 || inliers2[oi].z != 0) &&
00391                            (maxDepth <= 0 || (inliers1[oi].x > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth)))
00392                         {
00393                                 if(correspondences)
00394                                 {
00395                                         correspondences->at(oi) = *iter;
00396                                 }
00397                                 ++oi;
00398                         }
00399                 }
00400         }
00401         inliers1.resize(oi);
00402         inliers2.resize(oi);
00403         if(correspondences)
00404         {
00405                 correspondences->resize(oi);
00406         }
00407 }
00408 
00409 }
00410 
00411 }


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:32