normals.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002  * \file
00003  *
00004  * $Id: normals.cpp 694 2012-04-20 10:24:24Z ihulik $
00005  *
00006  * Copyright (C) Brno University of Technology
00007  *
00008  * This file is part of software developed by dcgm-robotics@FIT group.
00009  *
00010  * Author: Rostislav Hulik (ihulik@fit.vutbr.cz)
00011  * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
00012  * Date: 11.01.2012 (version 0.8)
00013  * 
00014  * This file is free software: you can redistribute it and/or modify
00015  * it under the terms of the GNU Lesser General Public License as published by
00016  * the Free Software Foundation, either version 3 of the License, or
00017  * (at your option) any later version.
00018  * 
00019  * This file is distributed in the hope that it will be useful,
00020  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00021  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00022  * GNU Lesser General Public License for more details.
00023  * 
00024  * You should have received a copy of the GNU Lesser General Public License
00025  * along with this file.  If not, see <http://www.gnu.org/licenses/>.
00026  */
00027 
00033 #include <srs_env_model_percp/but_segmentation/normals.h>
00034 
00035 // std
00036 #include <stdlib.h>
00037 #include <stdio.h>
00038 #include <time.h>
00039 
00040 // Eigen
00041 #include <Eigen/Core>
00042 #include <Eigen/Eigenvalues>
00043 
00044 // Open CV
00045 #include <opencv2/imgproc/imgproc.hpp>
00046 
00047 // pcl
00048 #include <pcl/point_cloud.h>
00049 #include <pcl/point_types.h>
00050 #include <pcl/features/integral_image_normal.h>
00051 
00052 using namespace sensor_msgs;
00053 using namespace std;
00054 using namespace cv;
00055 
00056 
00057 namespace but_plane_detector
00058 {
00059 
00061 // Constructor - computes real point positions (in scene coordinates) and normals and initiates all variables
00062 // @param points Input CV_16UC depth matrix (raw input from kinect)
00063 // @param cam_info Camera info message (ROS)
00064 // @param normalType Type of normal computation method (NormalType enum)
00065 // @see NormalType()
00066 // @param neighborhood Neighborhood from which normals are computed
00067 // @param threshold Threshold for depth difference outlier marking (if depth of neighbor is greater than this threshold, point is skipped)
00068 // @param outlierThreshold Outlier threshold for least trimmed squares regression (max error between point depth and proposed plane)
00069 // @param iter Maximum RANSAC iterations
00071 Normals::Normals(cv::Mat &points, const CameraInfoConstPtr& cam_info, int normalType, int neighborhood, float threshold, float outlierThreshold, int iter):
00072                                                                                                         m_points(points.size(), CV_32FC3),
00073                                                                                                         m_planes(points.size(), CV_32FC4)
00074 {
00075 
00076         m_cam_info = (CameraInfoConstPtr)cam_info;
00077 
00078         float aux;
00079         Vec3f nullvector(0.0, 0.0, 0.0);
00080         Vec4f nullvector4(0.0, 0.0, 0.0, 0.0);
00081 
00082         if (normalType == NormalType::PCL)
00083         {
00084                 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>());
00085                 // ... fill point cloud...
00086 
00087                 cloud->width = points.cols;
00088                 cloud->height = points.rows;
00089                 cloud->points.resize (cloud->width * cloud->height);
00090 
00091                 for (int i = 0; i < points.rows; ++i)
00092                 for (int j = 0; j < points.cols; ++j)
00093                 {
00094 
00095 
00096 
00097                         if ((aux = points.at<unsigned short>(i, j)) != 0)
00098                         {
00099                                 Vec3f realPoint;
00100                                 realPoint[2] = aux/1000.0;
00101                                 realPoint[0] = ( (j - cam_info->K[2]) * realPoint[2] / cam_info->K[0] );
00102                                 realPoint[1] = ( (i - cam_info->K[5]) * realPoint[2] / cam_info->K[4] );
00103                                 cloud->operator()(j, i).x = realPoint[0];
00104                                 cloud->operator()(j, i).y = realPoint[1];
00105                                 cloud->operator()(j, i).z = realPoint[2];
00106 
00107                                 m_points.at<Vec3f>(i, j) = realPoint;
00108                         }
00109                         else
00110                         {
00111                                 m_points.at<Vec3f>(i, j) = nullvector;
00112                                 cloud->operator()(j, i).x = 0.0;
00113                                 cloud->operator()(j, i).y = 0.0;
00114                                 cloud->operator()(j, i).z = 0.0;
00115                         }
00116                 }
00117 
00118                 // Estimate normals
00119                 pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
00120 
00121                 pcl::PointCloud<pcl::Normal> normals;
00122 
00123                 ne.setNormalEstimationMethod (ne.AVERAGE_DEPTH_CHANGE);
00124                 ne.setDepthDependentSmoothing(true);
00125                 ne.setMaxDepthChangeFactor(threshold);
00126                 //ne.setRectSize(10, 10);
00127                 ne.setNormalSmoothingSize((float)(neighborhood*2+1));
00128                 ne.setInputCloud(cloud);
00129                 ne.compute(normals);
00130 
00131                 #pragma omp parallel for
00132                 for (int i = 0; i < points.rows; ++i)
00133                 for (int j = 0; j < points.cols; ++j)
00134                 {
00135                         Vec3f realPoint = m_points.at<Vec3f>(i, j);
00136                         if (realPoint != nullvector)
00137                         {
00138                                 Vec4f normal;
00139                                 if (normals(j, i).normal_x == normals(j, i).normal_x)
00140                                 {
00141                                         normal[0] = normals(j, i).normal_x;
00142                                         normal[1] = normals(j, i).normal_y;
00143                                         normal[2] = normals(j, i).normal_z;
00144                                         if (normal[2] < 0)
00145                                         {
00146                                                 normal[0] *= -1.0;
00147                                                 normal[1] *= -1.0;
00148                                                 normal[2] *= -1.0;
00149                                         }
00150                                         normal[3] = -(normal[0]*realPoint[0]+normal[1]*realPoint[1]+normal[2]*realPoint[2]);
00151                                         m_planes.at<Vec4f>(i, j) = normal;
00152                                 }
00153                                 else
00154                                         m_planes.at<Vec4f>(i, j) = nullvector4;
00155                         }
00156                         else
00157                         {
00158                                 m_planes.at<Vec4f>(i, j) = nullvector4;
00159                         }
00160                 }
00161         }
00162         else
00163         {
00164                 for (int i = 0; i < points.rows; ++i)
00165                 for (int j = 0; j < points.cols; ++j)
00166                 {
00167                         if ((aux = points.at<unsigned short>(i, j)) != 0)
00168                         {
00169                                 Vec3f realPoint;
00170                                 realPoint[2] = aux/1000.0;
00171                                 realPoint[0] = ( (j - cam_info->K[2]) * realPoint[2] / cam_info->K[0] );
00172                                 realPoint[1] = ( (i - cam_info->K[5]) * realPoint[2] / cam_info->K[4] );
00173                                 m_points.at<Vec3f>(i, j) = realPoint;
00174                         }
00175                         else
00176                         {
00177                                 m_points.at<Vec3f>(i, j) = nullvector;
00178                         }
00179                 }
00180 
00181                 if (normalType & NormalType::DIRECT)
00182                         {
00183                                 for (int i = 0; i < points.rows; ++i)
00184                                         for (int j = 0; j < points.cols; ++j)
00185                                         {
00186                                                 Vec3f realPoint = m_points.at<Vec3f>(i, j);
00187                                                 if (realPoint != nullvector)
00188                                                 {
00189                                                         Vec4f normal = getNormal(i, j, neighborhood, threshold);
00190                                                         m_planes.at<Vec4f>(i, j) = normal;
00191                                                 }
00192                                                 else
00193                                                 {
00194                                                         m_planes.at<Vec4f>(i, j) = nullvector4;
00195                                                 }
00196                                         }
00197                         }
00198                 else if (normalType & NormalType::LSQ)
00199                         {
00200                                 for (int i = 0; i < points.rows; ++i)
00201                                         for (int j = 0; j < points.cols; ++j)
00202                                         {
00203                                                 Vec3f realPoint = m_points.at<Vec3f>(i, j);
00204                                                 if (realPoint != nullvector)
00205                                                 {
00206                                                         Vec4f normal = getNormalLSQ(i, j, neighborhood, threshold);
00207                                                         m_planes.at<Vec4f>(i, j) = normal;
00208                                                 }
00209                                                 else
00210                                                 {
00211                                                         m_planes.at<Vec4f>(i, j) = nullvector4;
00212                                                 }
00213                                         }
00214                         }
00215                 else if (normalType & NormalType::LSQAROUND)
00216                         {
00217                                 #pragma omp parallel for
00218                                 for (int i = 0; i < points.rows; ++i)
00219                                         for (int j = 0; j < points.cols; ++j)
00220                                         {
00221                                                 Vec3f realPoint = m_points.at<Vec3f>(i, j);
00222                                                 if (realPoint != nullvector)
00223                                                 {
00224                                                         Vec4f normal= getNormalLSQAround(i, j, neighborhood, threshold);
00225                                                         m_planes.at<Vec4f>(i, j) = normal;
00226                                                 }
00227                                                 else
00228                                                         m_planes.at<Vec4f>(i, j) = nullvector4;
00229                                         }
00230                         }
00231                 else if (normalType & NormalType::LTS)
00232                         {
00233                                 for (int i = 0; i < points.rows; ++i)
00234                                         for (int j = 0; j < points.cols; ++j)
00235                                         {
00236                                                 Vec3f realPoint = m_points.at<Vec3f>(i, j);
00237                                                 if (realPoint != nullvector)
00238                                                 {
00239                                                         Vec4f normal = getNormalLTS(i, j, neighborhood, threshold, outlierThreshold, iter);
00240                                                         m_planes.at<Vec4f>(i, j) = normal;
00241                                                 }
00242                                                 else
00243                                                 {
00244                                                         m_planes.at<Vec4f>(i, j) = nullvector4;
00245                                                 }
00246                                         }
00247                         }
00248                 else if (normalType & NormalType::LTSAROUND)
00249                         {
00250                                 for (int i = 0; i < points.rows; ++i)
00251                                         for (int j = 0; j < points.cols; ++j)
00252                                         {
00253                                                 Vec3f realPoint = m_points.at<Vec3f>(i, j);
00254                                                 if (realPoint != nullvector)
00255                                                 {
00256                                                         Vec4f normal = getNormalLTSAround(i, j, neighborhood, threshold, outlierThreshold, iter);
00257                                                         m_planes.at<Vec4f>(i, j) = normal;
00258                                                 }
00259                                                 else
00260                                                 {
00261                                                         m_planes.at<Vec4f>(i, j) = nullvector4;
00262                                                 }
00263                                         }
00264                         }
00265         }
00266 
00267         //int n_bins = 16;
00268         //if (n_bins)
00269         //{
00270         //      m_quantbins = n_bins;
00271         // initQuantVectors(m_quantbins, m_quantVectors);
00272         //}
00273 
00274 }
00275 
00277 // Constructor - computes real point positions (in scene coordinates) and normals and initiates all variables
00278 // @param pointcloud Input point cloud
00279 // @param threshold Threshold for depth difference outlier marking (if depth of neighbor is greater than this threshold, point is skipped)
00280 // @param neighborhood Neighborhood from which normals are computed
00282 Normals::Normals(pcl::PointCloud<pcl::PointXYZ> &pointcloud, int normalType, int neighborhood,
00283                  float threshold, float outlierThreshold, int iter):
00284                                                                                                         m_points(cvSize(pointcloud.width, pointcloud.height), CV_32FC3),
00285                                                                                                         m_planes(cvSize(pointcloud.width, pointcloud.height), CV_32FC4)
00286 {
00287         std::cerr << "Normal computation method " << normalType << std::endl;
00288                 // ... fill point cloud...
00289         Vec3f nullvector(0.0, 0.0, 0.0);
00290         Vec4f nullvector4(0.0, 0.0, 0.0, 0.0);
00291 
00292         for(int y = 0; y < (int)pointcloud.height; ++y)
00293         for(int x = 0; x < (int)pointcloud.width; ++x)
00294         {
00295                                 Vec3f realPoint;
00296                                 realPoint[0] = pointcloud.at(x, y).x;
00297                                 realPoint[1] = pointcloud.at(x, y).y;
00298                                 realPoint[2] = pointcloud.at(x, y).z;
00299 
00300                                 m_points.at<Vec3f>(y, x) = realPoint;
00301         }
00302 
00303         if (normalType == NormalType::PCL)
00304         {
00305                 // Estimate normals
00306                 pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
00307                 pcl::PointCloud<pcl::Normal> normals;
00308 
00309                 ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
00310                 ne.setDepthDependentSmoothing(true);
00311                 ne.setMaxDepthChangeFactor(threshold);
00312                 ne.setNormalSmoothingSize(neighborhood);
00313                 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = pointcloud.makeShared();
00314                 ne.setInputCloud(cloud);
00315                 ne.compute(normals);
00316 
00317                 for (int i = 0; i < m_points.rows; ++i)
00318                         for (int j = 0; j < m_points.cols; ++j)
00319                         {
00320                                 Vec3f realPoint = m_points.at<Vec3f>(i, j);
00321 
00322                                 Vec4f normal;
00323                                 if (normals(j, i).normal_x == normals(j, i).normal_x &&
00324                                         normals(j, i).normal_y == normals(j, i).normal_y &&
00325                                         normals(j, i).normal_z == normals(j, i).normal_z)
00326                                 {
00327                                         normal[0] = normals(j, i).normal_x;
00328                                         normal[1] = normals(j, i).normal_y;
00329                                         normal[2] = normals(j, i).normal_z;
00330 
00331                                         normal[3] = -(normal[0]*realPoint[0]+normal[1]*realPoint[1]+normal[2]*realPoint[2]);
00332                                         m_planes.at<Vec4f>(i, j) = normal;
00333                                 }
00334                                 else
00335                                         m_planes.at<Vec4f>(i, j) = nullvector4;
00336                         }
00337         }
00338         else if (normalType == NormalType::LSQAROUND)
00339         {
00340                 for (int i = 0; i < m_points.rows; ++i)
00341                 for (int j = 0; j < m_points.cols; ++j)
00342                 {
00343                         Vec3f realPoint = m_points.at<Vec3f>(i, j);
00344                         if (realPoint != nullvector)
00345                         {
00346                                 Vec4f normal= getNormalLSQAround(i, j, neighborhood, threshold);
00347                                 m_planes.at<Vec4f>(i, j) = normal;
00348                         }
00349                                 else
00350                                 m_planes.at<Vec4f>(i, j) = nullvector4;
00351                 }
00352         }
00353 
00354         else if (normalType & NormalType::DIRECT)
00355         {
00356                 for (int i = 0; i < m_points.rows; ++i)
00357                 for (int j = 0; j < m_points.cols; ++j)
00358                 {
00359                         Vec3f realPoint = m_points.at<Vec3f>(i, j);
00360                         if (realPoint != nullvector)
00361                         {
00362                                 Vec4f normal = getNormal(i, j, neighborhood, threshold);
00363                                 m_planes.at<Vec4f>(i, j) = normal;
00364                         }
00365                         else
00366                         {
00367                                 m_planes.at<Vec4f>(i, j) = nullvector4;
00368                         }
00369                 }
00370         }
00371         else if (normalType & NormalType::LSQ)
00372         {
00373                 for (int i = 0; i < m_points.rows; ++i)
00374                 for (int j = 0; j < m_points.cols; ++j)
00375                 {
00376                         Vec3f realPoint = m_points.at<Vec3f>(i, j);
00377                         if (realPoint != nullvector)
00378                         {
00379                                 Vec4f normal = getNormalLSQ(i, j, neighborhood, threshold);
00380                                 m_planes.at<Vec4f>(i, j) = normal;
00381                         }
00382                         else
00383                         {
00384                                 m_planes.at<Vec4f>(i, j) = nullvector4;
00385                         }
00386                 }
00387         }
00388 
00389         else if (normalType & NormalType::LTS)
00390         {
00391                 for (int i = 0; i < m_points.rows; ++i)
00392                 for (int j = 0; j < m_points.cols; ++j)
00393                 {
00394                         Vec3f realPoint = m_points.at<Vec3f>(i, j);
00395                         if (realPoint != nullvector)
00396                         {
00397                                 Vec4f normal = getNormalLTS(i, j, neighborhood, threshold, outlierThreshold, iter);
00398                                 m_planes.at<Vec4f>(i, j) = normal;
00399                         }
00400                         else
00401                         {
00402                                 m_planes.at<Vec4f>(i, j) = nullvector4;
00403                         }
00404                 }
00405         }
00406         else if (normalType & NormalType::LTSAROUND)
00407         {
00408                 for (int i = 0; i < m_points.rows; ++i)
00409                 for (int j = 0; j < m_points.cols; ++j)
00410                 {
00411                         Vec3f realPoint = m_points.at<Vec3f>(i, j);
00412                         if (realPoint != nullvector)
00413                         {
00414                                 Vec4f normal = getNormalLTSAround(i, j, neighborhood, threshold, outlierThreshold, iter);
00415                                 m_planes.at<Vec4f>(i, j) = normal;
00416                         }
00417                         else
00418                         {
00419                                 m_planes.at<Vec4f>(i, j) = nullvector4;
00420                         }
00421                 }
00422         }
00423 
00424 //      for (int i = 0; i < m_points.rows; ++i)
00425 //      for (int j = 0; j < m_points.cols; ++j)
00426 //      {
00427 //              Vec3f realPoint = m_points.at<Vec3f>(i, j);
00428 //              if (realPoint != nullvector)
00429 //              {
00430 //                      Vec4f normal= getNormalLSQAround(i, j, 8, threshold);
00431 //                      m_planes.at<Vec4f>(i, j) = normal;
00432 //              }
00433 //              else
00434 //                      m_planes.at<Vec4f>(i, j) = nullvector4;
00435 //              }
00436 }
00438 // Function computes normal for point (i, j) using direct computation (mean of surrounding triangles)
00439 // @param i row index
00440 // @param j column index
00441 // @param step Neighborhood to compute with
00442 // @param depthThreshold Threshold for depth difference outlier marking (if depth of neighbor is greater than this threshold, point is skipped)
00444 cv::Vec4f Normals::getNormal(int i, int j, int step, float depthThreshold)
00445 {
00446         int size = (step*2)*4;
00447 
00448         cv::Vec<float, 4> normalVec;
00449                 normalVec[0] = 0;
00450                 normalVec[1] = 0;
00451                 normalVec[2] = 0;
00452                 normalVec[3] = 0;
00453 
00454         if (i < step || j < step || i >= m_points.rows-step || j >= m_points.cols-step)
00455                         return normalVec;
00456 
00457         cv::Vec3f aux;
00458         cv::Vec3f center;
00459 
00460         Vec3f realPoint = m_points.at<Vec3f>(i, j);
00461         center[0] = realPoint[0];
00462         center[1] = realPoint[1];
00463         center[2] = realPoint[2];
00464 
00465         std::vector<Vec3f> around;
00466 
00467 
00468         // neighbourhood
00469         int x = -step;
00470         int y = -step;
00471         int plusX = 1;
00472         int plusY = 0;
00473 
00474         Vec3f centroid;
00475         centroid[0] = 0;
00476         centroid[1] = 0;
00477         centroid[2] = 0;
00478 
00479         for (int index = 0; index < size; ++index)
00480         {
00481                 realPoint = m_points.at<Vec3f>(i+x, j+y);
00482                 if (abs(center[2]-realPoint[2]) < depthThreshold)
00483                 {
00484                         centroid += realPoint;
00485                         around.push_back(realPoint - center);
00486                 }
00487 
00488                 // move next pixel around
00489                 nextStep(step, x, y, plusX, plusY);
00490 
00491         }
00492         size = around.size();
00493         centroid[0] /= size;
00494         centroid[1] /= size;
00495         centroid[2] /= size;
00496 
00497         for (int index = 0; index < size; ++index)
00498         {
00499                 int second = ((index+1) % size);
00500                 aux = around[index].cross(around[second]);
00501                 normalVec = normalVec + Vec4f(aux[0], aux[1], aux[2], 0.0);
00502         }
00503 
00504         normalVec[3] = -(centroid[0]*normalVec[0] + centroid[1]*normalVec[1] + centroid[2]*normalVec[2]);
00505 
00506         float norm = sqrt(normalVec[0]*normalVec[0] + normalVec[1]*normalVec[1] + normalVec[2]*normalVec[2]);
00507         if (norm != 0)
00508         {
00509                 if (normalVec[2] < 0)
00510                 {
00511                         normalVec[0] = normalVec[0] / -norm;
00512                         normalVec[1] = normalVec[1] / -norm;
00513                         normalVec[2] = normalVec[2] / -norm;
00514                         normalVec[3] = normalVec[3] / -norm;
00515                 }
00516                 else
00517                 {
00518                         normalVec[0] = normalVec[0] / norm;
00519                         normalVec[1] = normalVec[1] / norm;
00520                         normalVec[2] = normalVec[2] / norm;
00521                         normalVec[3] = normalVec[3] / norm;
00522                 }
00523         }
00524 
00525         return normalVec;
00526 }
00527 
00529 // Function computes normal for point (i, j) using least squares regression
00530 // @param i row index
00531 // @param j column index
00532 // @param step Neighborhood to compute with
00533 // @param depthThreshold Threshold for depth difference outlier marking (if depth of neighbor is greater than this threshold, point is skipped)
00535 cv::Vec4f Normals::getNormalLSQ(int i, int j, int step, float depthThreshold)
00536 {
00537         cv::Vec<float, 4> normalVec;
00538                         normalVec[0] = 0;
00539                         normalVec[1] = 0;
00540                         normalVec[2] = 0;
00541                         normalVec[3] = 0;
00542 
00543                 if (i < step || j < step || i >= m_points.rows-step || j >= m_points.cols-step)
00544                 return normalVec;
00545 
00546         int iMax = i+step;
00547         int jMax = j+step;
00548 
00549         std::vector<Vec3f> points;
00550 
00551         Vec3f center = m_points.at<Vec3f>(i, j);
00552         Vec3f point;
00553         for (int x = i-step; x <= iMax; ++x)
00554                 for (int y = j-step; y <= jMax; ++y)
00555                 {
00556                         point = m_points.at<Vec3f>(x, y);
00557                         if (abs(center[2]-point[2]) < depthThreshold)
00558                         {
00559                                 points.push_back(point);
00560                         }
00561                 }
00562 
00563         if (points.size() < 3)
00564                 return normalVec;
00565 
00566         Plane<float> plane = LeastSquaresPlane(points);
00567 
00568         return Vec4f(plane.a, plane.b, plane.c, plane.d);
00569 }
00570 
00572 // Function computes normal for point (i, j) using least squares regression with using only outer neighborhood ring
00573 // @param i row index
00574 // @param j column index
00575 // @param step Neighborhood to compute with
00576 // @param depthThreshold Threshold for depth difference outlier marking (if depth of neighbor is greater than this threshold, point is skipped)
00578 cv::Vec4f Normals::getNormalLSQAround(int i, int j, int step, float depthThreshold)
00579 {
00580         cv::Vec<float, 4> normalVec;
00581                         normalVec[0] = 0;
00582                         normalVec[1] = 0;
00583                         normalVec[2] = 0;
00584                         normalVec[3] = 0;
00585 
00586         if (i < step || j < step || i >= m_points.rows-step || j >= m_points.cols-step)
00587         return normalVec;
00588 
00589         std::vector<Vec3f> points;
00590 
00591         Vec3f center = m_points.at<Vec3f>(i, j);
00592         Vec3f point;
00593         int x = -step;
00594         int y = -step;
00595         int plusX = 1;
00596         int plusY = 0;
00597         int size = (step*2)*4;
00598 
00599         for (int index = 0; index < size; ++index)
00600         {
00601                 point = m_points.at<Vec3f>(i+x, j+y);
00602                 if (abs(center[2]-point[2]) < depthThreshold)
00603                 {
00604                         points.push_back(point);
00605                 }
00606 
00607                 // move next pixel around
00608                 nextStep(step, x, y, plusX, plusY);
00609 
00610         }
00611 
00612         if (points.size() < 3)
00613                 return normalVec;
00614 
00615         Plane<float> plane = LeastSquaresPlane(points);
00616 
00617         return Vec4f(plane.a, plane.b, plane.c, plane.d);
00618 }
00619 
00621 // Function computes normal for point (i, j) using least trimmed squares regression
00622 // @param i row index
00623 // @param j column index
00624 // @param step Neighborhood to compute with
00625 // @param depthThreshold Threshold for depth difference outlier marking (if depth of neighbor is greater than this threshold, point is skipped)
00626 // @param outlierThreshold Threshold for marking outliers in RANSAC (maximum difference from proposed plane)
00627 // @param maxIter Maximum RANSAC iterations
00629 cv::Vec4f Normals::getNormalLTS(int i, int j, int step, float depthThreshold, float outlierThreshold, int maxIter)
00630 {
00631         cv::Vec<float, 4> normalVec;
00632                         normalVec[0] = 0;
00633                         normalVec[1] = 0;
00634                         normalVec[2] = 0;
00635                         normalVec[3] = 0;
00636 
00637         if (i < step || j < step || i >= m_points.rows-step || j >= m_points.cols-step)
00638                         return normalVec;
00639 
00640         int iMax = i+step;
00641         int jMax = j+step;
00642 
00643         Plane<float> best(0.0, 0.0, 0.0, 0.0);
00644         float bestScore = 9999999.0;
00645         Plane<float> candidate(0.0, 0.0, 0.0, 0.0);
00646         Vec3f center = m_points.at<Vec3f>(i, j);
00647         Vec3f point;
00648         for (int cnt = 0; cnt < maxIter; ++cnt)
00649         {
00650                 std::vector<Vec3f> points;
00651                 for (int x = i-step; x <= iMax; ++x)
00652                         for (int y = j-step; y <= jMax; ++y)
00653                         {
00654                                 point = m_points.at<Vec3f>(x, y);
00655                                 if (rand() > RAND_MAX/2 && abs(center[2]-point[2]) < depthThreshold)
00656                                 {
00657                                         points.push_back(point);
00658                                 }
00659                         }
00660 
00661                 candidate = LeastSquaresPlane(points);
00662 
00663                 float score = 0.0;
00664                 float count = 0.0;
00665                 float aux;
00666 
00667                 for (int x = i-step; x < iMax; ++x)
00668                         for (int y = j-step; y < jMax; ++y)
00669                         {
00670                                 aux = candidate.distance(m_points.at<Vec3f>(x, y));
00671                                 if (aux < outlierThreshold)
00672                                 {
00673                                         score += aux;
00674                                         count += 1.0;
00675                                 }
00676                         }
00677 
00678                 score /= count;
00679 
00680                 if (bestScore > score)
00681                 {
00682                         bestScore = score;
00683                         best = candidate;
00684                 }
00685         }
00686 
00687         return Vec4f(best.a, best.b, best.c, best.d);
00688 }
00689 
00691 // Function computes normal for point (i, j) using least trimmed squares regression with using only outer neighborhood ring
00692 // @param i row index
00693 // @param j column index
00694 // @param step Neighborhood to compute with
00695 // @param depthThreshold Threshold for depth difference outlier marking (if depth of neighbor is greater than this threshold, point is skipped)
00696 // @param outlierThreshold Threshold for marking outliers in RANSAC (maximum difference from proposed plane)
00697 // @param maxIter Maximum RANSAC iterations
00699 cv::Vec4f Normals::getNormalLTSAround(int i, int j, int step, float depthThreshold, float outlierThreshold, int maxIter)
00700 {
00701         cv::Vec<float, 4> normalVec;
00702                         normalVec[0] = 0;
00703                         normalVec[1] = 0;
00704                         normalVec[2] = 0;
00705                         normalVec[3] = 0;
00706 
00707         if (i < step || j < step || i >= m_points.rows-step || j >= m_points.cols-step)
00708                         return normalVec;
00709 
00710         Plane<float> best(0.0, 0.0, 0.0, 0.0);
00711         float bestScore = 9999999.0;
00712         Plane<float> candidate(0.0, 0.0, 0.0, 0.0);
00713         Vec3f center = m_points.at<Vec3f>(i, j);
00714         Vec3f point;
00715         int size = (step*2)*4;
00716         for (int cnt = 0; cnt < maxIter; ++cnt)
00717         {
00718                 std::vector<Vec3f> points;
00719 
00720                 int x = -step;
00721                 int y = -step;
00722                 int plusX = 1;
00723                 int plusY = 0;
00724 
00725                 for (int index = 0; index < size; ++index)
00726                 {
00727                         point = m_points.at<Vec3f>(i+x, j+y);
00728                         if (rand() > RAND_MAX/2 && abs(center[2]-point[2]) < depthThreshold)
00729                                 points.push_back(point);
00730 
00731                         // move next pixel around
00732                         nextStep(step, x, y, plusX, plusY);
00733 
00734                 }
00735 
00736                 candidate = LeastSquaresPlane(points);
00737 
00738                 float score = 0.0;
00739                 float count = 0.0;
00740                 float aux;
00741 
00742                 x = -step;
00743                 y = -step;
00744                 plusX = 1;
00745                 plusY = 0;
00746 
00747                 for (int index = 0; index < size; ++index)
00748                 {
00749                         aux = candidate.distance(m_points.at<Vec3f>(i+x, j+y));
00750                         if (aux < outlierThreshold)
00751                         {
00752                                 score += aux;
00753                                 count += 1.0;
00754                         }
00755                         // move next pixel around
00756                         nextStep(step, x, y, plusX, plusY);
00757                 }
00758 
00759                 score /= count;
00760 
00761                 if (bestScore > score)
00762                 {
00763                         bestScore = score;
00764                         best = candidate;
00765                 }
00766         }
00767 
00768         return Vec4f(best.a, best.b, best.c, best.d);
00769 }
00770 
00772 // Function computes plane using least squares regression from given point set
00773 // @param points Vector of Vec3f points
00774 // @return Plane object
00775 // @see Plane()
00777 Plane<float> Normals::LeastSquaresPlane(std::vector<cv::Vec3f> &points)
00778 {
00779         if (points.size() == 0) return Plane<float>(0,0,0,0);
00780         cv::Vec3f centroid(0.0, 0.0, 0.0);
00781         Eigen::Matrix3f tensor = Eigen::Matrix3f::Zero();
00782 
00784         // first, compute a centroid (mean value)
00785         cv::Vec3f current = *points.begin();
00786         for (unsigned int i = 0; i < points.size(); ++i)
00787                 centroid += points[i];
00788         centroid[0] /= points.size();
00789         centroid[1] /= points.size();
00790         centroid[2] /= points.size();
00791 
00792 
00794         // second step, fill a tenzor
00795         double dx, dy, dz;
00796         for (unsigned int i = 0; i < points.size(); ++i)
00797         {
00798                 dx = centroid[0] - points[i][0];
00799                 dy = centroid[1] - points[i][1];
00800                 dz = centroid[2] - points[i][2];
00801 
00802                 tensor(0, 0) += dx*dx;
00803                 tensor(0, 1) += dx*dy;
00804                 tensor(0, 2) += dx*dz;
00805                 tensor(1, 0) += dy*dx;
00806                 tensor(1, 1) += dy*dy;
00807                 tensor(1, 2) += dy*dz;
00808                 tensor(2, 0) += dz*dx;
00809                 tensor(2, 1) += dz*dy;
00810                 tensor(2, 2) += dz*dz;
00811         }
00812 
00814         // compute eigenvectors and eigenvalues
00815         Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(tensor);
00816 
00817         Eigen::Matrix3f eigenVectors;
00818         Eigen::Vector3f eigenValues;
00819 
00820         eigenVectors = solver.eigenvectors();
00821         eigenValues = solver.eigenvalues();
00822         Plane<float> plane(eigenVectors(0, 0), eigenVectors(1, 0), eigenVectors(2, 0), -(centroid[0]*eigenVectors(0,0) + centroid[1]*eigenVectors(1,0) + centroid[2]*eigenVectors(2,0)));
00823 
00825         // Normalize
00826         if (plane.norm != 0)
00827         {
00828                 plane.a /= plane.norm;
00829                 plane.b /= plane.norm;
00830                 plane.c /= plane.norm;
00831                 plane.d /= plane.norm;
00832                 plane.norm = 1;
00833         }
00834         if (plane.c < 0)
00835         {
00836                 plane.a *= -1.0;
00837                 plane.b *= -1.0;
00838                 plane.c *= -1.0;
00839                 plane.d *= -1.0;
00840         }
00841 
00842 
00843         return plane;
00844 }
00845 
00847 // Helper function for "Around" functions - sets next point on outer ring
00848 // @param step Maximum distance from center (neighborhood)
00849 // @param x Current x offset
00850 // @param y Current y offset
00851 // @param plusX Next x shift
00852 // @param plusY Next y shift
00854 void Normals::nextStep(int step, int &x, int &y, int &plusX, int &plusY)
00855 {
00856         if (x == -step && y == -step)
00857         {
00858                 plusX = 1;
00859                 plusY = 0;
00860         }
00861         if (x == step && y == -step)
00862         {
00863                 plusX = 0;
00864                 plusY = 1;
00865         }
00866         if (x == step && y == step)
00867         {
00868                 plusX = -1;
00869                 plusY = 0;
00870         }
00871         if (x == -step && y == step)
00872         {
00873                 plusX = 0;
00874                 plusY = -1;
00875         }
00876         x += plusX;
00877         y += plusY;
00878 }
00879 
00881 // Helper function which initializes quantization vectors (not used, TODO)
00882 // @param n_bins Number of quantization bins
00883 // @param vec Vector of computed quantization vectors
00885 void Normals::initQuantVectors(int n_bins, std::vector<cv::Vec3f> &vec)
00886 {
00887         float twoPI = (2.0 * M_PI);
00888 
00889         float step = twoPI/n_bins;
00890         float sinPi4 = sin(M_PI_4);
00891         for (float i = 0; i < twoPI; i+= step)
00892         {
00893                 vec.push_back(cv::Vec3f(        cos(i)*sinPi4,
00894                                                                 sin(i)*sinPi4,
00895                                                                 sinPi4
00896                 ));
00897         }
00898 }
00899 
00901 // Helper function which returns bin index for given vector (not used, TODO)
00902 // @param vec Vector of computed quantization vectors
00903 // @param vector Vector whos bin we are computing
00905 unsigned int Normals::getQuantVector(std::vector<cv::Vec3f> &vec, cv::Vec3f &vector)
00906 {
00907         unsigned int size = vec.size();
00908         unsigned int minimum = 10;
00909         float minimalVal = 9999.0;
00910         for (unsigned int i = 0; i < size; ++i)
00911         {
00912                 float angle = acos(     vec[i][0] * vector[0] +
00913                                                         vec[i][1] * vector[1] +
00914                                                         vec[i][2] * vector[2]);
00915                 if (angle < minimalVal)
00916                 {
00917                         minimalVal = angle;
00918                         minimum = i;
00919                 }
00920         }
00921         return minimum;
00922 
00923 }
00924 
00925 }
00926 


srs_env_model_percp
Author(s): Rostislav Hulik (ihulik@fit.vutbr.cz), Tomas Hodan, Michal Spanel (spanel@fit.vutbr.cz)
autogenerated on Mon Oct 6 2014 08:07:21