$search
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