util3d.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2014, 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/EpipolarGeometry.h>
00029 #include <rtabmap/utilite/UMath.h>
00030 #include <rtabmap/utilite/UStl.h>
00031 #include <rtabmap/utilite/ULogger.h>
00032 #include <pcl/registration/transformation_estimation_svd.h>
00033 #include <pcl/registration/transformation_estimation_2D.h>
00034 #include <pcl/registration/correspondence_rejection_sample_consensus.h>
00035 #include <pcl/registration/icp_nl.h>
00036 #include <pcl/io/pcd_io.h>
00037 #include <pcl/common/distances.h>
00038 #include <pcl/surface/gp3.h>
00039 #include <pcl/features/normal_3d_omp.h>
00040 #include <pcl/surface/mls.h>
00041 #include <pcl/ModelCoefficients.h>
00042 #include <pcl/segmentation/sac_segmentation.h>
00043 
00044 #include <opencv2/calib3d/calib3d.hpp>
00045 #include <opencv2/video/tracking.hpp>
00046 #include <rtabmap/core/VWDictionary.h>
00047 #include <cmath>
00048 #include <stdio.h>
00049 
00050 #include "rtabmap/utilite/UConversion.h"
00051 #include "rtabmap/utilite/UTimer.h"
00052 #include "rtabmap/core/util3d.h"
00053 #include "rtabmap/core/Signature.h"
00054 
00055 #include <pcl/filters/random_sample.h>
00056 
00057 namespace rtabmap
00058 {
00059 
00060 namespace util3d
00061 {
00062 
00063 cv::Mat bgrFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder)
00064 {
00065         cv::Mat frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);
00066 
00067         for(unsigned int h = 0; h < cloud.height; h++)
00068         {
00069                 for(unsigned int w = 0; w < cloud.width; w++)
00070                 {
00071                         if(bgrOrder)
00072                         {
00073                                 frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).b;
00074                                 frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
00075                                 frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).r;
00076                         }
00077                         else
00078                         {
00079                                 frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).r;
00080                                 frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
00081                                 frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).b;
00082                         }
00083                 }
00084         }
00085         return frameBGR;
00086 }
00087 
00088 // return float image in meter
00089 cv::Mat depthFromCloud(
00090                 const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
00091                 float & fx,
00092                 float & fy,
00093                 bool depth16U)
00094 {
00095         cv::Mat frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1);
00096         fx = 0.0f; // needed to reconstruct the cloud
00097         fy = 0.0f; // needed to reconstruct the cloud
00098         for(unsigned int h = 0; h < cloud.height; h++)
00099         {
00100                 for(unsigned int w = 0; w < cloud.width; w++)
00101                 {
00102                         float depth = cloud.at(h*cloud.width + w).z;
00103                         if(depth16U)
00104                         {
00105                                 depth *= 1000.0f;
00106                                 unsigned short depthMM = 0;
00107                                 if(depth <= (float)USHRT_MAX)
00108                                 {
00109                                         depthMM = (unsigned short)depth;
00110                                 }
00111                                 frameDepth.at<unsigned short>(h,w) = depthMM;
00112                         }
00113                         else
00114                         {
00115                                 frameDepth.at<float>(h,w) = depth;
00116                         }
00117 
00118                         // update constants
00119                         if(fx == 0.0f &&
00120                            uIsFinite(cloud.at(h*cloud.width + w).x) &&
00121                            uIsFinite(depth) &&
00122                            w != cloud.width/2 &&
00123                            depth > 0)
00124                         {
00125                                 fx = cloud.at(h*cloud.width + w).x / ((float(w) - float(cloud.width)/2.0f) * depth);
00126                                 if(depth16U)
00127                                 {
00128                                         fx*=1000.0f;
00129                                 }
00130                         }
00131                         if(fy == 0.0f &&
00132                            uIsFinite(cloud.at(h*cloud.width + w).y) &&
00133                            uIsFinite(depth) &&
00134                            h != cloud.height/2 &&
00135                            depth > 0)
00136                         {
00137                                 fy = cloud.at(h*cloud.width + w).y / ((float(h) - float(cloud.height)/2.0f) * depth);
00138                                 if(depth16U)
00139                                 {
00140                                         fy*=1000.0f;
00141                                 }
00142                         }
00143                 }
00144         }
00145         return frameDepth;
00146 }
00147 
00148 // return (unsigned short 16bits image in mm) (float 32bits image in m)
00149 void rgbdFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
00150                 cv::Mat & frameBGR,
00151                 cv::Mat & frameDepth,
00152                 float & fx,
00153                 float & fy,
00154                 bool bgrOrder,
00155                 bool depth16U)
00156 {
00157         frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1);
00158         frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);
00159 
00160         fx = 0.0f; // needed to reconstruct the cloud
00161         fy = 0.0f; // needed to reconstruct the cloud
00162         for(unsigned int h = 0; h < cloud.height; h++)
00163         {
00164                 for(unsigned int w = 0; w < cloud.width; w++)
00165                 {
00166                         //rgb
00167                         if(bgrOrder)
00168                         {
00169                                 frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).b;
00170                                 frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
00171                                 frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).r;
00172                         }
00173                         else
00174                         {
00175                                 frameBGR.at<cv::Vec3b>(h,w)[0] = cloud.at(h*cloud.width + w).r;
00176                                 frameBGR.at<cv::Vec3b>(h,w)[1] = cloud.at(h*cloud.width + w).g;
00177                                 frameBGR.at<cv::Vec3b>(h,w)[2] = cloud.at(h*cloud.width + w).b;
00178                         }
00179 
00180                         //depth
00181                         float depth = cloud.at(h*cloud.width + w).z;
00182                         if(depth16U)
00183                         {
00184                                 depth *= 1000.0f;
00185                                 unsigned short depthMM = 0;
00186                                 if(depth <= (float)USHRT_MAX)
00187                                 {
00188                                         depthMM = (unsigned short)depth;
00189                                 }
00190                                 frameDepth.at<unsigned short>(h,w) = depthMM;
00191                         }
00192                         else
00193                         {
00194                                 frameDepth.at<float>(h,w) = depth;
00195                         }
00196 
00197                         // update constants
00198                         if(fx == 0.0f &&
00199                            uIsFinite(cloud.at(h*cloud.width + w).x) &&
00200                            uIsFinite(depth) &&
00201                            w != cloud.width/2 &&
00202                            depth > 0)
00203                         {
00204                                 fx = 1.0f/(cloud.at(h*cloud.width + w).x / ((float(w) - float(cloud.width)/2.0f) * depth));
00205                                 if(depth16U)
00206                                 {
00207                                         fx/=1000.0f;
00208                                 }
00209                         }
00210                         if(fy == 0.0f &&
00211                            uIsFinite(cloud.at(h*cloud.width + w).y) &&
00212                            uIsFinite(depth) &&
00213                            h != cloud.height/2 &&
00214                            depth > 0)
00215                         {
00216                                 fy = 1.0f/(cloud.at(h*cloud.width + w).y / ((float(h) - float(cloud.height)/2.0f) * depth));
00217                                 if(depth16U)
00218                                 {
00219                                         fy/=1000.0f;
00220                                 }
00221                         }
00222                 }
00223         }
00224 }
00225 
00226 cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
00227 {
00228         UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
00229         cv::Mat depth16U;
00230         if(!depth32F.empty())
00231         {
00232                 depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
00233                 for(int i=0; i<depth32F.rows; ++i)
00234                 {
00235                         for(int j=0; j<depth32F.cols; ++j)
00236                         {
00237                                 float depth = (depth32F.at<float>(i,j)*1000.0f);
00238                                 unsigned short depthMM = 0;
00239                                 if(depth <= (float)USHRT_MAX)
00240                                 {
00241                                         depthMM = (unsigned short)depth;
00242                                 }
00243                                 depth16U.at<unsigned short>(i, j) = depthMM;
00244                         }
00245                 }
00246         }
00247         return depth16U;
00248 }
00249 
00250 cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
00251 {
00252         UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
00253         cv::Mat depth32F;
00254         if(!depth16U.empty())
00255         {
00256                 depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
00257                 for(int i=0; i<depth16U.rows; ++i)
00258                 {
00259                         for(int j=0; j<depth16U.cols; ++j)
00260                         {
00261                                 float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
00262                                 depth32F.at<float>(i, j) = depth;
00263                         }
00264                 }
00265         }
00266         return depth32F;
00267 }
00268 
00269 pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
00270                 const std::vector<cv::KeyPoint> & keypoints,
00271                 const cv::Mat & depth,
00272                 float fx,
00273                 float fy,
00274                 float cx,
00275                 float cy,
00276                 const Transform & transform)
00277 {
00278         UASSERT(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1));
00279         pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
00280         if(!depth.empty())
00281         {
00282                 keypoints3d->resize(keypoints.size());
00283                 for(unsigned int i=0; i!=keypoints.size(); ++i)
00284                 {
00285                         pcl::PointXYZ pt = util3d::projectDepthTo3D(
00286                                         depth,
00287                                         keypoints[i].pt.x,
00288                                         keypoints[i].pt.y,
00289                                         cx,
00290                                         cy,
00291                                         fx,
00292                                         fy,
00293                                         true);
00294 
00295                         if(!transform.isNull() && !transform.isIdentity())
00296                         {
00297                                 pt = pcl::transformPoint(pt, transform.toEigen3f());
00298                         }
00299                         keypoints3d->at(i) = pt;
00300                 }
00301         }
00302         return keypoints3d;
00303 }
00304 
00305 pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
00306                 const std::vector<cv::KeyPoint> & keypoints,
00307                 const cv::Mat & disparity,
00308                 float fx,
00309                 float baseline,
00310                 float cx,
00311                 float cy,
00312                 const Transform & transform)
00313 {
00314         UASSERT(!disparity.empty() && (disparity.type() == CV_16SC1 || disparity.type() == CV_32F));
00315         pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
00316         keypoints3d->resize(keypoints.size());
00317         for(unsigned int i=0; i!=keypoints.size(); ++i)
00318         {
00319                 pcl::PointXYZ pt = util3d::projectDisparityTo3D(
00320                                 keypoints[i].pt,
00321                                 disparity,
00322                                 cx,
00323                                 cy,
00324                                 fx,
00325                                 baseline);
00326 
00327                 if(pcl::isFinite(pt) && !transform.isNull() && !transform.isIdentity())
00328                 {
00329                         pt = pcl::transformPoint(pt, transform.toEigen3f());
00330                 }
00331                 keypoints3d->at(i) = pt;
00332         }
00333         return keypoints3d;
00334 }
00335 
00336 pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
00337                 const std::vector<cv::KeyPoint> & keypoints,
00338                 const cv::Mat & leftImage,
00339                 const cv::Mat & rightImage,
00340                 float fx,
00341                 float baseline,
00342                 float cx,
00343                 float cy,
00344                 const Transform & transform,
00345                 int flowWinSize,
00346                 int flowMaxLevel,
00347                 int flowIterations,
00348                 double flowEps)
00349 {
00350         UASSERT(!leftImage.empty() && !rightImage.empty() &&
00351                         leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
00352                         leftImage.rows == rightImage.rows && leftImage.cols == rightImage.cols);
00353 
00354         std::vector<cv::Point2f> leftCorners;
00355         cv::KeyPoint::convert(keypoints, leftCorners);
00356 
00357         // Find features in the new left image
00358         std::vector<unsigned char> status;
00359         std::vector<float> err;
00360         std::vector<cv::Point2f> rightCorners;
00361         UDEBUG("cv::calcOpticalFlowPyrLK() begin");
00362         cv::calcOpticalFlowPyrLK(
00363                         leftImage,
00364                         rightImage,
00365                         leftCorners,
00366                         rightCorners,
00367                         status,
00368                         err,
00369                         cv::Size(flowWinSize, flowWinSize), flowMaxLevel,
00370                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations, flowEps),
00371                         cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
00372         UDEBUG("cv::calcOpticalFlowPyrLK() end");
00373 
00374         pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
00375         keypoints3d->resize(keypoints.size());
00376         float bad_point = std::numeric_limits<float>::quiet_NaN ();
00377         UASSERT(status.size() == keypoints.size());
00378         for(unsigned int i=0; i<status.size(); ++i)
00379         {
00380                 pcl::PointXYZ pt(bad_point, bad_point, bad_point);
00381                 if(status[i])
00382                 {
00383                         float disparity = leftCorners[i].x - rightCorners[i].x;
00384                         if(disparity > 0.0f)
00385                         {
00386                                 pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
00387                                                 leftCorners[i],
00388                                                 disparity,
00389                                                 cx, cy, fx, baseline);
00390 
00391                                 if(pcl::isFinite(tmpPt))
00392                                 {
00393                                         pt = tmpPt;
00394                                         if(!transform.isNull() && !transform.isIdentity())
00395                                         {
00396                                                 pt = pcl::transformPoint(pt, transform.toEigen3f());
00397                                         }
00398                                 }
00399                         }
00400                 }
00401 
00402                 keypoints3d->at(i) = pt;
00403         }
00404         return keypoints3d;
00405 }
00406 
00407 // cameraTransform, from ref to next
00408 // return 3D points in ref referential
00409 // If cameraTransform is not null, it will be used for triangulation instead of the camera transform computed by epipolar geometry
00410 // when refGuess3D is passed and cameraTransform is null, scale will be estimated, returning scaled cloud and camera transform
00411 std::multimap<int, pcl::PointXYZ> generateWords3DMono(
00412                 const std::multimap<int, cv::KeyPoint> & refWords,
00413                 const std::multimap<int, cv::KeyPoint> & nextWords,
00414                 float fx,
00415                 float fy,
00416                 float cx,
00417                 float cy,
00418                 const Transform & localTransform,
00419                 Transform & cameraTransform,
00420                 int pnpIterations,
00421                 float pnpReprojError,
00422                 int pnpFlags,
00423                 float ransacParam1,
00424                 float ransacParam2,
00425                 const std::multimap<int, pcl::PointXYZ> & refGuess3D,
00426                 double * varianceOut)
00427 {
00428         std::multimap<int, pcl::PointXYZ> words3D;
00429         std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
00430         if(EpipolarGeometry::findPairsUnique(refWords, nextWords, pairs) > 8)
00431         {
00432                 std::vector<unsigned char> status;
00433                 cv::Mat F = EpipolarGeometry::findFFromWords(pairs, status, ransacParam1, ransacParam2);
00434                 if(!F.empty())
00435                 {
00436                         //get inliers
00437                         //normalize coordinates
00438                         int oi = 0;
00439                         UASSERT(status.size() == pairs.size());
00440                         std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin();
00441                         std::vector<cv::Point2f> refCorners(status.size());
00442                         std::vector<cv::Point2f> newCorners(status.size());
00443                         std::vector<int> indexes(status.size());
00444                         for(unsigned int i=0; i<status.size(); ++i)
00445                         {
00446                                 if(status[i])
00447                                 {
00448                                         refCorners[oi] = iter->second.first.pt;
00449                                         newCorners[oi] = iter->second.second.pt;
00450                                         indexes[oi] = iter->first;
00451                                         ++oi;
00452                                 }
00453                                 ++iter;
00454                         }
00455                         refCorners.resize(oi);
00456                         newCorners.resize(oi);
00457                         indexes.resize(oi);
00458 
00459                         UDEBUG("inliers=%d/%d", oi, pairs.size());
00460                         if(oi > 3)
00461                         {
00462                                 std::vector<cv::Point2f> refCornersRefined;
00463                                 std::vector<cv::Point2f> newCornersRefined;
00464                                 cv::correctMatches(F, refCorners, newCorners, refCornersRefined, newCornersRefined);
00465                                 refCorners = refCornersRefined;
00466                                 newCorners = newCornersRefined;
00467 
00468                                 cv::Mat x(3, (int)refCorners.size(), CV_64FC1);
00469                                 cv::Mat xp(3, (int)refCorners.size(), CV_64FC1);
00470                                 for(unsigned int i=0; i<refCorners.size(); ++i)
00471                                 {
00472                                         x.at<double>(0, i) = refCorners[i].x;
00473                                         x.at<double>(1, i) = refCorners[i].y;
00474                                         x.at<double>(2, i) = 1;
00475 
00476                                         xp.at<double>(0, i) = newCorners[i].x;
00477                                         xp.at<double>(1, i) = newCorners[i].y;
00478                                         xp.at<double>(2, i) = 1;
00479                                 }
00480 
00481                                 cv::Mat K = (cv::Mat_<double>(3,3) <<
00482                                         fx, 0, cx,
00483                                         0, fy, cy,
00484                                         0, 0, 1);
00485                                 cv::Mat Kinv = K.inv();
00486                                 cv::Mat E = K.t()*F*K;
00487                                 cv::Mat x_norm = Kinv * x;
00488                                 cv::Mat xp_norm = Kinv * xp;
00489                                 x_norm = x_norm.rowRange(0,2);
00490                                 xp_norm = xp_norm.rowRange(0,2);
00491 
00492                                 cv::Mat P = EpipolarGeometry::findPFromE(E, x_norm, xp_norm);
00493                                 if(!P.empty())
00494                                 {
00495                                         cv::Mat P0 = cv::Mat::zeros(3, 4, CV_64FC1);
00496                                         P0.at<double>(0,0) = 1;
00497                                         P0.at<double>(1,1) = 1;
00498                                         P0.at<double>(2,2) = 1;
00499 
00500                                         bool useCameraTransformGuess = !cameraTransform.isNull();
00501                                         //if camera transform is set, use it instead of the computed one from epipolar geometry
00502                                         if(useCameraTransformGuess)
00503                                         {
00504                                                 Transform t = (localTransform.inverse()*cameraTransform*localTransform).inverse();
00505                                                 P = (cv::Mat_<double>(3,4) <<
00506                                                                 (double)t.r11(), (double)t.r12(), (double)t.r13(), (double)t.x(),
00507                                                                 (double)t.r21(), (double)t.r22(), (double)t.r23(), (double)t.y(),
00508                                                                 (double)t.r31(), (double)t.r32(), (double)t.r33(), (double)t.z());
00509                                         }
00510 
00511                                         // triangulate the points
00512                                         //std::vector<double> reprojErrors;
00513                                         //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
00514                                         //EpipolarGeometry::triangulatePoints(x_norm, xp_norm, P0, P, cloud, reprojErrors);
00515                                         cv::Mat pts4D;
00516                                         cv::triangulatePoints(P0, P, x_norm, xp_norm, pts4D);
00517 
00518                                         for(unsigned int i=0; i<indexes.size(); ++i)
00519                                         {
00520                                                 //if(cloud->at(i).z > 0)
00521                                                 //{
00522                                                 //      words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(cloud->at(i), localTransform)));
00523                                                 //}
00524                                                 pts4D.col(i) /= pts4D.at<double>(3,i);
00525                                                 if(pts4D.at<double>(2,i) > 0)
00526                                                 {
00527                                                         words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(pcl::PointXYZ(pts4D.at<double>(0,i), pts4D.at<double>(1,i), pts4D.at<double>(2,i)), localTransform)));
00528                                                 }
00529                                         }
00530 
00531                                         if(!useCameraTransformGuess)
00532                                         {
00533                                                 cv::Mat R, T;
00534                                                 EpipolarGeometry::findRTFromP(P, R, T);
00535 
00536                                                 Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), T.at<double>(0),
00537                                                                         R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1),
00538                                                                         R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2));
00539 
00540                                                 cameraTransform = (localTransform * t).inverse() * localTransform;
00541                                         }
00542 
00543                                         if(refGuess3D.size())
00544                                         {
00545                                                 // scale estimation
00546                                                 pcl::PointCloud<pcl::PointXYZ>::Ptr inliersRef(new pcl::PointCloud<pcl::PointXYZ>);
00547                                                 pcl::PointCloud<pcl::PointXYZ>::Ptr inliersRefGuess(new pcl::PointCloud<pcl::PointXYZ>);
00548                                                 util3d::findCorrespondences(
00549                                                                 words3D,
00550                                                                 refGuess3D,
00551                                                                 *inliersRef,
00552                                                                 *inliersRefGuess,
00553                                                                 0);
00554 
00555                                                 if(inliersRef->size())
00556                                                 {
00557                                                         // estimate the scale
00558                                                         float scale = 1.0f;
00559                                                         float variance = 1.0f;
00560                                                         if(!useCameraTransformGuess)
00561                                                         {
00562                                                                 std::multimap<float, float> scales; // <variance, scale>
00563                                                                 for(unsigned int i=0; i<inliersRef->size(); ++i)
00564                                                                 {
00565                                                                         // using x as depth, assuming we are in global referential
00566                                                                         float s = inliersRefGuess->at(i).x/inliersRef->at(i).x;
00567                                                                         std::vector<float> errorSqrdDists(inliersRef->size());
00568                                                                         for(unsigned int j=0; j<inliersRef->size(); ++j)
00569                                                                         {
00570                                                                                 pcl::PointXYZ refPt = inliersRef->at(j);
00571                                                                                 refPt.x *= s;
00572                                                                                 refPt.y *= s;
00573                                                                                 refPt.z *= s;
00574                                                                                 const pcl::PointXYZ & newPt = inliersRefGuess->at(j);
00575                                                                                 errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
00576                                                                         }
00577                                                                         std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
00578                                                                         double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
00579                                                                         float var = 2.1981 * median_error_sqr;
00580                                                                         //UDEBUG("scale %d = %f variance = %f", (int)i, s, variance);
00581 
00582                                                                         scales.insert(std::make_pair(var, s));
00583                                                                 }
00584                                                                 scale = scales.begin()->second;
00585                                                                 variance = scales.begin()->first;;
00586                                                         }
00587                                                         else
00588                                                         {
00589                                                                 //compute variance at scale=1
00590                                                                 std::vector<float> errorSqrdDists(inliersRef->size());
00591                                                                 for(unsigned int j=0; j<inliersRef->size(); ++j)
00592                                                                 {
00593                                                                         const pcl::PointXYZ & refPt = inliersRef->at(j);
00594                                                                         const pcl::PointXYZ & newPt = inliersRefGuess->at(j);
00595                                                                         errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
00596                                                                 }
00597                                                                 std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
00598                                                                 double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
00599                                                                  variance = 2.1981 * median_error_sqr;
00600                                                         }
00601 
00602                                                         UDEBUG("scale used = %f (variance=%f)", scale, variance);
00603                                                         if(varianceOut)
00604                                                         {
00605                                                                 *varianceOut = variance;
00606                                                         }
00607 
00608                                                         if(!useCameraTransformGuess)
00609                                                         {
00610                                                                 std::vector<cv::Point3f> objectPoints(indexes.size());
00611                                                                 std::vector<cv::Point2f> imagePoints(indexes.size());
00612                                                                 int oi=0;
00613                                                                 for(unsigned int i=0; i<indexes.size(); ++i)
00614                                                                 {
00615                                                                         std::multimap<int, pcl::PointXYZ>::iterator iter = words3D.find(indexes[i]);
00616                                                                         if(pcl::isFinite(iter->second))
00617                                                                         {
00618                                                                                 iter->second.x *= scale;
00619                                                                                 iter->second.y *= scale;
00620                                                                                 iter->second.z *= scale;
00621                                                                                 objectPoints[oi].x = iter->second.x;
00622                                                                                 objectPoints[oi].y = iter->second.y;
00623                                                                                 objectPoints[oi].z = iter->second.z;
00624                                                                                 imagePoints[oi] = newCorners[i];
00625                                                                                 ++oi;
00626                                                                         }
00627                                                                 }
00628                                                                 objectPoints.resize(oi);
00629                                                                 imagePoints.resize(oi);
00630 
00631                                                                 //PnPRansac
00632                                                                 Transform guess = localTransform.inverse();
00633                                                                 cv::Mat R = (cv::Mat_<double>(3,3) <<
00634                                                                                 (double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
00635                                                                                 (double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
00636                                                                                 (double)guess.r31(), (double)guess.r32(), (double)guess.r33());
00637                                                                 cv::Mat rvec(1,3, CV_64FC1);
00638                                                                 cv::Rodrigues(R, rvec);
00639                                                                 cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
00640                                                                 std::vector<int> inliersV;
00641                                                                 cv::solvePnPRansac(
00642                                                                                 objectPoints,
00643                                                                                 imagePoints,
00644                                                                                 K,
00645                                                                                 cv::Mat(),
00646                                                                                 rvec,
00647                                                                                 tvec,
00648                                                                                 true,
00649                                                                                 pnpIterations,
00650                                                                                 pnpReprojError,
00651                                                                                 0,
00652                                                                                 inliersV,
00653                                                                                 pnpFlags);
00654 
00655                                                                 UDEBUG("PnP inliers = %d / %d", (int)inliersV.size(), (int)objectPoints.size());
00656 
00657                                                                 if(inliersV.size())
00658                                                                 {
00659                                                                         cv::Rodrigues(rvec, R);
00660                                                                         Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
00661                                                                                                    R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
00662                                                                                                    R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
00663 
00664                                                                         cameraTransform = (localTransform * pnp).inverse();
00665                                                                 }
00666                                                                 else
00667                                                                 {
00668                                                                         UWARN("No inliers after PnP!");
00669                                                                         cameraTransform = Transform();
00670                                                                 }
00671                                                         }
00672                                                 }
00673                                                 else
00674                                                 {
00675                                                         UWARN("Cannot compute the scale, no points corresponding between the generated ref words and words guess");
00676                                                 }
00677                                         }
00678                                 }
00679                         }
00680                 }
00681         }
00682         UDEBUG("wordsSet=%d / %d", (int)words3D.size(), (int)refWords.size());
00683 
00684         return words3D;
00685 }
00686 
00687 std::multimap<int, cv::KeyPoint> aggregate(
00688                 const std::list<int> & wordIds,
00689                 const std::vector<cv::KeyPoint> & keypoints)
00690 {
00691         std::multimap<int, cv::KeyPoint> words;
00692         std::vector<cv::KeyPoint>::const_iterator kpIter = keypoints.begin();
00693         for(std::list<int>::const_iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
00694         {
00695                 words.insert(std::pair<int, cv::KeyPoint >(*iter, *kpIter));
00696                 ++kpIter;
00697         }
00698         return words;
00699 }
00700 
00701 std::list<std::pair<cv::Point2f, cv::Point2f> > findCorrespondences(
00702                 const std::multimap<int, cv::KeyPoint> & words1,
00703                 const std::multimap<int, cv::KeyPoint> & words2)
00704 {
00705         std::list<std::pair<cv::Point2f, cv::Point2f> > correspondences;
00706 
00707         // Find pairs
00708         std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
00709         rtabmap::EpipolarGeometry::findPairsUnique(words1, words2, pairs);
00710 
00711         if(pairs.size() > 7) // 8 min?
00712         {
00713                 // Find fundamental matrix
00714                 std::vector<uchar> status;
00715                 cv::Mat fundamentalMatrix = rtabmap::EpipolarGeometry::findFFromWords(pairs, status);
00716                 //ROS_INFO("inliers = %d/%d", uSum(status), pairs.size());
00717                 if(!fundamentalMatrix.empty())
00718                 {
00719                         int i = 0;
00720                         //int goodCount = 0;
00721                         for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
00722                         {
00723                                 if(status[i])
00724                                 {
00725                                         correspondences.push_back(std::pair<cv::Point2f, cv::Point2f>(iter->second.first.pt, iter->second.second.pt));
00726                                         //ROS_INFO("inliers kpts %f %f vs %f %f", iter->second.first.pt.x, iter->second.first.pt.y, iter->second.second.pt.x, iter->second.second.pt.y);
00727                                 }
00728                                 ++i;
00729                         }
00730                 }
00731         }
00732         return correspondences;
00733 }
00734 
00735 void findCorrespondences(
00736                 const std::multimap<int, pcl::PointXYZ> & words1,
00737                 const std::multimap<int, pcl::PointXYZ> & words2,
00738                 pcl::PointCloud<pcl::PointXYZ> & inliers1,
00739                 pcl::PointCloud<pcl::PointXYZ> & inliers2,
00740                 float maxDepth,
00741                 std::set<int> * uniqueCorrespondences)
00742 {
00743         std::list<int> ids = uUniqueKeys(words1);
00744         // Find pairs
00745         inliers1.resize(ids.size());
00746         inliers2.resize(ids.size());
00747 
00748         int oi=0;
00749         for(std::list<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
00750         {
00751                 if(words1.count(*iter) == 1 && words2.count(*iter) == 1)
00752                 {
00753                         inliers1[oi] = words1.find(*iter)->second;
00754                         inliers2[oi] = words2.find(*iter)->second;
00755                         if(pcl::isFinite(inliers1[oi]) &&
00756                            pcl::isFinite(inliers2[oi]) &&
00757                            (inliers1[oi].x != 0 || inliers1[oi].y != 0 || inliers1[oi].z != 0) &&
00758                            (inliers2[oi].x != 0 || inliers2[oi].y != 0 || inliers2[oi].z != 0) &&
00759                            (maxDepth <= 0 || (inliers1[oi].x > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth)))
00760                         {
00761                                 ++oi;
00762                                 if(uniqueCorrespondences)
00763                                 {
00764                                         uniqueCorrespondences->insert(*iter);
00765                                 }
00766                         }
00767                 }
00768         }
00769         inliers1.resize(oi);
00770         inliers2.resize(oi);
00771 }
00772 
00773 float getDepth(
00774                 const cv::Mat & depthImage,
00775                 float x, float y,
00776                 bool smoothing,
00777                 float maxZError)
00778 {
00779         UASSERT(!depthImage.empty());
00780         UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
00781 
00782         int u = int(x+0.5f);
00783         int v = int(y+0.5f);
00784 
00785         if(!(u >=0 && u<depthImage.cols && v >=0 && v<depthImage.rows))
00786         {
00787                 UERROR("!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows) cond failed! returning bad point. (x=%f (u=%d), y=%f (v=%d), cols=%d, rows=%d)",
00788                                 x,u,y,v,depthImage.cols, depthImage.rows);
00789                 return 0;
00790         }
00791 
00792         bool isInMM = depthImage.type() == CV_16UC1; // is in mm?
00793 
00794         // Inspired from RGBDFrame::getGaussianMixtureDistribution() method from
00795         // https://github.com/ccny-ros-pkg/rgbdtools/blob/master/src/rgbd_frame.cpp
00796         // Window weights:
00797         //  | 1 | 2 | 1 |
00798         //  | 2 | 4 | 2 |
00799         //  | 1 | 2 | 1 |
00800         int u_start = std::max(u-1, 0);
00801         int v_start = std::max(v-1, 0);
00802         int u_end = std::min(u+1, depthImage.cols-1);
00803         int v_end = std::min(v+1, depthImage.rows-1);
00804 
00805         float depth = isInMM?(float)depthImage.at<uint16_t>(v,u)*0.001f:depthImage.at<float>(v,u);
00806         if(depth!=0.0f && uIsFinite(depth))
00807         {
00808                 if(smoothing)
00809                 {
00810                         float sumWeights = 0.0f;
00811                         float sumDepths = 0.0f;
00812                         for(int uu = u_start; uu <= u_end; ++uu)
00813                         {
00814                                 for(int vv = v_start; vv <= v_end; ++vv)
00815                                 {
00816                                         if(!(uu == u && vv == v))
00817                                         {
00818                                                 float d = isInMM?(float)depthImage.at<uint16_t>(vv,uu)*0.001f:depthImage.at<float>(vv,uu);
00819                                                 // ignore if not valid or depth difference is too high
00820                                                 if(d != 0.0f && uIsFinite(d) && fabs(d - depth) < maxZError)
00821                                                 {
00822                                                         if(uu == u || vv == v)
00823                                                         {
00824                                                                 sumWeights+=2.0f;
00825                                                                 d*=2.0f;
00826                                                         }
00827                                                         else
00828                                                         {
00829                                                                 sumWeights+=1.0f;
00830                                                         }
00831                                                         sumDepths += d;
00832                                                 }
00833                                         }
00834                                 }
00835                         }
00836                         // set window weight to center point
00837                         depth *= 4.0f;
00838                         sumWeights += 4.0f;
00839 
00840                         // mean
00841                         depth = (depth+sumDepths)/sumWeights;
00842                 }
00843         }
00844         else
00845         {
00846                 depth = 0;
00847         }
00848         return depth;
00849 }
00850 
00851 pcl::PointXYZ projectDepthTo3D(
00852                 const cv::Mat & depthImage,
00853                 float x, float y,
00854                 float cx, float cy,
00855                 float fx, float fy,
00856                 bool smoothing,
00857                 float maxZError)
00858 {
00859         UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
00860 
00861         pcl::PointXYZ pt;
00862         float bad_point = std::numeric_limits<float>::quiet_NaN ();
00863 
00864         float depth = getDepth(depthImage, x, y, smoothing, maxZError);
00865         if(depth)
00866         {
00867                 // Use correct principal point from calibration
00868                 cx = cx > 0.0f ? cx : float(depthImage.cols/2) - 0.5f; //cameraInfo.K.at(2)
00869                 cy = cy > 0.0f ? cy : float(depthImage.rows/2) - 0.5f; //cameraInfo.K.at(5)
00870 
00871                 // Fill in XYZ
00872                 pt.x = (x - cx) * depth / fx;
00873                 pt.y = (y - cy) * depth / fy;
00874                 pt.z = depth;
00875         }
00876         else
00877         {
00878                 pt.x = pt.y = pt.z = bad_point;
00879         }
00880         return pt;
00881 }
00882 
00883 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
00884                 const cv::Mat & imageDepth,
00885                 float cx, float cy,
00886                 float fx, float fy,
00887                 int decimation)
00888 {
00889         UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
00890         UASSERT(imageDepth.rows % decimation == 0);
00891         UASSERT(imageDepth.cols % decimation == 0);
00892 
00893         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
00894         if(decimation < 1)
00895         {
00896                 return cloud;
00897         }
00898 
00899         //cloud.header = cameraInfo.header;
00900         cloud->height = imageDepth.rows/decimation;
00901         cloud->width  = imageDepth.cols/decimation;
00902         cloud->is_dense = false;
00903 
00904         cloud->resize(cloud->height * cloud->width);
00905 
00906         int count = 0 ;
00907 
00908         for(int h = 0; h < imageDepth.rows; h+=decimation)
00909         {
00910                 for(int w = 0; w < imageDepth.cols; w+=decimation)
00911                 {
00912                         pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
00913 
00914                         pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, cx, cy, fx, fy, false);
00915                         pt.x = ptXYZ.x;
00916                         pt.y = ptXYZ.y;
00917                         pt.z = ptXYZ.z;
00918                         ++count;
00919                 }
00920         }
00921 
00922         return cloud;
00923 }
00924 
00925 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
00926                 const cv::Mat & imageRgb,
00927                 const cv::Mat & imageDepth,
00928                 float cx, float cy,
00929                 float fx, float fy,
00930                 int decimation)
00931 {
00932         UASSERT(imageRgb.rows == imageDepth.rows && imageRgb.cols == imageDepth.cols);
00933         UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
00934         UASSERT(imageDepth.rows % decimation == 0);
00935         UASSERT(imageDepth.cols % decimation == 0);
00936 
00937         pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00938         if(decimation < 1)
00939         {
00940                 return cloud;
00941         }
00942 
00943         bool mono;
00944         if(imageRgb.channels() == 3) // BGR
00945         {
00946                 mono = false;
00947         }
00948         else if(imageRgb.channels() == 1) // Mono
00949         {
00950                 mono = true;
00951         }
00952         else
00953         {
00954                 return cloud;
00955         }
00956 
00957         //cloud.header = cameraInfo.header;
00958         cloud->height = imageDepth.rows/decimation;
00959         cloud->width  = imageDepth.cols/decimation;
00960         cloud->is_dense = false;
00961         cloud->resize(cloud->height * cloud->width);
00962 
00963         for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
00964         {
00965                 for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
00966                 {
00967                         pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
00968                         if(!mono)
00969                         {
00970                                 pt.b = imageRgb.at<cv::Vec3b>(h,w)[0];
00971                                 pt.g = imageRgb.at<cv::Vec3b>(h,w)[1];
00972                                 pt.r = imageRgb.at<cv::Vec3b>(h,w)[2];
00973                         }
00974                         else
00975                         {
00976                                 unsigned char v = imageRgb.at<unsigned char>(h,w);
00977                                 pt.b = v;
00978                                 pt.g = v;
00979                                 pt.r = v;
00980                         }
00981 
00982                         pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, cx, cy, fx, fy, false);
00983                         pt.x = ptXYZ.x;
00984                         pt.y = ptXYZ.y;
00985                         pt.z = ptXYZ.z;
00986                 }
00987         }
00988         return cloud;
00989 }
00990 
00991 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
00992                 const cv::Mat & imageDisparity,
00993                 float cx, float cy,
00994                 float fx, float baseline,
00995                 int decimation)
00996 {
00997         UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1);
00998         UASSERT(imageDisparity.rows % decimation == 0);
00999         UASSERT(imageDisparity.cols % decimation == 0);
01000 
01001         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
01002         if(decimation < 1)
01003         {
01004                 return cloud;
01005         }
01006 
01007         //cloud.header = cameraInfo.header;
01008         cloud->height = imageDisparity.rows/decimation;
01009         cloud->width  = imageDisparity.cols/decimation;
01010         cloud->is_dense = false;
01011         cloud->resize(cloud->height * cloud->width);
01012 
01013         if(imageDisparity.type()==CV_16SC1)
01014         {
01015                 for(int h = 0; h < imageDisparity.rows && h/decimation < (int)cloud->height; h+=decimation)
01016                 {
01017                         for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
01018                         {
01019                                 float disp = float(imageDisparity.at<short>(h,w))/16.0f;
01020                                 cloud->at((h/decimation)*cloud->width + (w/decimation)) = projectDisparityTo3D(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
01021                         }
01022                 }
01023         }
01024         else
01025         {
01026                 for(int h = 0; h < imageDisparity.rows && h/decimation < (int)cloud->height; h+=decimation)
01027                 {
01028                         for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
01029                         {
01030                                 float disp = imageDisparity.at<float>(h,w);
01031                                 cloud->at((h/decimation)*cloud->width + (w/decimation)) = projectDisparityTo3D(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
01032                         }
01033                 }
01034         }
01035         return cloud;
01036 }
01037 
01038 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
01039                 const cv::Mat & imageRgb,
01040                 const cv::Mat & imageDisparity,
01041                 float cx, float cy,
01042                 float fx, float baseline,
01043                 int decimation)
01044 {
01045         UASSERT(imageRgb.rows == imageDisparity.rows &&
01046                         imageRgb.cols == imageDisparity.cols &&
01047                         (imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1));
01048         UASSERT(imageDisparity.rows % decimation == 0);
01049         UASSERT(imageDisparity.cols % decimation == 0);
01050         pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
01051         if(decimation < 1)
01052         {
01053                 return cloud;
01054         }
01055 
01056         bool mono;
01057         if(imageRgb.channels() == 3) // BGR
01058         {
01059                 mono = false;
01060         }
01061         else if(imageRgb.channels() == 1) // Mono
01062         {
01063                 mono = true;
01064         }
01065         else
01066         {
01067                 return cloud;
01068         }
01069 
01070         //cloud.header = cameraInfo.header;
01071         cloud->height = imageRgb.rows/decimation;
01072         cloud->width  = imageRgb.cols/decimation;
01073         cloud->is_dense = false;
01074         cloud->resize(cloud->height * cloud->width);
01075 
01076         for(int h = 0; h < imageRgb.rows && h/decimation < (int)cloud->height; h+=decimation)
01077         {
01078                 for(int w = 0; w < imageRgb.cols && w/decimation < (int)cloud->width; w+=decimation)
01079                 {
01080                         pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
01081                         if(!mono)
01082                         {
01083                                 pt.b = imageRgb.at<cv::Vec3b>(h,w)[0];
01084                                 pt.g = imageRgb.at<cv::Vec3b>(h,w)[1];
01085                                 pt.r = imageRgb.at<cv::Vec3b>(h,w)[2];
01086                         }
01087                         else
01088                         {
01089                                 unsigned char v = imageRgb.at<unsigned char>(h,w);
01090                                 pt.b = v;
01091                                 pt.g = v;
01092                                 pt.r = v;
01093                         }
01094 
01095                         float disp = imageDisparity.type()==CV_16SC1?float(imageDisparity.at<short>(h,w))/16.0f:imageDisparity.at<float>(h,w);
01096                         pcl::PointXYZ ptXYZ = projectDisparityTo3D(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
01097                         pt.x = ptXYZ.x;
01098                         pt.y = ptXYZ.y;
01099                         pt.z = ptXYZ.z;
01100                 }
01101         }
01102         return cloud;
01103 }
01104 
01105 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
01106                 const cv::Mat & imageLeft,
01107                 const cv::Mat & imageRight,
01108                 float cx, float cy,
01109                 float fx, float baseline,
01110                 int decimation)
01111 {
01112         UASSERT(imageRight.type() == CV_8UC1);
01113 
01114         cv::Mat leftMono;
01115         if(imageLeft.channels() == 3)
01116         {
01117                 cv::cvtColor(imageLeft, leftMono, CV_BGR2GRAY);
01118         }
01119         else
01120         {
01121                 leftMono = imageLeft;
01122         }
01123         return rtabmap::util3d::cloudFromDisparityRGB(
01124                         imageLeft,
01125                         util3d::disparityFromStereoImages(leftMono, imageRight),
01126                         cx, cy,
01127                         fx, baseline,
01128                         decimation);
01129 }
01130 
01131 cv::Mat disparityFromStereoImages(
01132                 const cv::Mat & leftImage,
01133                 const cv::Mat & rightImage)
01134 {
01135         UASSERT(!leftImage.empty() && !rightImage.empty() &&
01136                         (leftImage.type() == CV_8UC1 || leftImage.type() == CV_8UC3) && rightImage.type() == CV_8UC1 &&
01137                         leftImage.cols == rightImage.cols &&
01138                         leftImage.rows == rightImage.rows);
01139 
01140         cv::Mat leftMono;
01141         if(leftImage.channels() == 3)
01142         {
01143                 cv::cvtColor(leftImage, leftMono, CV_BGR2GRAY);
01144         }
01145         else
01146         {
01147                 leftMono = leftImage;
01148         }
01149 
01150         cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET);
01151         stereo.state->SADWindowSize = 15;
01152         stereo.state->minDisparity = 0;
01153         stereo.state->numberOfDisparities = 64;
01154         stereo.state->preFilterSize = 9;
01155         stereo.state->preFilterCap = 31;
01156         stereo.state->uniquenessRatio = 15;
01157         stereo.state->textureThreshold = 10;
01158         stereo.state->speckleWindowSize = 100;
01159         stereo.state->speckleRange = 4;
01160         cv::Mat disparity;
01161         stereo(leftMono, rightImage, disparity, CV_16SC1);
01162         return disparity;
01163 }
01164 
01165 cv::Mat disparityFromStereoImages(
01166                 const cv::Mat & leftImage,
01167                 const cv::Mat & rightImage,
01168                 const std::vector<cv::Point2f> & leftCorners,
01169                 int flowWinSize,
01170                 int flowMaxLevel,
01171                 int flowIterations,
01172                 double flowEps,
01173                 float maxCorrespondencesSlope)
01174 {
01175         UASSERT(!leftImage.empty() && !rightImage.empty() &&
01176                         leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
01177                         leftImage.cols == rightImage.cols &&
01178                         leftImage.rows == rightImage.rows);
01179 
01180         // Find features in the new left image
01181         std::vector<unsigned char> status;
01182         std::vector<float> err;
01183         std::vector<cv::Point2f> rightCorners;
01184         UDEBUG("cv::calcOpticalFlowPyrLK() begin");
01185         cv::calcOpticalFlowPyrLK(
01186                         leftImage,
01187                         rightImage,
01188                         leftCorners,
01189                         rightCorners,
01190                         status,
01191                         err,
01192                         cv::Size(flowWinSize, flowWinSize), flowMaxLevel,
01193                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations, flowEps),
01194                         cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
01195         UDEBUG("cv::calcOpticalFlowPyrLK() end");
01196 
01197         return disparityFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status, maxCorrespondencesSlope);
01198 }
01199 
01200 cv::Mat depthFromStereoImages(
01201                 const cv::Mat & leftImage,
01202                 const cv::Mat & rightImage,
01203                 const std::vector<cv::Point2f> & leftCorners,
01204                 float fx,
01205                 float baseline,
01206                 int flowWinSize,
01207                 int flowMaxLevel,
01208                 int flowIterations,
01209                 double flowEps)
01210 {
01211         UASSERT(!leftImage.empty() && !rightImage.empty() &&
01212                         leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
01213                         leftImage.cols == rightImage.cols &&
01214                         leftImage.rows == rightImage.rows);
01215         UASSERT(fx > 0.0f && baseline > 0.0f);
01216 
01217         // Find features in the new left image
01218         std::vector<unsigned char> status;
01219         std::vector<float> err;
01220         std::vector<cv::Point2f> rightCorners;
01221         UDEBUG("cv::calcOpticalFlowPyrLK() begin");
01222         cv::calcOpticalFlowPyrLK(
01223                         leftImage,
01224                         rightImage,
01225                         leftCorners,
01226                         rightCorners,
01227                         status,
01228                         err,
01229                         cv::Size(flowWinSize, flowWinSize), flowMaxLevel,
01230                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations, flowEps),
01231                         cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
01232         UDEBUG("cv::calcOpticalFlowPyrLK() end");
01233 
01234         return depthFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status, fx, baseline);
01235 }
01236 
01237 cv::Mat disparityFromStereoCorrespondences(
01238                 const cv::Mat & leftImage,
01239                 const std::vector<cv::Point2f> & leftCorners,
01240                 const std::vector<cv::Point2f> & rightCorners,
01241                 const std::vector<unsigned char> & mask,
01242                 float maxSlope)
01243 {
01244         UASSERT(!leftImage.empty() && leftCorners.size() == rightCorners.size());
01245         UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
01246         cv::Mat disparity = cv::Mat::zeros(leftImage.rows, leftImage.cols, CV_32FC1);
01247         for(unsigned int i=0; i<leftCorners.size(); ++i)
01248         {
01249                 if(mask.size() == 0 || mask[i])
01250                 {
01251                         float d = leftCorners[i].x - rightCorners[i].x;
01252                         float slope = fabs((leftCorners[i].y - rightCorners[i].y) / (leftCorners[i].x - rightCorners[i].x));
01253                         if(d > 0.0f && slope < maxSlope)
01254                         {
01255                                 disparity.at<float>(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d;
01256                         }
01257                 }
01258         }
01259         return disparity;
01260 }
01261 
01262 cv::Mat depthFromStereoCorrespondences(
01263                 const cv::Mat & leftImage,
01264                 const std::vector<cv::Point2f> & leftCorners,
01265                 const std::vector<cv::Point2f> & rightCorners,
01266                 const std::vector<unsigned char> & mask,
01267                 float fx, float baseline)
01268 {
01269         UASSERT(!leftImage.empty() && leftCorners.size() == rightCorners.size());
01270         UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
01271         cv::Mat depth = cv::Mat::zeros(leftImage.rows, leftImage.cols, CV_32FC1);
01272         for(unsigned int i=0; i<leftCorners.size(); ++i)
01273         {
01274                 if(mask.size() == 0 || mask[i])
01275                 {
01276                         float disparity = leftCorners[i].x - rightCorners[i].x;
01277                         if(disparity > 0.0f)
01278                         {
01279                                 float d = baseline * fx / disparity;
01280                                 depth.at<float>(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d;
01281                         }
01282                 }
01283         }
01284         return depth;
01285 }
01286 
01287 // inspired from ROS image_geometry/src/stereo_camera_model.cpp
01288 pcl::PointXYZ projectDisparityTo3D(
01289                 const cv::Point2f & pt,
01290                 float disparity,
01291                 float cx, float cy, float fx, float baseline)
01292 {
01293         if(disparity > 0.0f && baseline > 0.0f && fx > 0.0f)
01294         {
01295                 float W = disparity/baseline;// + (right_.cx() - left_.cx()) / Tx;
01296                 return pcl::PointXYZ((pt.x - cx)/W, (pt.y - cy)/W, fx/W);
01297         }
01298         float bad_point = std::numeric_limits<float>::quiet_NaN ();
01299         return pcl::PointXYZ(bad_point, bad_point, bad_point);
01300 }
01301 
01302 pcl::PointXYZ projectDisparityTo3D(
01303                 const cv::Point2f & pt,
01304                 const cv::Mat & disparity,
01305                 float cx, float cy, float fx, float baseline)
01306 {
01307         UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
01308         int u = int(pt.x+0.5f);
01309         int v = int(pt.y+0.5f);
01310         float bad_point = std::numeric_limits<float>::quiet_NaN ();
01311         if(uIsInBounds(u, 0, disparity.cols) &&
01312            uIsInBounds(v, 0, disparity.rows))
01313         {
01314                 float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u);
01315                 return projectDisparityTo3D(pt, d, cx, cy, fx, baseline);
01316         }
01317         return pcl::PointXYZ(bad_point, bad_point, bad_point);
01318 }
01319 
01320 cv::Mat depthFromDisparity(const cv::Mat & disparity,
01321                 float fx, float baseline,
01322                 int type)
01323 {
01324         UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
01325         UASSERT(type == CV_32FC1 || type == CV_16U);
01326         cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, type);
01327         for (int i = 0; i < disparity.rows; i++)
01328         {
01329                 for (int j = 0; j < disparity.cols; j++)
01330                 {
01331                         float disparity_value = disparity.type() == CV_16SC1?float(disparity.at<short>(i,j))/16.0f:disparity.at<float>(i,j);
01332                         if (disparity_value > 0.0f)
01333                         {
01334                                 // baseline * focal / disparity
01335                                 float d = baseline * fx / disparity_value;
01336                                 if(depth.type() == CV_32FC1)
01337                                 {
01338                                         depth.at<float>(i,j) = d;
01339                                 }
01340                                 else
01341                                 {
01342                                         depth.at<unsigned short>(i,j) = (unsigned short)(d*1000.0f);
01343                                 }
01344                         }
01345                 }
01346         }
01347         return depth;
01348 }
01349 
01350 cv::Mat registerDepth(
01351                 const cv::Mat & depth,
01352                 const cv::Mat & depthK,
01353                 const cv::Mat & colorK,
01354                 const rtabmap::Transform & transform)
01355 {
01356         UASSERT(!transform.isNull());
01357         UASSERT(!depth.empty());
01358         UASSERT(depth.type() == CV_16UC1); // mm
01359         UASSERT(depthK.type() == CV_64FC1 && depthK.cols == 3 && depthK.cols == 3);
01360         UASSERT(colorK.type() == CV_64FC1 && colorK.cols == 3 && colorK.cols == 3);
01361 
01362         float fx = depthK.at<double>(0,0);
01363         float fy = depthK.at<double>(1,1);
01364         float cx = depthK.at<double>(0,2);
01365         float cy = depthK.at<double>(1,2);
01366 
01367         float rfx = colorK.at<double>(0,0);
01368         float rfy = colorK.at<double>(1,1);
01369         float rcx = colorK.at<double>(0,2);
01370         float rcy = colorK.at<double>(1,2);
01371 
01372         Eigen::Affine3f proj = transform.toEigen3f();
01373         Eigen::Vector4f P4,P3;
01374         P4[3] = 1;
01375         cv::Mat registered = cv::Mat::zeros(depth.rows, depth.cols, depth.type());
01376 
01377         for(int y=0; y<depth.rows; ++y)
01378         {
01379                 for(int x=0; x<depth.cols; ++x)
01380                 {
01381                         //filtering
01382                         float dz = float(depth.at<unsigned short>(y,x))*0.001f; // put in meter for projection
01383                         if(dz>=0.0f)
01384                         {
01385                                 // Project to 3D
01386                                 P4[0] = (x - cx) * dz / fx; // Optimization: we could have (x-cx)/fx in a lookup table
01387                                 P4[1] = (y - cy) * dz / fy; // Optimization: we could have (y-cy)/fy in a lookup table
01388                                 P4[2] = dz;
01389 
01390                                 P3 = proj * P4;
01391                                 float z = P3[2];
01392                                 float invZ = 1.0f/z;
01393                                 int dx = (rfx*P3[0])*invZ + rcx;
01394                                 int dy = (rfy*P3[1])*invZ + rcy;
01395 
01396                                 if(uIsInBounds(dx, 0, registered.cols) && uIsInBounds(dy, 0, registered.rows))
01397                                 {
01398                                         unsigned short z16 = z * 1000; //mm
01399                                         unsigned short &zReg = registered.at<unsigned short>(dy, dx);
01400                                         if(zReg == 0 || z16 < zReg)
01401                                         {
01402                                                 zReg = z16;
01403                                         }
01404                                 }
01405                         }
01406                 }
01407         }
01408         return registered;
01409 }
01410 
01411 void fillRegisteredDepthHoles(cv::Mat & registeredDepth, bool vertical, bool horizontal, bool fillDoubleHoles)
01412 {
01413         UASSERT(registeredDepth.type() == CV_16UC1);
01414         int margin = fillDoubleHoles?2:1;
01415         for(int x=1; x<registeredDepth.cols-margin; ++x)
01416         {
01417                 for(int y=1; y<registeredDepth.rows-margin; ++y)
01418                 {
01419                         unsigned short & b = registeredDepth.at<unsigned short>(y, x);
01420                         bool set = false;
01421                         if(vertical)
01422                         {
01423                                 const unsigned short & a = registeredDepth.at<unsigned short>(y-1, x);
01424                                 unsigned short & c = registeredDepth.at<unsigned short>(y+1, x);
01425                                 if(a && c)
01426                                 {
01427                                         unsigned short error = 0.01*((a+c)/2);
01428                                         if(((b == 0 && a && c) || (b > a+error && b > c+error)) &&
01429                                                 (a>c?a-c<=error:c-a<=error))
01430                                         {
01431                                                 b = (a+c)/2;
01432                                                 set = true;
01433                                                 if(!horizontal)
01434                                                 {
01435                                                         ++y;
01436                                                 }
01437                                         }
01438                                 }
01439                                 if(!set && fillDoubleHoles)
01440                                 {
01441                                         const unsigned short & d = registeredDepth.at<unsigned short>(y+2, x);
01442                                         if(a && d && (b==0 || c==0))
01443                                         {
01444                                                 unsigned short error = 0.01*((a+d)/2);
01445                                                 if(((b == 0 && a && d) || (b > a+error && b > d+error)) &&
01446                                                    ((c == 0 && a && d) || (c > a+error && c > d+error)) &&
01447                                                         (a>d?a-d<=error:d-a<=error))
01448                                                 {
01449                                                         if(a>d)
01450                                                         {
01451                                                                 unsigned short tmp = (a-d)/4;
01452                                                                 b = d + tmp;
01453                                                                 c = d + 3*tmp;
01454                                                         }
01455                                                         else
01456                                                         {
01457                                                                 unsigned short tmp = (d-a)/4;
01458                                                                 b = a + tmp;
01459                                                                 c = a + 3*tmp;
01460                                                         }
01461                                                         set = true;
01462                                                         if(!horizontal)
01463                                                         {
01464                                                                 y+=2;
01465                                                         }
01466                                                 }
01467                                         }
01468                                 }
01469                         }
01470                         if(!set && horizontal)
01471                         {
01472                                 const unsigned short & a = registeredDepth.at<unsigned short>(y, x-1);
01473                                 unsigned short & c = registeredDepth.at<unsigned short>(y, x+1);
01474                                 if(a && c)
01475                                 {
01476                                         unsigned short error = 0.01*((a+c)/2);
01477                                         if(((b == 0 && a && c) || (b > a+error && b > c+error)) &&
01478                                                 (a>c?a-c<=error:c-a<=error))
01479                                         {
01480                                                 b = (a+c)/2;
01481                                                 set = true;
01482                                         }
01483                                 }
01484                                 if(!set && fillDoubleHoles)
01485                                 {
01486                                         const unsigned short & d = registeredDepth.at<unsigned short>(y, x+2);
01487                                         if(a && d && (b==0 || c==0))
01488                                         {
01489                                                 unsigned short error = 0.01*((a+d)/2);
01490                                                 if(((b == 0 && a && d) || (b > a+error && b > d+error)) &&
01491                                                    ((c == 0 && a && d) || (c > a+error && c > d+error)) &&
01492                                                         (a>d?a-d<=error:d-a<=error))
01493                                                 {
01494                                                         if(a>d)
01495                                                         {
01496                                                                 unsigned short tmp = (a-d)/4;
01497                                                                 b = d + tmp;
01498                                                                 c = d + 3*tmp;
01499                                                         }
01500                                                         else
01501                                                         {
01502                                                                 unsigned short tmp = (d-a)/4;
01503                                                                 b = a + tmp;
01504                                                                 c = a + 3*tmp;
01505                                                         }
01506                                                 }
01507                                         }
01508                                 }
01509                         }
01510                 }
01511         }
01512 }
01513 
01514 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
01515 {
01516         cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
01517         for(unsigned int i=0; i<cloud.size(); ++i)
01518         {
01519                 laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
01520                 laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
01521         }
01522         return laserScan;
01523 }
01524 
01525 pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan)
01526 {
01527         UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
01528 
01529         pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
01530         output->resize(laserScan.cols);
01531         for(int i=0; i<laserScan.cols; ++i)
01532         {
01533                 output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
01534                 output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
01535         }
01536         return output;
01537 }
01538 
01539 void extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,
01540                                                                           const std::multimap<int, pcl::PointXYZ> & words2,
01541                                                                           pcl::PointCloud<pcl::PointXYZ> & cloud1,
01542                                                                           pcl::PointCloud<pcl::PointXYZ> & cloud2)
01543 {
01544         const std::list<int> & ids = uUniqueKeys(words1);
01545         for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
01546         {
01547                 if(words1.count(*i) == 1 && words2.count(*i) == 1)
01548                 {
01549                         const pcl::PointXYZ & pt1 = words1.find(*i)->second;
01550                         const pcl::PointXYZ & pt2 = words2.find(*i)->second;
01551                         if(pcl::isFinite(pt1) && pcl::isFinite(pt2))
01552                         {
01553                                 cloud1.push_back(pt1);
01554                                 cloud2.push_back(pt2);
01555                         }
01556                 }
01557         }
01558 }
01559 
01560 void extractXYZCorrespondencesRANSAC(const std::multimap<int, pcl::PointXYZ> & words1,
01561                                                                           const std::multimap<int, pcl::PointXYZ> & words2,
01562                                                                           pcl::PointCloud<pcl::PointXYZ> & cloud1,
01563                                                                           pcl::PointCloud<pcl::PointXYZ> & cloud2)
01564 {
01565         std::list<std::pair<pcl::PointXYZ, pcl::PointXYZ> > pairs;
01566         const std::list<int> & ids = uUniqueKeys(words1);
01567         for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
01568         {
01569                 if(words1.count(*i) == 1 && words2.count(*i) == 1)
01570                 {
01571                         const pcl::PointXYZ & pt1 = words1.find(*i)->second;
01572                         const pcl::PointXYZ & pt2 = words2.find(*i)->second;
01573                         if(pcl::isFinite(pt1) && pcl::isFinite(pt2))
01574                         {
01575                                 pairs.push_back(std::pair<pcl::PointXYZ, pcl::PointXYZ>(pt1, pt2));
01576                         }
01577                 }
01578         }
01579 
01580         if(pairs.size() > 7)
01581         {
01582                 // Remove outliers using fundamental matrix RANSAC
01583                 std::vector<uchar> status(pairs.size(), 0);
01584                 //Convert Keypoints to a structure that OpenCV understands
01585                 //3 dimensions (Homogeneous vectors)
01586                 cv::Mat points1(1, (int)pairs.size(), CV_32FC2);
01587                 cv::Mat points2(1, (int)pairs.size(), CV_32FC2);
01588 
01589                 float * points1data = points1.ptr<float>(0);
01590                 float * points2data = points2.ptr<float>(0);
01591 
01592                 // Fill the points here ...
01593                 int i=0;
01594                 for(std::list<std::pair<pcl::PointXYZ, pcl::PointXYZ> >::const_iterator iter = pairs.begin();
01595                         iter != pairs.end();
01596                         ++iter )
01597                 {
01598                         points1data[i*2] = (*iter).first.x;
01599                         points1data[i*2+1] = (*iter).first.y;
01600 
01601                         points2data[i*2] = (*iter).second.x;
01602                         points2data[i*2+1] = (*iter).second.y;
01603 
01604                         ++i;
01605                 }
01606 
01607                 // Find the fundamental matrix
01608                 cv::Mat fundamentalMatrix = cv::findFundamentalMat(
01609                                         points1,
01610                                         points2,
01611                                         status,
01612                                         cv::FM_RANSAC,
01613                                         3.0,
01614                                         0.99);
01615 
01616                 if(!fundamentalMatrix.empty())
01617                 {
01618                         int i = 0;
01619                         for(std::list<std::pair<pcl::PointXYZ, pcl::PointXYZ> >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
01620                         {
01621                                 if(status[i])
01622                                 {
01623                                         cloud1.push_back(iter->first);
01624                                         cloud2.push_back(iter->second);
01625                                 }
01626                                 ++i;
01627                         }
01628                 }
01629         }
01630 }
01631 
01632 void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
01633                                                                            const cv::Mat & depthImage1,
01634                                                                            const cv::Mat & depthImage2,
01635                                                                            float cx, float cy,
01636                                                                            float fx, float fy,
01637                                                                            float maxDepth,
01638                                                                            pcl::PointCloud<pcl::PointXYZ> & cloud1,
01639                                                                            pcl::PointCloud<pcl::PointXYZ> & cloud2)
01640 {
01641         cloud1.resize(correspondences.size());
01642         cloud2.resize(correspondences.size());
01643         int oi=0;
01644         for(std::list<std::pair<cv::Point2f, cv::Point2f> >::const_iterator iter = correspondences.begin();
01645                 iter!=correspondences.end();
01646                 ++iter)
01647         {
01648                 pcl::PointXYZ pt1 = projectDepthTo3D(depthImage1, iter->first.x, iter->first.y, cx, cy, fx, fy, true);
01649                 pcl::PointXYZ pt2 = projectDepthTo3D(depthImage2, iter->second.x, iter->second.y, cx, cy, fx, fy, true);
01650                 if(pcl::isFinite(pt1) && pcl::isFinite(pt2) &&
01651                    (maxDepth <= 0 || (pt1.z <= maxDepth && pt2.z<=maxDepth)))
01652                 {
01653                         cloud1[oi] = pt1;
01654                         cloud2[oi] = pt2;
01655                         ++oi;
01656                 }
01657         }
01658         cloud1.resize(oi);
01659         cloud2.resize(oi);
01660 }
01661 
01662 template<typename PointT>
01663 inline void extractXYZCorrespondencesImpl(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
01664                                                            const pcl::PointCloud<PointT> & cloud1,
01665                                                            const pcl::PointCloud<PointT> & cloud2,
01666                                                            pcl::PointCloud<pcl::PointXYZ> & inliers1,
01667                                                            pcl::PointCloud<pcl::PointXYZ> & inliers2,
01668                                                            char depthAxis)
01669 {
01670         for(std::list<std::pair<cv::Point2f, cv::Point2f> >::const_iterator iter = correspondences.begin();
01671                 iter!=correspondences.end();
01672                 ++iter)
01673         {
01674                 PointT pt1 = cloud1.at(int(iter->first.y+0.5f) * cloud1.width + int(iter->first.x+0.5f));
01675                 PointT pt2 = cloud2.at(int(iter->second.y+0.5f) * cloud2.width + int(iter->second.x+0.5f));
01676                 if(pcl::isFinite(pt1) &&
01677                    pcl::isFinite(pt2))
01678                 {
01679                         inliers1.push_back(pcl::PointXYZ(pt1.x, pt1.y, pt1.z));
01680                         inliers2.push_back(pcl::PointXYZ(pt2.x, pt2.y, pt2.z));
01681                 }
01682         }
01683 }
01684 
01685 void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
01686                                                            const pcl::PointCloud<pcl::PointXYZ> & cloud1,
01687                                                            const pcl::PointCloud<pcl::PointXYZ> & cloud2,
01688                                                            pcl::PointCloud<pcl::PointXYZ> & inliers1,
01689                                                            pcl::PointCloud<pcl::PointXYZ> & inliers2,
01690                                                            char depthAxis)
01691 {
01692         extractXYZCorrespondencesImpl(correspondences, cloud1, cloud2, inliers1, inliers2, depthAxis);
01693 }
01694 void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
01695                                                            const pcl::PointCloud<pcl::PointXYZRGB> & cloud1,
01696                                                            const pcl::PointCloud<pcl::PointXYZRGB> & cloud2,
01697                                                            pcl::PointCloud<pcl::PointXYZ> & inliers1,
01698                                                            pcl::PointCloud<pcl::PointXYZ> & inliers2,
01699                                                            char depthAxis)
01700 {
01701         extractXYZCorrespondencesImpl(correspondences, cloud1, cloud2, inliers1, inliers2, depthAxis);
01702 }
01703 
01704 int countUniquePairs(const std::multimap<int, pcl::PointXYZ> & wordsA,
01705                                          const std::multimap<int, pcl::PointXYZ> & wordsB)
01706 {
01707         const std::list<int> & ids = uUniqueKeys(wordsA);
01708         int pairs = 0;
01709         for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
01710         {
01711                 std::list<pcl::PointXYZ> ptsA = uValues(wordsA, *i);
01712                 std::list<pcl::PointXYZ> ptsB = uValues(wordsB, *i);
01713                 if(ptsA.size() == 1 && ptsB.size() == 1)
01714                 {
01715                         ++pairs;
01716                 }
01717         }
01718         return pairs;
01719 }
01720 
01721 void filterMaxDepth(pcl::PointCloud<pcl::PointXYZ> & inliers1,
01722                                         pcl::PointCloud<pcl::PointXYZ> & inliers2,
01723                                         float maxDepth,
01724                                         char depthAxis,
01725                                         bool removeDuplicates)
01726 {
01727         std::list<pcl::PointXYZ> addedPts;
01728         if(maxDepth > 0.0f && inliers1.size() && inliers1.size() == inliers2.size())
01729         {
01730                 pcl::PointCloud<pcl::PointXYZ> tmpInliers1;
01731                 pcl::PointCloud<pcl::PointXYZ> tmpInliers2;
01732                 for(unsigned int i=0; i<inliers1.size(); ++i)
01733                 {
01734                         if((depthAxis == 'x' && inliers1.at(i).x < maxDepth && inliers2.at(i).x < maxDepth) ||
01735                            (depthAxis == 'y' && inliers1.at(i).y < maxDepth && inliers2.at(i).y < maxDepth) ||
01736                            (depthAxis == 'z' && inliers1.at(i).z < maxDepth && inliers2.at(i).z < maxDepth))
01737                         {
01738                                 bool dup = false;
01739                                 if(removeDuplicates)
01740                                 {
01741                                         for(std::list<pcl::PointXYZ>::iterator iter = addedPts.begin(); iter!=addedPts.end(); ++iter)
01742                                         {
01743                                                 if(iter->x == inliers1.at(i).x &&
01744                                                    iter->y == inliers1.at(i).y &&
01745                                                    iter->z == inliers1.at(i).z)
01746                                                 {
01747                                                         dup = true;
01748                                                 }
01749                                         }
01750                                         if(!dup)
01751                                         {
01752                                                 addedPts.push_back(inliers1.at(i));
01753                                         }
01754                                 }
01755 
01756                                 if(!dup)
01757                                 {
01758                                         tmpInliers1.push_back(inliers1.at(i));
01759                                         tmpInliers2.push_back(inliers2.at(i));
01760                                 }
01761                         }
01762                 }
01763                 inliers1 = tmpInliers1;
01764                 inliers2 = tmpInliers2;
01765         }
01766 }
01767 
01768 // Get transform from cloud2 to cloud1
01769 Transform transformFromXYZCorrespondences(
01770                 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud1,
01771                 const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
01772                 double inlierThreshold,
01773                 int iterations,
01774                 bool refineModel,
01775                 double refineModelSigma,
01776                 int refineModelIterations,
01777                 std::vector<int> * inliersOut,
01778                 double * varianceOut)
01779 {
01780         //NOTE: this method is a mix of two methods:
01781         //  - getRemainingCorrespondences() in pcl/registration/impl/correspondence_rejection_sample_consensus.hpp
01782         //  - refineModel() in pcl/sample_consensus/sac.h
01783 
01784         if(varianceOut)
01785         {
01786                 *varianceOut = 1.0;
01787         }
01788         Transform transform;
01789         if(cloud1->size() >=3 && cloud1->size() == cloud2->size())
01790         {
01791                 // RANSAC
01792                 UDEBUG("iterations=%d inlierThreshold=%f", iterations, inlierThreshold);
01793                 std::vector<int> source_indices (cloud2->size());
01794                 std::vector<int> target_indices (cloud1->size());
01795 
01796                 // Copy the query-match indices
01797                 for (int i = 0; i < (int)cloud1->size(); ++i)
01798                 {
01799                         source_indices[i] = i;
01800                         target_indices[i] = i;
01801                 }
01802 
01803                 // From the set of correspondences found, attempt to remove outliers
01804                 // Create the registration model
01805                 pcl::SampleConsensusModelRegistration<pcl::PointXYZ>::Ptr model;
01806                 model.reset(new pcl::SampleConsensusModelRegistration<pcl::PointXYZ>(cloud2, source_indices));
01807                 // Pass the target_indices
01808                 model->setInputTarget (cloud1, target_indices);
01809                 // Create a RANSAC model
01810                 pcl::RandomSampleConsensus<pcl::PointXYZ> sac (model, inlierThreshold);
01811                 sac.setMaxIterations(iterations);
01812 
01813                 // Compute the set of inliers
01814                 if(sac.computeModel())
01815                 {
01816                         std::vector<int> inliers;
01817                         Eigen::VectorXf model_coefficients;
01818 
01819                         sac.getInliers(inliers);
01820                         sac.getModelCoefficients (model_coefficients);
01821 
01822                         if (refineModel)
01823                         {
01824                                 double inlier_distance_threshold_sqr = inlierThreshold * inlierThreshold;
01825                                 double error_threshold = inlierThreshold;
01826                                 double sigma_sqr = refineModelSigma * refineModelSigma;
01827                                 int refine_iterations = 0;
01828                                 bool inlier_changed = false, oscillating = false;
01829                                 std::vector<int> new_inliers, prev_inliers = inliers;
01830                                 std::vector<size_t> inliers_sizes;
01831                                 Eigen::VectorXf new_model_coefficients = model_coefficients;
01832                                 do
01833                                 {
01834                                         // Optimize the model coefficients
01835                                         model->optimizeModelCoefficients (prev_inliers, new_model_coefficients, new_model_coefficients);
01836                                         inliers_sizes.push_back (prev_inliers.size ());
01837 
01838                                         // Select the new inliers based on the optimized coefficients and new threshold
01839                                         model->selectWithinDistance (new_model_coefficients, error_threshold, new_inliers);
01840                                         UDEBUG("RANSAC refineModel: Number of inliers found (before/after): %d/%d, with an error threshold of %f.",
01841                                                         (int)prev_inliers.size (), (int)new_inliers.size (), error_threshold);
01842 
01843                                         if (new_inliers.empty ())
01844                                         {
01845                                                 ++refine_iterations;
01846                                                 if (refine_iterations >= refineModelIterations)
01847                                                 {
01848                                                         break;
01849                                                 }
01850                                                 continue;
01851                                         }
01852 
01853                                         // Estimate the variance and the new threshold
01854                                         double variance = model->computeVariance ();
01855                                         error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
01856 
01857                                         UDEBUG ("RANSAC refineModel: New estimated error threshold: %f (variance=%f) on iteration %d out of %d.",
01858                                                   error_threshold, variance, refine_iterations, refineModelIterations);
01859                                         inlier_changed = false;
01860                                         std::swap (prev_inliers, new_inliers);
01861 
01862                                         // If the number of inliers changed, then we are still optimizing
01863                                         if (new_inliers.size () != prev_inliers.size ())
01864                                         {
01865                                                 // Check if the number of inliers is oscillating in between two values
01866                                                 if (inliers_sizes.size () >= 4)
01867                                                 {
01868                                                         if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] &&
01869                                                         inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4])
01870                                                         {
01871                                                                 oscillating = true;
01872                                                                 break;
01873                                                         }
01874                                                 }
01875                                                 inlier_changed = true;
01876                                                 continue;
01877                                         }
01878 
01879                                         // Check the values of the inlier set
01880                                         for (size_t i = 0; i < prev_inliers.size (); ++i)
01881                                         {
01882                                                 // If the value of the inliers changed, then we are still optimizing
01883                                                 if (prev_inliers[i] != new_inliers[i])
01884                                                 {
01885                                                         inlier_changed = true;
01886                                                         break;
01887                                                 }
01888                                         }
01889                                 }
01890                                 while (inlier_changed && ++refine_iterations < refineModelIterations);
01891 
01892                                 // If the new set of inliers is empty, we didn't do a good job refining
01893                                 if (new_inliers.empty ())
01894                                 {
01895                                         UWARN ("RANSAC refineModel: Refinement failed: got an empty set of inliers!");
01896                                 }
01897 
01898                                 if (oscillating)
01899                                 {
01900                                         UDEBUG("RANSAC refineModel: Detected oscillations in the model refinement.");
01901                                 }
01902 
01903                                 std::swap (inliers, new_inliers);
01904                                 model_coefficients = new_model_coefficients;
01905                         }
01906 
01907                         if (inliers.size() >= 3)
01908                         {
01909                                 if(inliersOut)
01910                                 {
01911                                         *inliersOut = inliers;
01912                                 }
01913                                 if(varianceOut)
01914                                 {
01915                                         *varianceOut = model->computeVariance();
01916                                 }
01917 
01918                                 // get best transformation
01919                                 Eigen::Matrix4f bestTransformation;
01920                                 bestTransformation.row (0) = model_coefficients.segment<4>(0);
01921                                 bestTransformation.row (1) = model_coefficients.segment<4>(4);
01922                                 bestTransformation.row (2) = model_coefficients.segment<4>(8);
01923                                 bestTransformation.row (3) = model_coefficients.segment<4>(12);
01924 
01925                                 transform = Transform::fromEigen4f(bestTransformation);
01926                                 UDEBUG("RANSAC inliers=%d/%d tf=%s", (int)inliers.size(), (int)cloud1->size(), transform.prettyPrint().c_str());
01927 
01928                                 return transform.inverse(); // inverse to get actual pose transform (not correspondences transform)
01929                         }
01930                         else
01931                         {
01932                                 UDEBUG("RANSAC: Model with inliers < 3");
01933                         }
01934                 }
01935                 else
01936                 {
01937                         UDEBUG("RANSAC: Failed to find model");
01938                 }
01939         }
01940         else
01941         {
01942                 UDEBUG("Not enough points to compute the transform");
01943         }
01944         return Transform();
01945 }
01946 
01947 // return transform from source to target (All points must be finite!!!)
01948 Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
01949                           const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
01950                           double maxCorrespondenceDistance,
01951                           int maximumIterations,
01952                           bool * hasConvergedOut,
01953                           double * variance,
01954                           int * correspondencesOut)
01955 {
01956         pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
01957         // Set the input source and target
01958         icp.setInputTarget (cloud_target);
01959         icp.setInputSource (cloud_source);
01960 
01961         // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
01962         icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
01963         // Set the maximum number of iterations (criterion 1)
01964         icp.setMaximumIterations (maximumIterations);
01965         // Set the transformation epsilon (criterion 2)
01966         //icp.setTransformationEpsilon (transformationEpsilon);
01967         // Set the euclidean distance difference epsilon (criterion 3)
01968         //icp.setEuclideanFitnessEpsilon (1);
01969         //icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
01970 
01971         // Perform the alignment
01972         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointXYZ>);
01973         icp.align (*cloud_source_registered);
01974         bool hasConverged = icp.hasConverged();
01975 
01976         // compute variance
01977         if((correspondencesOut || variance) && hasConverged)
01978         {
01979                 pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
01980                 est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
01981                 est->setInputTarget(cloud_target);
01982                 est->setInputSource(cloud_source_registered);
01983                 pcl::Correspondences correspondences;
01984                 est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
01985                 if(variance)
01986                 {
01987                         if(correspondences.size()>=3)
01988                         {
01989                                 std::vector<double> distances(correspondences.size());
01990                                 for(unsigned int i=0; i<correspondences.size(); ++i)
01991                                 {
01992                                         distances[i] = correspondences[i].distance;
01993                                 }
01994 
01995                                 //variance
01996                                 std::sort(distances.begin (), distances.end ());
01997                                 double median_error_sqr = distances[distances.size () >> 1];
01998                                 *variance = (2.1981 * median_error_sqr);
01999                         }
02000                         else
02001                         {
02002                                 hasConverged = false;
02003                                 *variance = -1.0;
02004                         }
02005                 }
02006 
02007                 if(correspondencesOut)
02008                 {
02009                         *correspondencesOut = (int)correspondences.size();
02010                 }
02011         }
02012         else
02013         {
02014                 if(correspondencesOut)
02015                 {
02016                         *correspondencesOut = 0;
02017                 }
02018                 if(variance)
02019                 {
02020                         *variance = -1;
02021                 }
02022         }
02023 
02024         if(hasConvergedOut)
02025         {
02026                 *hasConvergedOut = hasConverged;
02027         }
02028 
02029         return Transform::fromEigen4f(icp.getFinalTransformation());
02030 }
02031 
02032 // return transform from source to target (All points/normals must be finite!!!)
02033 Transform icpPointToPlane(
02034                 const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
02035                 const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
02036                 double maxCorrespondenceDistance,
02037                 int maximumIterations,
02038                 bool * hasConvergedOut,
02039                 double * variance,
02040                 int * correspondencesOut)
02041 {
02042         pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
02043         // Set the input source and target
02044         icp.setInputTarget (cloud_target);
02045         icp.setInputSource (cloud_source);
02046 
02047         pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>::Ptr est;
02048         est.reset(new pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>);
02049         icp.setTransformationEstimation(est);
02050 
02051         // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
02052         icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
02053         // Set the maximum number of iterations (criterion 1)
02054         icp.setMaximumIterations (maximumIterations);
02055         // Set the transformation epsilon (criterion 2)
02056         //icp.setTransformationEpsilon (transformationEpsilon);
02057         // Set the euclidean distance difference epsilon (criterion 3)
02058         //icp.setEuclideanFitnessEpsilon (1);
02059         //icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
02060 
02061         // Perform the alignment
02062         pcl::PointCloud<pcl::PointNormal>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointNormal>);
02063         icp.align (*cloud_source_registered);
02064         bool hasConverged = icp.hasConverged();
02065 
02066         // compute variance
02067         if((correspondencesOut || variance) && hasConverged)
02068         {
02069                 pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
02070                 est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
02071                 est->setInputTarget(cloud_target);
02072                 est->setInputSource(cloud_source_registered);
02073                 pcl::Correspondences correspondences;
02074                 est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
02075                 if(variance)
02076                 {
02077                         if(correspondences.size()>=3)
02078                         {
02079                                 std::vector<double> distances(correspondences.size());
02080                                 for(unsigned int i=0; i<correspondences.size(); ++i)
02081                                 {
02082                                         distances[i] = correspondences[i].distance;
02083                                 }
02084 
02085                                 //variance
02086                                 std::sort(distances.begin (), distances.end ());
02087                                 double median_error_sqr = distances[distances.size () >> 1];
02088                                 *variance = (2.1981 * median_error_sqr);
02089                         }
02090                         else
02091                         {
02092                                 hasConverged = false;
02093                                 *variance = -1.0;
02094                         }
02095                 }
02096 
02097                 if(correspondencesOut)
02098                 {
02099                         *correspondencesOut = (int)correspondences.size();
02100                 }
02101         }
02102         else
02103         {
02104                 if(correspondencesOut)
02105                 {
02106                         *correspondencesOut = 0;
02107                 }
02108                 if(variance)
02109                 {
02110                         *variance = -1;
02111                 }
02112         }
02113 
02114         if(hasConvergedOut)
02115         {
02116                 *hasConvergedOut = hasConverged;
02117         }
02118 
02119         return Transform::fromEigen4f(icp.getFinalTransformation());
02120 }
02121 
02122 // return transform from source to target (All points must be finite!!!)
02123 Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
02124                           const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
02125                           double maxCorrespondenceDistance,
02126                           int maximumIterations,
02127                           bool * hasConvergedOut,
02128                           double * variance,
02129                           int * correspondencesOut)
02130 {
02131         pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
02132         // Set the input source and target
02133         icp.setInputTarget (cloud_target);
02134         icp.setInputSource (cloud_source);
02135 
02136         pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
02137         est.reset(new pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>);
02138         icp.setTransformationEstimation(est);
02139 
02140         // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
02141         icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
02142         // Set the maximum number of iterations (criterion 1)
02143         icp.setMaximumIterations (maximumIterations);
02144         // Set the transformation epsilon (criterion 2)
02145         //icp.setTransformationEpsilon (transformationEpsilon);
02146         // Set the euclidean distance difference epsilon (criterion 3)
02147         //icp.setEuclideanFitnessEpsilon (1);
02148         //icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
02149 
02150         // Perform the alignment
02151         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source_registered(new pcl::PointCloud<pcl::PointXYZ>);
02152         icp.align (*cloud_source_registered);
02153         bool hasConverged = icp.hasConverged();
02154 
02155         // compute variance
02156         if((correspondencesOut || variance) && hasConverged)
02157         {
02158                 pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
02159                 est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
02160                 est->setInputTarget(cloud_target);
02161                 est->setInputSource(cloud_source_registered);
02162                 pcl::Correspondences correspondences;
02163                 est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
02164                 if(variance)
02165                 {
02166                         if(correspondences.size()>=3)
02167                         {
02168                                 std::vector<double> distances(correspondences.size());
02169                                 for(unsigned int i=0; i<correspondences.size(); ++i)
02170                                 {
02171                                         distances[i] = correspondences[i].distance;
02172                                 }
02173 
02174                                 //variance
02175                                 std::sort(distances.begin (), distances.end ());
02176                                 double median_error_sqr = distances[distances.size () >> 1];
02177                                 *variance = (2.1981 * median_error_sqr);
02178                         }
02179                         else
02180                         {
02181                                 hasConverged = false;
02182                                 *variance = -1.0;
02183                         }
02184                 }
02185 
02186                 if(correspondencesOut)
02187                 {
02188                         *correspondencesOut = (int)correspondences.size();
02189                 }
02190         }
02191         else
02192         {
02193                 if(correspondencesOut)
02194                 {
02195                         *correspondencesOut = 0;
02196                 }
02197                 if(variance)
02198                 {
02199                         *variance = -1;
02200                 }
02201         }
02202 
02203         if(hasConvergedOut)
02204         {
02205                 *hasConvergedOut = hasConverged;
02206         }
02207 
02208         return Transform::fromEigen4f(icp.getFinalTransformation());
02209 }
02210 
02211 pcl::PointCloud<pcl::PointNormal>::Ptr computeNormals(
02212                 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
02213                 int normalKSearch)
02214 {
02215         pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
02216         pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
02217         tree->setInputCloud (cloud);
02218 
02219         // Normal estimation*
02220         pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
02221         pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
02222         n.setInputCloud (cloud);
02223         n.setSearchMethod (tree);
02224         n.setKSearch (normalKSearch);
02225         n.compute (*normals);
02226         //* normals should not contain the point normals + surface curvatures
02227 
02228         // Concatenate the XYZ and normal fields*
02229         pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
02230         //* cloud_with_normals = cloud + normals*/
02231 
02232         return cloud_with_normals;
02233 }
02234 
02235 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormals(
02236                 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
02237                 int normalKSearch)
02238 {
02239         pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
02240         pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
02241         tree->setInputCloud (cloud);
02242 
02243         // Normal estimation*
02244         pcl::NormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal> n;
02245         pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
02246         n.setInputCloud (cloud);
02247         n.setSearchMethod (tree);
02248         n.setKSearch (normalKSearch);
02249         n.compute (*normals);
02250         //* normals should not contain the point normals + surface curvatures
02251 
02252         // Concatenate the XYZ and normal fields*
02253         pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
02254         //* cloud_with_normals = cloud + normals*/
02255 
02256         return cloud_with_normals;
02257 }
02258 
02259 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormalsSmoothed(
02260                 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
02261                 float smoothingSearchRadius,
02262                 bool smoothingPolynomialFit)
02263 {
02264         pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
02265         pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
02266         tree->setInputCloud (cloud);
02267 
02268         // Init object (second point type is for the normals, even if unused)
02269         pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> mls;
02270 
02271         mls.setComputeNormals (true);
02272 
02273         // Set parameters
02274         mls.setInputCloud (cloud);
02275         mls.setPolynomialFit (smoothingPolynomialFit);
02276         mls.setSearchMethod (tree);
02277         mls.setSearchRadius (smoothingSearchRadius);
02278 
02279         // Reconstruct
02280         mls.process (*cloud_with_normals);
02281 
02282         return cloud_with_normals;
02283 }
02284 
02285 // a kdtree is constructed with cloud_target, then nearest neighbor
02286 // is computed for each cloud_source points.
02287 int getCorrespondencesCount(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
02288                                                         const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
02289                                                         float maxDistance)
02290 {
02291         pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
02292         kdTree->setInputCloud(cloud_target);
02293         int count = 0;
02294         float sqrdMaxDistance = maxDistance * maxDistance;
02295         for(unsigned int i=0; i<cloud_source->size(); ++i)
02296         {
02297                 std::vector<int> ind(1);
02298                 std::vector<float> dist(1);
02299                 if(kdTree->nearestKSearch(cloud_source->at(i), 1, ind, dist) && dist[0] < sqrdMaxDistance)
02300                 {
02301                         ++count;
02302                 }
02303         }
02304         return count;
02305 }
02306 
02311 void findCorrespondences(
02312                 const std::multimap<int, cv::KeyPoint> & wordsA,
02313                 const std::multimap<int, cv::KeyPoint> & wordsB,
02314                 std::list<std::pair<cv::Point2f, cv::Point2f> > & pairs)
02315 {
02316         const std::list<int> & ids = uUniqueKeys(wordsA);
02317         pairs.clear();
02318         for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
02319         {
02320                 std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
02321                 std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
02322                 if(ptsA.size() == 1 && ptsB.size() == 1)
02323                 {
02324                         pairs.push_back(std::pair<cv::Point2f, cv::Point2f>(ptsA.front().pt, ptsB.front().pt));
02325                 }
02326         }
02327 }
02328 
02329 pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
02330                 const cv::Mat & matrix,
02331                 const Transform & tranform)
02332 {
02333         UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3);
02334         UASSERT(matrix.rows == 1);
02335 
02336         Eigen::Affine3f t = tranform.toEigen3f();
02337         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
02338         cloud->resize(matrix.cols);
02339         if(matrix.channels() == 2)
02340         {
02341                 for(int i=0; i<matrix.cols; ++i)
02342                 {
02343                         cloud->at(i).x = matrix.at<cv::Vec2f>(0,i)[0];
02344                         cloud->at(i).y = matrix.at<cv::Vec2f>(0,i)[1];
02345                         cloud->at(i).z = 0.0f;
02346                         cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
02347                 }
02348         }
02349         else // channels=3
02350         {
02351                 for(int i=0; i<matrix.cols; ++i)
02352                 {
02353                         cloud->at(i).x = matrix.at<cv::Vec3f>(0,i)[0];
02354                         cloud->at(i).y = matrix.at<cv::Vec3f>(0,i)[1];
02355                         cloud->at(i).z = matrix.at<cv::Vec3f>(0,i)[2];
02356                         cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
02357                 }
02358         }
02359         return cloud;
02360 }
02361 
02362 // If "voxel" > 0, "samples" is ignored
02363 pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
02364                 const cv::Mat & depth,
02365                 float fx,
02366                 float fy,
02367                 float cx,
02368                 float cy,
02369                 int decimation,
02370                 double maxDepth,
02371                 float voxel,
02372                 int samples,
02373                 const Transform & transform)
02374 {
02375         UASSERT(!depth.empty() && (depth.type() == CV_16UC1 || depth.type() == CV_32FC1));
02376         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
02377         cloud = cloudFromDepth(
02378                         depth,
02379                         cx,
02380                         cy,
02381                         fx,
02382                         fy,
02383                         decimation);
02384 
02385         if(cloud->size())
02386         {
02387                 if(maxDepth>0.0)
02388                 {
02389                         cloud = passThrough<pcl::PointXYZ>(cloud, "z", 0, maxDepth);
02390                 }
02391 
02392                 if(cloud->size())
02393                 {
02394                         if(voxel>0)
02395                         {
02396                                 cloud = voxelize<pcl::PointXYZ>(cloud, voxel);
02397                         }
02398                         else if(samples>0 && (int)cloud->size() > samples)
02399                         {
02400                                 cloud = sampling<pcl::PointXYZ>(cloud, samples);
02401                         }
02402 
02403                         if(cloud->size())
02404                         {
02405                                 if(!transform.isNull() && !transform.isIdentity())
02406                                 {
02407                                         cloud = transformPointCloud<pcl::PointXYZ>(cloud, transform);
02408                                 }
02409                         }
02410                 }
02411         }
02412 
02413         return cloud;
02414 }
02415 
02416 pcl::PointCloud<pcl::PointXYZ>::Ptr concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds)
02417 {
02418         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
02419         for(std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr>::const_iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
02420         {
02421                 *cloud += *(*iter);
02422         }
02423         return cloud;
02424 }
02425 
02426 pcl::PointCloud<pcl::PointXYZRGB>::Ptr concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds)
02427 {
02428         pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
02429         for(std::list<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::const_iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
02430         {
02431                 *cloud+=*(*iter);
02432         }
02433         return cloud;
02434 }
02435 
02436 pcl::PointCloud<pcl::PointXYZ>::Ptr get3DFASTKpts(
02437                 const cv::Mat & image,
02438                 const cv::Mat & imageDepth,
02439                 float constant,
02440                 int fastThreshold,
02441                 bool fastNonmaxSuppression,
02442                 float maxDepth)
02443 {
02444         // Extract words
02445         cv::FastFeatureDetector detector(fastThreshold, fastNonmaxSuppression);
02446         std::vector<cv::KeyPoint> kpts;
02447         detector.detect(image, kpts);
02448 
02449         pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);
02450         for(unsigned int i=0; i<kpts.size(); ++i)
02451         {
02452                 pcl::PointXYZ pt = projectDepthTo3D(imageDepth, kpts[i].pt.x, kpts[i].pt.y, 0, 0, 1.0f/constant, 1.0f/constant, true);
02453                 if(uIsFinite(pt.z) && (maxDepth <= 0 || pt.z <= maxDepth))
02454                 {
02455                         points->push_back(pt);
02456                 }
02457         }
02458         UDEBUG("points %d -> %d", (int)kpts.size(), (int)points->size());
02459         return points;
02460 }
02461 
02462 pcl::PolygonMesh::Ptr createMesh(
02463                 const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloudWithNormals,
02464                 float gp3SearchRadius,
02465                 float gp3Mu,
02466                 int gp3MaximumNearestNeighbors,
02467                 float gp3MaximumSurfaceAngle,
02468                 float gp3MinimumAngle,
02469                 float gp3MaximumAngle,
02470                 bool gp3NormalConsistency)
02471 {
02472         pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormalsNoNaN = removeNaNNormalsFromPointCloud<pcl::PointXYZRGBNormal>(cloudWithNormals);
02473 
02474         // Create search tree*
02475         pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
02476         tree2->setInputCloud (cloudWithNormalsNoNaN);
02477 
02478         // Initialize objects
02479         pcl::GreedyProjectionTriangulation<pcl::PointXYZRGBNormal> gp3;
02480         pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
02481 
02482         // Set the maximum distance between connected points (maximum edge length)
02483         gp3.setSearchRadius (gp3SearchRadius);
02484 
02485         // Set typical values for the parameters
02486         gp3.setMu (gp3Mu);
02487         gp3.setMaximumNearestNeighbors (gp3MaximumNearestNeighbors);
02488         gp3.setMaximumSurfaceAngle(gp3MaximumSurfaceAngle); // 45 degrees
02489         gp3.setMinimumAngle(gp3MinimumAngle); // 10 degrees
02490         gp3.setMaximumAngle(gp3MaximumAngle); // 120 degrees
02491         gp3.setNormalConsistency(gp3NormalConsistency);
02492 
02493         // Get result
02494         gp3.setInputCloud (cloudWithNormalsNoNaN);
02495         gp3.setSearchMethod (tree2);
02496         gp3.reconstruct (*mesh);
02497 
02498         return mesh;
02499 }
02500 
02501 void occupancy2DFromLaserScan(
02502                 const cv::Mat & scan,
02503                 cv::Mat & ground,
02504                 cv::Mat & obstacles,
02505                 float cellSize)
02506 {
02507         if(scan.empty())
02508         {
02509                 return;
02510         }
02511 
02512         std::map<int, Transform> poses;
02513         poses.insert(std::make_pair(1, Transform::getIdentity()));
02514 
02515         pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud = util3d::laserScanToPointCloud(scan);
02516         //obstaclesCloud = util3d::voxelize<pcl::PointXYZ>(obstaclesCloud, cellSize);
02517 
02518         std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr> scans;
02519         scans.insert(std::make_pair(1, obstaclesCloud));
02520 
02521         float xMin, yMin;
02522         cv::Mat map8S = create2DMap(poses, scans, cellSize, false, xMin, yMin);
02523 
02524         // find ground cells
02525         std::list<int> groundIndices;
02526         for(unsigned int i=0; i< map8S.total(); ++i)
02527         {
02528                 if(map8S.data[i] == 0)
02529                 {
02530                         groundIndices.push_back(i);
02531                 }
02532         }
02533 
02534         // Convert to position matrices, get points to each center of the cells
02535         ground = cv::Mat();
02536         if(groundIndices.size())
02537         {
02538                 ground = cv::Mat((int)groundIndices.size(), 1, CV_32FC2);
02539                 int i=0;
02540                 for(std::list<int>::iterator iter=groundIndices.begin();iter!=groundIndices.end(); ++iter)
02541                 {
02542                         int x = *iter / map8S.cols;
02543                         int y = *iter - x*map8S.cols;
02544                         ground.at<cv::Vec2f>(i)[0] = (float(y)+0.5)*cellSize + xMin;
02545                         ground.at<cv::Vec2f>(i)[1] = (float(x)+0.5)*cellSize + yMin;
02546                         ++i;
02547                 }
02548         }
02549 
02550         // copy directly obstacles precise positions
02551         obstacles = cv::Mat();
02552         if(obstaclesCloud->size())
02553         {
02554                 obstacles = cv::Mat((int)obstaclesCloud->size(), 1, CV_32FC2);
02555                 for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
02556                 {
02557                         obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x;
02558                         obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloud->at(i).y;
02559                 }
02560         }
02561 }
02562 
02576 cv::Mat create2DMapFromOccupancyLocalMaps(
02577                 const std::map<int, Transform> & poses,
02578                 const std::map<int, std::pair<cv::Mat, cv::Mat> > & occupancy,
02579                 float cellSize,
02580                 float & xMin,
02581                 float & yMin,
02582                 float minMapSize,
02583                 bool erode)
02584 {
02585         UASSERT(minMapSize >= 0.0f);
02586         UDEBUG("cellSize=%f m, minMapSize=%f m, erode=%d", cellSize, minMapSize, erode?1:0);
02587         UTimer timer;
02588 
02589         std::map<int, cv::Mat> emptyLocalMaps;
02590         std::map<int, cv::Mat> occupiedLocalMaps;
02591 
02592         float minX=-minMapSize/2.0, minY=-minMapSize/2.0, maxX=minMapSize/2.0, maxY=minMapSize/2.0;
02593         bool undefinedSize = minMapSize == 0.0f;
02594         float x=0.0f,y=0.0f,z=0.0f,roll=0.0f,pitch=0.0f,yaw=0.0f,cosT=0.0f,sinT=0.0f;
02595         cv::Mat affineTransform(2,3,CV_32FC1);
02596         for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
02597         {
02598                 UASSERT(!iter->second.isNull());
02599 
02600                 iter->second.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
02601 
02602                 if(undefinedSize)
02603                 {
02604                         minX = maxX = x;
02605                         minY = maxY = y;
02606                         undefinedSize = false;
02607                 }
02608                 else
02609                 {
02610                         if(minX > x)
02611                                 minX = x;
02612                         else if(maxX < x)
02613                                 maxX = x;
02614 
02615                         if(minY > y)
02616                                 minY = y;
02617                         else if(maxY < y)
02618                                 maxY = y;
02619                 }
02620 
02621                 if(uContains(occupancy, iter->first))
02622                 {
02623                         const std::pair<cv::Mat, cv::Mat> & pair = occupancy.at(iter->first);
02624                         cosT = cos(yaw);
02625                         sinT = sin(yaw);
02626                         affineTransform.at<float>(0,0) = cosT;
02627                         affineTransform.at<float>(0,1) = -sinT;
02628                         affineTransform.at<float>(1,0) = sinT;
02629                         affineTransform.at<float>(1,1) = cosT;
02630                         affineTransform.at<float>(0,2) = x;
02631                         affineTransform.at<float>(1,2) = y;
02632 
02633                         //ground
02634                         if(pair.first.rows)
02635                         {
02636                                 UASSERT(pair.first.type() == CV_32FC2);
02637                                 cv::Mat ground(pair.first.rows, pair.first.cols, pair.first.type());
02638                                 cv::transform(pair.first, ground, affineTransform);
02639                                 for(int i=0; i<ground.rows; ++i)
02640                                 {
02641                                         if(minX > ground.at<float>(i,0))
02642                                                 minX = ground.at<float>(i,0);
02643                                         else if(maxX < ground.at<float>(i,0))
02644                                                 maxX = ground.at<float>(i,0);
02645 
02646                                         if(minY > ground.at<float>(i,1))
02647                                                 minY = ground.at<float>(i,1);
02648                                         else if(maxY < ground.at<float>(i,1))
02649                                                 maxY = ground.at<float>(i,1);
02650                                 }
02651                                 emptyLocalMaps.insert(std::make_pair(iter->first, ground));
02652                         }
02653 
02654                         //obstacles
02655                         if(pair.second.rows)
02656                         {
02657                                 UASSERT(pair.second.type() == CV_32FC2);
02658                                 cv::Mat obstacles(pair.second.rows, pair.second.cols, pair.second.type());
02659                                 cv::transform(pair.second, obstacles, affineTransform);
02660                                 for(int i=0; i<obstacles.rows; ++i)
02661                                 {
02662                                         if(minX > obstacles.at<float>(i,0))
02663                                                 minX = obstacles.at<float>(i,0);
02664                                         else if(maxX < obstacles.at<float>(i,0))
02665                                                 maxX = obstacles.at<float>(i,0);
02666 
02667                                         if(minY > obstacles.at<float>(i,1))
02668                                                 minY = obstacles.at<float>(i,1);
02669                                         else if(maxY < obstacles.at<float>(i,1))
02670                                                 maxY = obstacles.at<float>(i,1);
02671                                 }
02672                                 occupiedLocalMaps.insert(std::make_pair(iter->first, obstacles));
02673                         }
02674                 }
02675         }
02676         UDEBUG("timer=%fs", timer.ticks());
02677 
02678         cv::Mat map;
02679         if(minX != maxX && minY != maxY)
02680         {
02681                 //Get map size
02682                 float margin = cellSize*10.0f;
02683                 xMin = minX-margin;
02684                 yMin = minY-margin;
02685                 float xMax = maxX+margin;
02686                 float yMax = maxY+margin;
02687                 if(fabs((yMax - yMin) / cellSize) > 99999 ||
02688                    fabs((xMax - xMin) / cellSize) > 99999)
02689                 {
02690                         UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
02691                                         "There's maybe an error with the poses provided! The map will not be created!",
02692                                         xMin, yMin, xMax, yMax);
02693                 }
02694                 else
02695                 {
02696                         UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax);
02697 
02698 
02699                         map = cv::Mat::ones((yMax - yMin) / cellSize + 0.5f, (xMax - xMin) / cellSize + 0.5f, CV_8S)*-1;
02700                         for(std::map<int, Transform>::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
02701                         {
02702                                 std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
02703                                 std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
02704                                 if(iter!=emptyLocalMaps.end())
02705                                 {
02706                                         for(int i=0; i<iter->second.rows; ++i)
02707                                         {
02708                                                 cv::Point2i pt((iter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (iter->second.at<float>(i,1)-yMin)/cellSize + 0.5f);
02709                                                 map.at<char>(pt.y, pt.x) = 0; // free space
02710                                         }
02711                                 }
02712                                 if(jter!=occupiedLocalMaps.end())
02713                                 {
02714                                         for(int i=0; i<jter->second.rows; ++i)
02715                                         {
02716                                                 cv::Point2i pt((jter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (jter->second.at<float>(i,1)-yMin)/cellSize + 0.5f);
02717                                                 map.at<char>(pt.y, pt.x) = 100; // obstacles
02718                                         }
02719                                 }
02720 
02721                                 //UDEBUG("empty=%d occupied=%d", empty, occupied);
02722                         }
02723 
02724                         // fill holes and remove empty from obstacle borders
02725                         cv::Mat updatedMap = map.clone();
02726                         std::list<std::pair<int, int> > obstacleIndices;
02727                         for(int i=2; i<map.rows-2; ++i)
02728                         {
02729                                 for(int j=2; j<map.cols-2; ++j)
02730                                 {
02731                                         if(map.at<char>(i, j) == -1 &&
02732                                                 map.at<char>(i+1, j) != -1 &&
02733                                                 map.at<char>(i-1, j) != -1 &&
02734                                                 map.at<char>(i, j+1) != -1 &&
02735                                                 map.at<char>(i, j-1) != -1)
02736                                         {
02737                                                 updatedMap.at<char>(i, j) = 0;
02738                                         }
02739                                         else if(map.at<char>(i, j) == 100)
02740                                         {
02741                                                 // obstacle/empty/unknown -> remove empty
02742                                                 // unknown/empty/obstacle -> remove empty
02743                                                 if(map.at<char>(i-1, j) == 0 &&
02744                                                         map.at<char>(i-2, j) == -1)
02745                                                 {
02746                                                         updatedMap.at<char>(i-1, j) = -1;
02747                                                 }
02748                                                 else if(map.at<char>(i+1, j) == 0 &&
02749                                                                 map.at<char>(i+2, j) == -1)
02750                                                 {
02751                                                         updatedMap.at<char>(i+1, j) = -1;
02752                                                 }
02753                                                 if(map.at<char>(i, j-1) == 0 &&
02754                                                         map.at<char>(i, j-2) == -1)
02755                                                 {
02756                                                         updatedMap.at<char>(i, j-1) = -1;
02757                                                 }
02758                                                 else if(map.at<char>(i, j+1) == 0 &&
02759                                                                 map.at<char>(i, j+2) == -1)
02760                                                 {
02761                                                         updatedMap.at<char>(i, j+1) = -1;
02762                                                 }
02763 
02764                                                 if(erode)
02765                                                 {
02766                                                         obstacleIndices.push_back(std::make_pair(i, j));
02767                                                 }
02768                                         }
02769                                         else if(map.at<char>(i, j) == 0)
02770                                         {
02771                                                 // obstacle/empty/obstacle -> remove empty
02772                                                 if(map.at<char>(i-1, j) == 100 &&
02773                                                         map.at<char>(i+1, j) == 100)
02774                                                 {
02775                                                         updatedMap.at<char>(i, j) = -1;
02776                                                 }
02777                                                 else if(map.at<char>(i, j-1) == 100 &&
02778                                                         map.at<char>(i, j+1) == 100)
02779                                                 {
02780                                                         updatedMap.at<char>(i, j) = -1;
02781                                                 }
02782                                         }
02783 
02784                                 }
02785                         }
02786                         map = updatedMap;
02787 
02788                         if(erode)
02789                         {
02790                                 // remove obstacles which touch to empty cells but not unknown cells
02791                                 cv::Mat erodedMap = map.clone();
02792                                 for(std::list<std::pair<int,int> >::iterator iter = obstacleIndices.begin();
02793                                         iter!= obstacleIndices.end();
02794                                         ++iter)
02795                                 {
02796                                         int i = iter->first;
02797                                         int j = iter->second;
02798                                         bool touchEmpty = map.at<char>(i+1, j) == 0 ||
02799                                                 map.at<char>(i-1, j) == 0 ||
02800                                                 map.at<char>(i, j+1) == 0 ||
02801                                                 map.at<char>(i, j-1) == 0;
02802                                         if(touchEmpty && map.at<char>(i+1, j) != -1 &&
02803                                                 map.at<char>(i-1, j) != -1 &&
02804                                                 map.at<char>(i, j+1) != -1 &&
02805                                                 map.at<char>(i, j-1) != -1)
02806                                         {
02807                                                 erodedMap.at<char>(i, j) = 0; // empty
02808                                         }
02809                                 }
02810                                 map = erodedMap;
02811                         }
02812                 }
02813         }
02814         UDEBUG("timer=%fs", timer.ticks());
02815         return map;
02816 }
02817 
02830 cv::Mat create2DMap(const std::map<int, Transform> & poses,
02831                 const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,
02832                 float cellSize,
02833                 bool unknownSpaceFilled,
02834                 float & xMin,
02835                 float & yMin,
02836                 float minMapSize)
02837 {
02838         UDEBUG("poses=%d, scans = %d", poses.size(), scans.size());
02839         std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > localScans;
02840 
02841         pcl::PointCloud<pcl::PointXYZ> minMax;
02842         if(minMapSize > 0.0f)
02843         {
02844                 minMax.push_back(pcl::PointXYZ(-minMapSize/2.0, -minMapSize/2.0, 0));
02845                 minMax.push_back(pcl::PointXYZ(minMapSize/2.0, minMapSize/2.0, 0));
02846         }
02847         for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
02848         {
02849                 if(uContains(scans, iter->first) && scans.at(iter->first)->size())
02850                 {
02851                         UASSERT(!iter->second.isNull());
02852                         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = transformPointCloud<pcl::PointXYZ>(scans.at(iter->first), iter->second);
02853                         pcl::PointXYZ min, max;
02854                         pcl::getMinMax3D(*cloud, min, max);
02855                         minMax.push_back(min);
02856                         minMax.push_back(max);
02857                         minMax.push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
02858                         localScans.insert(std::make_pair(iter->first, cloud));
02859                 }
02860         }
02861 
02862         cv::Mat map;
02863         if(minMax.size())
02864         {
02865                 //Get map size
02866                 pcl::PointXYZ min, max;
02867                 pcl::getMinMax3D(minMax, min, max);
02868 
02869                 // Added X2 to make sure that all points are inside the map (when rounded to integer)
02870                 float marging = cellSize*10.0f;
02871                 xMin = min.x-marging;
02872                 yMin = min.y-marging;
02873                 float xMax = max.x+marging;
02874                 float yMax = max.y+marging;
02875 
02876                 UDEBUG("map min=(%f, %f) max=(%f,%f)", xMin, yMin, xMax, yMax);
02877 
02878                 UTimer timer;
02879 
02880                 map = cv::Mat::ones((yMax - yMin) / cellSize + 0.5f, (xMax - xMin) / cellSize + 0.5f, CV_8S)*-1;
02881                 std::vector<float> maxSquaredLength(localScans.size(), 0.0f);
02882                 int j=0;
02883                 for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
02884                 {
02885                         const Transform & pose = poses.at(iter->first);
02886                         cv::Point2i start((pose.x()-xMin)/cellSize + 0.5f, (pose.y()-yMin)/cellSize + 0.5f);
02887                         for(unsigned int i=0; i<iter->second->size(); ++i)
02888                         {
02889                                 cv::Point2i end((iter->second->points[i].x-xMin)/cellSize + 0.5f, (iter->second->points[i].y-yMin)/cellSize + 0.5f);
02890                                 map.at<char>(end.y, end.x) = 100; // obstacle
02891                                 rayTrace(start, end, map, true); // trace free space
02892 
02893                                 if(unknownSpaceFilled)
02894                                 {
02895                                         float dx = iter->second->points[i].x - pose.x();
02896                                         float dy = iter->second->points[i].y - pose.y();
02897                                         float l = dx*dx + dy*dy;
02898                                         if(l > maxSquaredLength[j])
02899                                         {
02900                                                 maxSquaredLength[j] = l;
02901                                         }
02902                                 }
02903                         }
02904                         ++j;
02905                 }
02906                 UDEBUG("Ray trace known space=%fs", timer.ticks());
02907 
02908                 // now fill unknown spaces
02909                 if(unknownSpaceFilled)
02910                 {
02911                         j=0;
02912                         for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
02913                         {
02914                                 if(iter->second->size() > 1 && maxSquaredLength[j] > 0.0f)
02915                                 {
02916                                         float maxLength = sqrt(maxSquaredLength[j]);
02917                                         if(maxLength > cellSize)
02918                                         {
02919                                                 // compute angle
02920                                                 float a = (CV_PI/2.0f) /  (maxLength / cellSize);
02921                                                 //UWARN("a=%f PI/256=%f", a, CV_PI/256.0f);
02922                                                 UASSERT_MSG(a >= 0 && a < 5.0f*CV_PI/8.0f, uFormat("a=%f length=%f cell=%f", a, maxLength, cellSize).c_str());
02923 
02924                                                 const Transform & pose = poses.at(iter->first);
02925                                                 cv::Point2i start((pose.x()-xMin)/cellSize + 0.5f, (pose.y()-yMin)/cellSize + 0.5f);
02926 
02927                                                 //UWARN("maxLength = %f", maxLength);
02928                                                 //rotate counterclockwise from the first point until we pass the last point
02929                                                 cv::Mat rotation = (cv::Mat_<float>(2,2) << cos(a), -sin(a),
02930                                                                                                                                          sin(a), cos(a));
02931                                                 cv::Mat origin(2,1,CV_32F), endFirst(2,1,CV_32F), endLast(2,1,CV_32F);
02932                                                 origin.at<float>(0) = pose.x();
02933                                                 origin.at<float>(1) = pose.y();
02934                                                 endFirst.at<float>(0) = iter->second->points[0].x;
02935                                                 endFirst.at<float>(1) = iter->second->points[0].y;
02936                                                 endLast.at<float>(0) = iter->second->points[iter->second->points.size()-1].x;
02937                                                 endLast.at<float>(1) = iter->second->points[iter->second->points.size()-1].y;
02938                                                 //UWARN("origin = %f %f", origin.at<float>(0), origin.at<float>(1));
02939                                                 //UWARN("endFirst = %f %f", endFirst.at<float>(0), endFirst.at<float>(1));
02940                                                 //UWARN("endLast = %f %f", endLast.at<float>(0), endLast.at<float>(1));
02941                                                 cv::Mat tmp = (endFirst - origin);
02942                                                 cv::Mat endRotated = rotation*((tmp/cv::norm(tmp))*maxLength) + origin;
02943                                                 cv::Mat endLastVector(3,1,CV_32F), endRotatedVector(3,1,CV_32F);
02944                                                 endLastVector.at<float>(0) = endLast.at<float>(0) - origin.at<float>(0);
02945                                                 endLastVector.at<float>(1) = endLast.at<float>(1) - origin.at<float>(1);
02946                                                 endLastVector.at<float>(2) = 0.0f;
02947                                                 endRotatedVector.at<float>(0) = endRotated.at<float>(0) - origin.at<float>(0);
02948                                                 endRotatedVector.at<float>(1) = endRotated.at<float>(1) - origin.at<float>(1);
02949                                                 endRotatedVector.at<float>(2) = 0.0f;
02950                                                 //UWARN("endRotated = %f %f", endRotated.at<float>(0), endRotated.at<float>(1));
02951                                                 while(endRotatedVector.cross(endLastVector).at<float>(2) > 0.0f)
02952                                                 {
02953                                                         cv::Point2i end((endRotated.at<float>(0)-xMin)/cellSize + 0.5f, (endRotated.at<float>(1)-yMin)/cellSize + 0.5f);
02954                                                         //end must be inside the grid
02955                                                         end.x = end.x < 0?0:end.x;
02956                                                         end.x = end.x >= map.cols?map.cols-1:end.x;
02957                                                         end.y = end.y < 0?0:end.y;
02958                                                         end.y = end.y >= map.rows?map.rows-1:end.y;
02959                                                         rayTrace(start, end, map, true); // trace free space
02960 
02961                                                         // next point
02962                                                         endRotated = rotation*(endRotated - origin) + origin;
02963                                                         endRotatedVector.at<float>(0) = endRotated.at<float>(0) - origin.at<float>(0);
02964                                                         endRotatedVector.at<float>(1) = endRotated.at<float>(1) - origin.at<float>(1);
02965                                                         //UWARN("endRotated = %f %f", endRotated.at<float>(0), endRotated.at<float>(1));
02966                                                 }
02967                                         }
02968                                 }
02969                                 ++j;
02970                         }
02971                         UDEBUG("Fill empty space=%fs", timer.ticks());
02972                 }
02973         }
02974         return map;
02975 }
02976 
02977 void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid, bool stopOnObstacle)
02978 {
02979         UASSERT_MSG(start.x >= 0 && start.x < grid.cols, uFormat("start.x=%d grid.cols=%d", start.x, grid.cols).c_str());
02980         UASSERT_MSG(start.y >= 0 && start.y < grid.rows, uFormat("start.y=%d grid.rows=%d", start.y, grid.rows).c_str());
02981         UASSERT_MSG(end.x >= 0 && end.x < grid.cols, uFormat("end.x=%d grid.cols=%d", end.x, grid.cols).c_str());
02982         UASSERT_MSG(end.y >= 0 && end.y < grid.rows, uFormat("end.x=%d grid.cols=%d", end.y, grid.rows).c_str());
02983 
02984         cv::Point2i ptA, ptB;
02985         ptA = start;
02986         ptB = end;
02987 
02988         float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x);
02989 
02990         bool swapped = false;
02991         if(slope<-1.0f || slope>1.0f)
02992         {
02993                 // swap x and y
02994                 slope = 1.0f/slope;
02995 
02996                 int tmp = ptA.x;
02997                 ptA.x = ptA.y;
02998                 ptA.y = tmp;
02999 
03000                 tmp = ptB.x;
03001                 ptB.x = ptB.y;
03002                 ptB.y = tmp;
03003 
03004                 swapped = true;
03005         }
03006 
03007         float b = ptA.y - slope*ptA.x;
03008         for(int x=ptA.x; ptA.x<ptB.x?x<ptB.x:x>ptB.x; ptA.x<ptB.x?++x:--x)
03009         {
03010                 int upperbound = float(x)*slope + b;
03011                 int lowerbound = upperbound;
03012                 if(x != ptA.x)
03013                 {
03014                         lowerbound = (ptA.x<ptB.x?x+1:x-1)*slope + b;
03015                 }
03016 
03017                 if(lowerbound > upperbound)
03018                 {
03019                         int tmp = upperbound;
03020                         upperbound = lowerbound;
03021                         lowerbound = tmp;
03022                 }
03023 
03024                 if(!swapped)
03025                 {
03026                         UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.rows, uFormat("lowerbound=%f grid.rows=%d x=%d slope=%f b=%f x=%f", lowerbound, grid.rows, x, slope, b, x).c_str());
03027                         UASSERT_MSG(upperbound >= 0 && upperbound < grid.rows, uFormat("upperbound=%f grid.rows=%d x+1=%d slope=%f b=%f x=%f", upperbound, grid.rows, x+1, slope, b, x).c_str());
03028                 }
03029                 else
03030                 {
03031                         UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.cols, uFormat("lowerbound=%f grid.cols=%d x=%d slope=%f b=%f x=%f", lowerbound, grid.cols, x, slope, b, x).c_str());
03032                         UASSERT_MSG(upperbound >= 0 && upperbound < grid.cols, uFormat("upperbound=%f grid.cols=%d x+1=%d slope=%f b=%f x=%f", upperbound, grid.cols, x+1, slope, b, x).c_str());
03033                 }
03034 
03035                 for(int y = lowerbound; y<=(int)upperbound; ++y)
03036                 {
03037                         char * v;
03038                         if(swapped)
03039                         {
03040                                 v = &grid.at<char>(x, y);
03041                         }
03042                         else
03043                         {
03044                                 v = &grid.at<char>(y, x);
03045                         }
03046                         if(*v == 100 && stopOnObstacle)
03047                         {
03048                                 return;
03049                         }
03050                         else
03051                         {
03052                                 *v = 0; // free space
03053                         }
03054                 }
03055         }
03056 }
03057 
03058 //convert to gray scaled map
03059 cv::Mat convertMap2Image8U(const cv::Mat & map8S)
03060 {
03061         UASSERT(map8S.channels() == 1 && map8S.type() == CV_8S);
03062         cv::Mat map8U = cv::Mat(map8S.rows, map8S.cols, CV_8U);
03063         for (int i = 0; i < map8S.rows; ++i)
03064         {
03065                 for (int j = 0; j < map8S.cols; ++j)
03066                 {
03067                         char v = map8S.at<char>(i, j);
03068                         unsigned char gray;
03069                         if(v == 0)
03070                         {
03071                                 gray = 178;
03072                         }
03073                         else if(v == 100)
03074                         {
03075                                 gray = 0;
03076                         }
03077                         else // -1
03078                         {
03079                                 gray = 89;
03080                         }
03081                         map8U.at<unsigned char>(i, j) = gray;
03082                 }
03083         }
03084         return map8U;
03085 }
03086 
03087 pcl::IndicesPtr concatenate(const std::vector<pcl::IndicesPtr> & indices)
03088 {
03089         //compute total size
03090         unsigned int totalSize = 0;
03091         for(unsigned int i=0; i<indices.size(); ++i)
03092         {
03093                 totalSize += (unsigned int)indices[i]->size();
03094         }
03095         pcl::IndicesPtr ind(new std::vector<int>(totalSize));
03096         unsigned int io = 0;
03097         for(unsigned int i=0; i<indices.size(); ++i)
03098         {
03099                 for(unsigned int j=0; j<indices[i]->size(); ++j)
03100                 {
03101                         ind->at(io++) = indices[i]->at(j);
03102                 }
03103         }
03104         return ind;
03105 }
03106 
03107 pcl::IndicesPtr concatenate(const pcl::IndicesPtr & indicesA, const pcl::IndicesPtr & indicesB)
03108 {
03109         pcl::IndicesPtr ind(new std::vector<int>(*indicesA));
03110         ind->resize(ind->size()+indicesB->size());
03111         unsigned int oi = (unsigned int)indicesA->size();
03112         for(unsigned int i=0; i<indicesB->size(); ++i)
03113         {
03114                 ind->at(oi++) = indicesB->at(i);
03115         }
03116         return ind;
03117 }
03118 
03119 cv::Mat decimate(const cv::Mat & image, int decimation)
03120 {
03121         UASSERT(decimation >= 1);
03122         cv::Mat out;
03123         if(!image.empty())
03124         {
03125                 if(decimation > 1)
03126                 {
03127                         if((image.type() == CV_32FC1 || image.type()==CV_16UC1))
03128                         {
03129                                 UASSERT_MSG(image.rows % decimation == 0 && image.cols % decimation == 0, "Decimation of depth images should be exact!");
03130 
03131                                 out = cv::Mat(image.rows/decimation, image.cols/decimation, image.type());
03132                                 if(image.type() == CV_32FC1)
03133                                 {
03134                                         for(int j=0; j<out.rows; ++j)
03135                                         {
03136                                                 for(int i=0; i<out.cols; ++i)
03137                                                 {
03138                                                         out.at<float>(j, i) = image.at<float>(j*decimation, i*decimation);
03139                                                 }
03140                                         }
03141                                 }
03142                                 else // CV_16UC1
03143                                 {
03144                                         for(int j=0; j<out.rows; ++j)
03145                                         {
03146                                                 for(int i=0; i<out.cols; ++i)
03147                                                 {
03148                                                         out.at<unsigned short>(j, i) = image.at<unsigned short>(j*decimation, i*decimation);
03149                                                 }
03150                                         }
03151                                 }
03152                         }
03153                         else
03154                         {
03155                                 cv::resize(image, out, cv::Size(), 1.0f/float(decimation), 1.0f/float(decimation), cv::INTER_AREA);
03156                         }
03157                 }
03158                 else
03159                 {
03160                         out = image;
03161                 }
03162         }
03163         return out;
03164 }
03165 
03166 void savePCDWords(
03167                 const std::string & fileName,
03168                 const std::multimap<int, pcl::PointXYZ> & words,
03169                 const Transform & transform)
03170 {
03171         if(words.size())
03172         {
03173                 pcl::PointCloud<pcl::PointXYZ> cloud;
03174                 cloud.resize(words.size());
03175                 int i=0;
03176                 for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
03177                 {
03178                         cloud[i++] = util3d::transformPoint(iter->second, transform);
03179                 }
03180                 pcl::io::savePCDFile(fileName, cloud);
03181         }
03182 }
03183 
03184 }
03185 
03186 }


rtabmap
Author(s): Mathieu Labbe
autogenerated on Fri Aug 28 2015 12:51:42