Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2012, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 */ 00038 00039 #ifndef PCL_RANGE_IMAGE_IMPL_HPP_ 00040 #define PCL_RANGE_IMAGE_IMPL_HPP_ 00041 00042 #include <pcl/pcl_macros.h> 00043 #include <pcl/common/distances.h> 00044 00045 namespace pcl 00046 { 00047 00048 ///////////////////////////////////////////////////////////////////////// 00049 inline float 00050 RangeImage::asinLookUp (float value) 00051 { 00052 return (asin_lookup_table[ 00053 static_cast<int> ( 00054 static_cast<float> (pcl_lrintf ( (static_cast<float> (lookup_table_size-1) / 2.0f) * value)) + 00055 static_cast<float> (lookup_table_size-1) / 2.0f)]); 00056 } 00057 00058 ///////////////////////////////////////////////////////////////////////// 00059 inline float 00060 RangeImage::atan2LookUp (float y, float x) 00061 { 00062 if (x==0 && y==0) 00063 return 0; 00064 float ret; 00065 if (fabsf (x) < fabsf (y)) 00066 { 00067 ret = atan_lookup_table[ 00068 static_cast<int> ( 00069 static_cast<float> (pcl_lrintf ( (static_cast<float> (lookup_table_size-1) / 2.0f) * (x / y))) + 00070 static_cast<float> (lookup_table_size-1) / 2.0f)]; 00071 ret = static_cast<float> (x*y > 0 ? M_PI/2-ret : -M_PI/2-ret); 00072 } 00073 else 00074 ret = atan_lookup_table[ 00075 static_cast<int> ( 00076 static_cast<float> (pcl_lrintf ( (static_cast<float> (lookup_table_size-1) / 2.0f) * (y / x))) + 00077 static_cast<float> (lookup_table_size-1)/2.0f)]; 00078 if (x < 0) 00079 ret = static_cast<float> (y < 0 ? ret-M_PI : ret+M_PI); 00080 00081 return (ret); 00082 } 00083 00084 ///////////////////////////////////////////////////////////////////////// 00085 inline float 00086 RangeImage::cosLookUp (float value) 00087 { 00088 int cell_idx = static_cast<int> (pcl_lrintf ( (static_cast<float> (lookup_table_size-1)) * fabsf (value) / (2.0f * static_cast<float> (M_PI)))); 00089 return (cos_lookup_table[cell_idx]); 00090 } 00091 00092 ///////////////////////////////////////////////////////////////////////// 00093 template <typename PointCloudType> void 00094 RangeImage::createFromPointCloud (const PointCloudType& point_cloud, float angular_resolution, 00095 float max_angle_width, float max_angle_height, 00096 const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame, 00097 float noise_level, float min_range, int border_size) 00098 { 00099 createFromPointCloud (point_cloud, angular_resolution, angular_resolution, max_angle_width, max_angle_height, 00100 sensor_pose, coordinate_frame, noise_level, min_range, border_size); 00101 } 00102 00103 ///////////////////////////////////////////////////////////////////////// 00104 template <typename PointCloudType> void 00105 RangeImage::createFromPointCloud (const PointCloudType& point_cloud, 00106 float angular_resolution_x, float angular_resolution_y, 00107 float max_angle_width, float max_angle_height, 00108 const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame, 00109 float noise_level, float min_range, int border_size) 00110 { 00111 setAngularResolution (angular_resolution_x, angular_resolution_y); 00112 00113 width = static_cast<uint32_t> (pcl_lrint (floor (max_angle_width*angular_resolution_x_reciprocal_))); 00114 height = static_cast<uint32_t> (pcl_lrint (floor (max_angle_height*angular_resolution_y_reciprocal_))); 00115 00116 int full_width = static_cast<int> (pcl_lrint (floor (pcl::deg2rad (360.0f)*angular_resolution_x_reciprocal_))), 00117 full_height = static_cast<int> (pcl_lrint (floor (pcl::deg2rad (180.0f)*angular_resolution_y_reciprocal_))); 00118 image_offset_x_ = (full_width -static_cast<int> (width) )/2; 00119 image_offset_y_ = (full_height-static_cast<int> (height))/2; 00120 is_dense = false; 00121 00122 getCoordinateFrameTransformation (coordinate_frame, to_world_system_); 00123 to_world_system_ = sensor_pose * to_world_system_; 00124 00125 to_range_image_system_ = to_world_system_.inverse (Eigen::Isometry); 00126 //std::cout << "to_world_system_ is\n"<<to_world_system_<<"\nand to_range_image_system_ is\n"<<to_range_image_system_<<"\n\n"; 00127 00128 unsigned int size = width*height; 00129 points.clear (); 00130 points.resize (size, unobserved_point); 00131 00132 int top=height, right=-1, bottom=-1, left=width; 00133 doZBuffer (point_cloud, noise_level, min_range, top, right, bottom, left); 00134 00135 cropImage (border_size, top, right, bottom, left); 00136 00137 recalculate3DPointPositions (); 00138 } 00139 00140 ///////////////////////////////////////////////////////////////////////// 00141 template <typename PointCloudType> void 00142 RangeImage::createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, float angular_resolution, 00143 const Eigen::Vector3f& point_cloud_center, float point_cloud_radius, 00144 const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame, 00145 float noise_level, float min_range, int border_size) 00146 { 00147 createFromPointCloudWithKnownSize (point_cloud, angular_resolution, angular_resolution, point_cloud_center, point_cloud_radius, 00148 sensor_pose, coordinate_frame, noise_level, min_range, border_size); 00149 } 00150 00151 ///////////////////////////////////////////////////////////////////////// 00152 template <typename PointCloudType> void 00153 RangeImage::createFromPointCloudWithKnownSize (const PointCloudType& point_cloud, 00154 float angular_resolution_x, float angular_resolution_y, 00155 const Eigen::Vector3f& point_cloud_center, float point_cloud_radius, 00156 const Eigen::Affine3f& sensor_pose, RangeImage::CoordinateFrame coordinate_frame, 00157 float noise_level, float min_range, int border_size) 00158 { 00159 //MEASURE_FUNCTION_TIME; 00160 00161 //std::cout << "Starting to create range image from "<<point_cloud.points.size ()<<" points.\n"; 00162 00163 // If the sensor pose is inside of the sphere we have to calculate the image the normal way 00164 if ((point_cloud_center-sensor_pose.translation()).norm() <= point_cloud_radius) { 00165 createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y, 00166 pcl::deg2rad (360.0f), pcl::deg2rad (180.0f), 00167 sensor_pose, coordinate_frame, noise_level, min_range, border_size); 00168 return; 00169 } 00170 00171 setAngularResolution (angular_resolution_x, angular_resolution_y); 00172 00173 getCoordinateFrameTransformation (coordinate_frame, to_world_system_); 00174 to_world_system_ = sensor_pose * to_world_system_; 00175 to_range_image_system_ = to_world_system_.inverse (Eigen::Isometry); 00176 00177 float max_angle_size = getMaxAngleSize (sensor_pose, point_cloud_center, point_cloud_radius); 00178 int pixel_radius_x = pcl_lrint (ceil (0.5f*max_angle_size*angular_resolution_x_reciprocal_)), 00179 pixel_radius_y = pcl_lrint (ceil (0.5f*max_angle_size*angular_resolution_y_reciprocal_)); 00180 width = 2*pixel_radius_x; 00181 height = 2*pixel_radius_y; 00182 is_dense = false; 00183 00184 image_offset_x_ = image_offset_y_ = 0; // temporary values for getImagePoint 00185 int center_pixel_x, center_pixel_y; 00186 getImagePoint (point_cloud_center, center_pixel_x, center_pixel_y); 00187 image_offset_x_ = (std::max) (0, center_pixel_x-pixel_radius_x); 00188 image_offset_y_ = (std::max) (0, center_pixel_y-pixel_radius_y); 00189 00190 points.clear (); 00191 points.resize (width*height, unobserved_point); 00192 00193 int top=height, right=-1, bottom=-1, left=width; 00194 doZBuffer (point_cloud, noise_level, min_range, top, right, bottom, left); 00195 00196 cropImage (border_size, top, right, bottom, left); 00197 00198 recalculate3DPointPositions (); 00199 } 00200 00201 ///////////////////////////////////////////////////////////////////////// 00202 template <typename PointCloudTypeWithViewpoints> void 00203 RangeImage::createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud, 00204 float angular_resolution, 00205 float max_angle_width, float max_angle_height, 00206 RangeImage::CoordinateFrame coordinate_frame, 00207 float noise_level, float min_range, int border_size) 00208 { 00209 createFromPointCloudWithViewpoints (point_cloud, angular_resolution, angular_resolution, 00210 max_angle_width, max_angle_height, coordinate_frame, 00211 noise_level, min_range, border_size); 00212 } 00213 00214 ///////////////////////////////////////////////////////////////////////// 00215 template <typename PointCloudTypeWithViewpoints> void 00216 RangeImage::createFromPointCloudWithViewpoints (const PointCloudTypeWithViewpoints& point_cloud, 00217 float angular_resolution_x, float angular_resolution_y, 00218 float max_angle_width, float max_angle_height, 00219 RangeImage::CoordinateFrame coordinate_frame, 00220 float noise_level, float min_range, int border_size) 00221 { 00222 Eigen::Vector3f average_viewpoint = getAverageViewPoint (point_cloud); 00223 Eigen::Affine3f sensor_pose = static_cast<Eigen::Affine3f> (Eigen::Translation3f (average_viewpoint)); 00224 createFromPointCloud (point_cloud, angular_resolution_x, angular_resolution_y, max_angle_width, max_angle_height, 00225 sensor_pose, coordinate_frame, noise_level, min_range, border_size); 00226 } 00227 00228 ///////////////////////////////////////////////////////////////////////// 00229 template <typename PointCloudType> void 00230 RangeImage::doZBuffer (const PointCloudType& point_cloud, float noise_level, float min_range, int& top, int& right, int& bottom, int& left) 00231 { 00232 typedef typename PointCloudType::PointType PointType2; 00233 const typename pcl::PointCloud<PointType2>::VectorType &points2 = point_cloud.points; 00234 00235 unsigned int size = width*height; 00236 int* counters = new int[size]; 00237 ERASE_ARRAY (counters, size); 00238 00239 top=height; right=-1; bottom=-1; left=width; 00240 00241 float x_real, y_real, range_of_current_point; 00242 int x, y; 00243 for (typename pcl::PointCloud<PointType2>::VectorType::const_iterator it=points2.begin (); it!=points2.end (); ++it) 00244 { 00245 if (!isFinite (*it)) // Check for NAN etc 00246 continue; 00247 Vector3fMapConst current_point = it->getVector3fMap (); 00248 00249 this->getImagePoint (current_point, x_real, y_real, range_of_current_point); 00250 this->real2DToInt2D (x_real, y_real, x, y); 00251 00252 if (range_of_current_point < min_range|| !isInImage (x, y)) 00253 continue; 00254 //std::cout << " ("<<current_point[0]<<", "<<current_point[1]<<", "<<current_point[2]<<") falls into pixel "<<x<<","<<y<<".\n"; 00255 00256 // Do some minor interpolation by checking the three closest neighbors to the point, that are not filled yet. 00257 int floor_x = pcl_lrint (floor (x_real)), floor_y = pcl_lrint (floor (y_real)), 00258 ceil_x = pcl_lrint (ceil (x_real)), ceil_y = pcl_lrint (ceil (y_real)); 00259 00260 int neighbor_x[4], neighbor_y[4]; 00261 neighbor_x[0]=floor_x; neighbor_y[0]=floor_y; 00262 neighbor_x[1]=floor_x; neighbor_y[1]=ceil_y; 00263 neighbor_x[2]=ceil_x; neighbor_y[2]=floor_y; 00264 neighbor_x[3]=ceil_x; neighbor_y[3]=ceil_y; 00265 //std::cout << x_real<<","<<y_real<<": "; 00266 00267 for (int i=0; i<4; ++i) 00268 { 00269 int n_x=neighbor_x[i], n_y=neighbor_y[i]; 00270 //std::cout << n_x<<","<<n_y<<" "; 00271 if (n_x==x && n_y==y) 00272 continue; 00273 if (isInImage (n_x, n_y)) 00274 { 00275 int neighbor_array_pos = n_y*width + n_x; 00276 if (counters[neighbor_array_pos]==0) 00277 { 00278 float& neighbor_range = points[neighbor_array_pos].range; 00279 neighbor_range = (pcl_isinf (neighbor_range) ? range_of_current_point : (std::min) (neighbor_range, range_of_current_point)); 00280 top= (std::min) (top, n_y); right= (std::max) (right, n_x); bottom= (std::max) (bottom, n_y); left= (std::min) (left, n_x); 00281 } 00282 } 00283 } 00284 //std::cout <<std::endl; 00285 00286 // The point itself 00287 int arrayPos = y*width + x; 00288 float& range_at_image_point = points[arrayPos].range; 00289 int& counter = counters[arrayPos]; 00290 bool addCurrentPoint=false, replace_with_current_point=false; 00291 00292 if (counter==0) 00293 { 00294 replace_with_current_point = true; 00295 } 00296 else 00297 { 00298 if (range_of_current_point < range_at_image_point-noise_level) 00299 { 00300 replace_with_current_point = true; 00301 } 00302 else if (fabs (range_of_current_point-range_at_image_point)<=noise_level) 00303 { 00304 addCurrentPoint = true; 00305 } 00306 } 00307 00308 if (replace_with_current_point) 00309 { 00310 counter = 1; 00311 range_at_image_point = range_of_current_point; 00312 top= (std::min) (top, y); right= (std::max) (right, x); bottom= (std::max) (bottom, y); left= (std::min) (left, x); 00313 //std::cout << "Adding point "<<x<<","<<y<<"\n"; 00314 } 00315 else if (addCurrentPoint) 00316 { 00317 ++counter; 00318 range_at_image_point += (range_of_current_point-range_at_image_point)/counter; 00319 } 00320 } 00321 00322 delete[] counters; 00323 } 00324 00325 ///////////////////////////////////////////////////////////////////////// 00326 void 00327 RangeImage::getImagePoint (float x, float y, float z, float& image_x, float& image_y, float& range) const 00328 { 00329 Eigen::Vector3f point (x, y, z); 00330 getImagePoint (point, image_x, image_y, range); 00331 } 00332 00333 ///////////////////////////////////////////////////////////////////////// 00334 void 00335 RangeImage::getImagePoint (float x, float y, float z, float& image_x, float& image_y) const 00336 { 00337 float range; 00338 getImagePoint (x, y, z, image_x, image_y, range); 00339 } 00340 00341 ///////////////////////////////////////////////////////////////////////// 00342 void 00343 RangeImage::getImagePoint (float x, float y, float z, int& image_x, int& image_y) const 00344 { 00345 float image_x_float, image_y_float; 00346 getImagePoint (x, y, z, image_x_float, image_y_float); 00347 real2DToInt2D (image_x_float, image_y_float, image_x, image_y); 00348 } 00349 00350 ///////////////////////////////////////////////////////////////////////// 00351 void 00352 RangeImage::getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y, float& range) const 00353 { 00354 Eigen::Vector3f transformedPoint = to_range_image_system_ * point; 00355 range = transformedPoint.norm (); 00356 float angle_x = atan2LookUp (transformedPoint[0], transformedPoint[2]), 00357 angle_y = asinLookUp (transformedPoint[1]/range); 00358 getImagePointFromAngles (angle_x, angle_y, image_x, image_y); 00359 //std::cout << " ("<<point[0]<<","<<point[1]<<","<<point[2]<<")" 00360 //<< " => ("<<transformedPoint[0]<<","<<transformedPoint[1]<<","<<transformedPoint[2]<<")" 00361 //<< " => "<<angle_x<<","<<angle_y<<" => "<<image_x<<","<<image_y<<"\n"; 00362 } 00363 00364 ///////////////////////////////////////////////////////////////////////// 00365 void 00366 RangeImage::getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y, float& range) const { 00367 float image_x_float, image_y_float; 00368 getImagePoint (point, image_x_float, image_y_float, range); 00369 real2DToInt2D (image_x_float, image_y_float, image_x, image_y); 00370 } 00371 00372 ///////////////////////////////////////////////////////////////////////// 00373 void 00374 RangeImage::getImagePoint (const Eigen::Vector3f& point, float& image_x, float& image_y) const 00375 { 00376 float range; 00377 getImagePoint (point, image_x, image_y, range); 00378 } 00379 00380 ///////////////////////////////////////////////////////////////////////// 00381 void 00382 RangeImage::getImagePoint (const Eigen::Vector3f& point, int& image_x, int& image_y) const 00383 { 00384 float image_x_float, image_y_float; 00385 getImagePoint (point, image_x_float, image_y_float); 00386 real2DToInt2D (image_x_float, image_y_float, image_x, image_y); 00387 } 00388 00389 ///////////////////////////////////////////////////////////////////////// 00390 float 00391 RangeImage::checkPoint (const Eigen::Vector3f& point, PointWithRange& point_in_image) const 00392 { 00393 int image_x, image_y; 00394 float range; 00395 getImagePoint (point, image_x, image_y, range); 00396 if (!isInImage (image_x, image_y)) 00397 point_in_image = unobserved_point; 00398 else 00399 point_in_image = getPoint (image_x, image_y); 00400 return range; 00401 } 00402 00403 ///////////////////////////////////////////////////////////////////////// 00404 float 00405 RangeImage::getRangeDifference (const Eigen::Vector3f& point) const 00406 { 00407 int image_x, image_y; 00408 float range; 00409 getImagePoint (point, image_x, image_y, range); 00410 if (!isInImage (image_x, image_y)) 00411 return -std::numeric_limits<float>::infinity (); 00412 float image_point_range = getPoint (image_x, image_y).range; 00413 if (pcl_isinf (image_point_range)) 00414 { 00415 if (image_point_range > 0.0f) 00416 return std::numeric_limits<float>::infinity (); 00417 else 00418 return -std::numeric_limits<float>::infinity (); 00419 } 00420 return image_point_range - range; 00421 } 00422 00423 ///////////////////////////////////////////////////////////////////////// 00424 void 00425 RangeImage::getImagePointFromAngles (float angle_x, float angle_y, float& image_x, float& image_y) const 00426 { 00427 image_x = (angle_x*cosLookUp (angle_y) + static_cast<float> (M_PI))*angular_resolution_x_reciprocal_ - static_cast<float> (image_offset_x_); 00428 image_y = (angle_y + 0.5f*static_cast<float> (M_PI))*angular_resolution_y_reciprocal_ - static_cast<float> (image_offset_y_); 00429 } 00430 00431 ///////////////////////////////////////////////////////////////////////// 00432 void 00433 RangeImage::real2DToInt2D (float x, float y, int& xInt, int& yInt) const 00434 { 00435 xInt = static_cast<int> (pcl_lrintf (x)); 00436 yInt = static_cast<int> (pcl_lrintf (y)); 00437 } 00438 00439 ///////////////////////////////////////////////////////////////////////// 00440 bool 00441 RangeImage::isInImage (int x, int y) const 00442 { 00443 return (x >= 0 && x < static_cast<int> (width) && y >= 0 && y < static_cast<int> (height)); 00444 } 00445 00446 ///////////////////////////////////////////////////////////////////////// 00447 bool 00448 RangeImage::isValid (int x, int y) const 00449 { 00450 return isInImage (x,y) && pcl_isfinite (getPoint (x,y).range); 00451 } 00452 00453 ///////////////////////////////////////////////////////////////////////// 00454 bool 00455 RangeImage::isValid (int index) const 00456 { 00457 return pcl_isfinite (getPoint (index).range); 00458 } 00459 00460 ///////////////////////////////////////////////////////////////////////// 00461 bool 00462 RangeImage::isObserved (int x, int y) const 00463 { 00464 if (!isInImage (x,y) || (pcl_isinf (getPoint (x,y).range)&&getPoint (x,y).range<0.0f)) 00465 return false; 00466 return true; 00467 } 00468 00469 ///////////////////////////////////////////////////////////////////////// 00470 bool 00471 RangeImage::isMaxRange (int x, int y) const 00472 { 00473 float range = getPoint (x,y).range; 00474 return pcl_isinf (range) && range>0.0f; 00475 } 00476 00477 ///////////////////////////////////////////////////////////////////////// 00478 const PointWithRange& 00479 RangeImage::getPoint (int image_x, int image_y) const 00480 { 00481 if (!isInImage (image_x, image_y)) 00482 return unobserved_point; 00483 return points[image_y*width + image_x]; 00484 } 00485 00486 ///////////////////////////////////////////////////////////////////////// 00487 const PointWithRange& 00488 RangeImage::getPointNoCheck (int image_x, int image_y) const 00489 { 00490 return points[image_y*width + image_x]; 00491 } 00492 00493 ///////////////////////////////////////////////////////////////////////// 00494 PointWithRange& 00495 RangeImage::getPointNoCheck (int image_x, int image_y) 00496 { 00497 return points[image_y*width + image_x]; 00498 } 00499 00500 ///////////////////////////////////////////////////////////////////////// 00501 PointWithRange& 00502 RangeImage::getPoint (int image_x, int image_y) 00503 { 00504 return points[image_y*width + image_x]; 00505 } 00506 00507 00508 ///////////////////////////////////////////////////////////////////////// 00509 const PointWithRange& 00510 RangeImage::getPoint (int index) const 00511 { 00512 return points[index]; 00513 } 00514 00515 ///////////////////////////////////////////////////////////////////////// 00516 const PointWithRange& 00517 RangeImage::getPoint (float image_x, float image_y) const 00518 { 00519 int x, y; 00520 real2DToInt2D (image_x, image_y, x, y); 00521 return getPoint (x, y); 00522 } 00523 00524 ///////////////////////////////////////////////////////////////////////// 00525 PointWithRange& 00526 RangeImage::getPoint (float image_x, float image_y) 00527 { 00528 int x, y; 00529 real2DToInt2D (image_x, image_y, x, y); 00530 return getPoint (x, y); 00531 } 00532 00533 ///////////////////////////////////////////////////////////////////////// 00534 void 00535 RangeImage::getPoint (int image_x, int image_y, Eigen::Vector3f& point) const 00536 { 00537 //std::cout << getPoint (image_x, image_y)<< " - " << getPoint (image_x, image_y).getVector3fMap ()<<"\n"; 00538 point = getPoint (image_x, image_y).getVector3fMap (); 00539 } 00540 00541 ///////////////////////////////////////////////////////////////////////// 00542 void 00543 RangeImage::getPoint (int index, Eigen::Vector3f& point) const 00544 { 00545 point = getPoint (index).getVector3fMap (); 00546 } 00547 00548 ///////////////////////////////////////////////////////////////////////// 00549 const Eigen::Map<const Eigen::Vector3f> 00550 RangeImage::getEigenVector3f (int x, int y) const 00551 { 00552 return getPoint (x, y).getVector3fMap (); 00553 } 00554 00555 ///////////////////////////////////////////////////////////////////////// 00556 const Eigen::Map<const Eigen::Vector3f> 00557 RangeImage::getEigenVector3f (int index) const 00558 { 00559 return getPoint (index).getVector3fMap (); 00560 } 00561 00562 ///////////////////////////////////////////////////////////////////////// 00563 void 00564 RangeImage::calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const 00565 { 00566 float angle_x, angle_y; 00567 //std::cout << image_x<<","<<image_y<<","<<range; 00568 getAnglesFromImagePoint (image_x, image_y, angle_x, angle_y); 00569 00570 float cosY = cosf (angle_y); 00571 point = Eigen::Vector3f (range * sinf (angle_x) * cosY, range * sinf (angle_y), range * cosf (angle_x)*cosY); 00572 point = to_world_system_ * point; 00573 } 00574 00575 ///////////////////////////////////////////////////////////////////////// 00576 void 00577 RangeImage::calculate3DPoint (float image_x, float image_y, Eigen::Vector3f& point) const 00578 { 00579 const PointWithRange& point_in_image = getPoint (image_x, image_y); 00580 calculate3DPoint (image_x, image_y, point_in_image.range, point); 00581 } 00582 00583 ///////////////////////////////////////////////////////////////////////// 00584 void 00585 RangeImage::calculate3DPoint (float image_x, float image_y, float range, PointWithRange& point) const { 00586 point.range = range; 00587 Eigen::Vector3f tmp_point; 00588 calculate3DPoint (image_x, image_y, range, tmp_point); 00589 point.x=tmp_point[0]; point.y=tmp_point[1]; point.z=tmp_point[2]; 00590 } 00591 00592 ///////////////////////////////////////////////////////////////////////// 00593 void 00594 RangeImage::calculate3DPoint (float image_x, float image_y, PointWithRange& point) const 00595 { 00596 const PointWithRange& point_in_image = getPoint (image_x, image_y); 00597 calculate3DPoint (image_x, image_y, point_in_image.range, point); 00598 } 00599 00600 ///////////////////////////////////////////////////////////////////////// 00601 void 00602 RangeImage::getAnglesFromImagePoint (float image_x, float image_y, float& angle_x, float& angle_y) const 00603 { 00604 angle_y = (image_y+static_cast<float> (image_offset_y_))*angular_resolution_y_ - 0.5f*static_cast<float> (M_PI); 00605 float cos_angle_y = cosf (angle_y); 00606 angle_x = (cos_angle_y==0.0f ? 0.0f : ( (image_x+ static_cast<float> (image_offset_x_))*angular_resolution_x_ - static_cast<float> (M_PI))/cos_angle_y); 00607 } 00608 00609 ///////////////////////////////////////////////////////////////////////// 00610 float 00611 RangeImage::getImpactAngle (int x1, int y1, int x2, int y2) const 00612 { 00613 if (!isInImage (x1, y1) || !isInImage (x2,y2)) 00614 return -std::numeric_limits<float>::infinity (); 00615 return getImpactAngle (getPoint (x1,y1),getPoint (x2,y2)); 00616 } 00617 00618 ///////////////////////////////////////////////////////////////////////// 00619 float 00620 RangeImage::getImpactAngle (const PointWithRange& point1, const PointWithRange& point2) const { 00621 if ( (pcl_isinf (point1.range)&&point1.range<0) || (pcl_isinf (point2.range)&&point2.range<0)) 00622 return -std::numeric_limits<float>::infinity (); 00623 00624 float r1 = (std::min) (point1.range, point2.range), 00625 r2 = (std::max) (point1.range, point2.range); 00626 float impact_angle = static_cast<float> (0.5f * M_PI); 00627 00628 if (pcl_isinf (r2)) 00629 { 00630 if (r2 > 0.0f && !pcl_isinf (r1)) 00631 impact_angle = 0.0f; 00632 } 00633 else if (!pcl_isinf (r1)) 00634 { 00635 float r1Sqr = r1*r1, 00636 r2Sqr = r2*r2, 00637 dSqr = squaredEuclideanDistance (point1, point2), 00638 d = sqrtf (dSqr); 00639 float cos_impact_angle = (r2Sqr + dSqr - r1Sqr)/ (2.0f*r2*d); 00640 cos_impact_angle = (std::max) (0.0f, (std::min) (1.0f, cos_impact_angle)); 00641 impact_angle = acosf (cos_impact_angle); // Using the cosine rule 00642 } 00643 00644 if (point1.range > point2.range) 00645 impact_angle = -impact_angle; 00646 00647 return impact_angle; 00648 } 00649 00650 ///////////////////////////////////////////////////////////////////////// 00651 float 00652 RangeImage::getAcutenessValue (const PointWithRange& point1, const PointWithRange& point2) const 00653 { 00654 float impact_angle = getImpactAngle (point1, point2); 00655 if (pcl_isinf (impact_angle)) 00656 return -std::numeric_limits<float>::infinity (); 00657 float ret = 1.0f - float (fabs (impact_angle)/ (0.5f*M_PI)); 00658 if (impact_angle < 0.0f) 00659 ret = -ret; 00660 //if (fabs (ret)>1) 00661 //std::cout << PVARAC (impact_angle)<<PVARN (ret); 00662 return ret; 00663 } 00664 00665 ///////////////////////////////////////////////////////////////////////// 00666 float 00667 RangeImage::getAcutenessValue (int x1, int y1, int x2, int y2) const 00668 { 00669 if (!isInImage (x1, y1) || !isInImage (x2,y2)) 00670 return -std::numeric_limits<float>::infinity (); 00671 return getAcutenessValue (getPoint (x1,y1), getPoint (x2,y2)); 00672 } 00673 00674 ///////////////////////////////////////////////////////////////////////// 00675 const Eigen::Vector3f 00676 RangeImage::getSensorPos () const 00677 { 00678 return Eigen::Vector3f (to_world_system_ (0,3), to_world_system_ (1,3), to_world_system_ (2,3)); 00679 } 00680 00681 ///////////////////////////////////////////////////////////////////////// 00682 void 00683 RangeImage::getSurfaceAngleChange (int x, int y, int radius, float& angle_change_x, float& angle_change_y) const 00684 { 00685 angle_change_x = angle_change_y = -std::numeric_limits<float>::infinity (); 00686 if (!isValid (x,y)) 00687 return; 00688 Eigen::Vector3f point; 00689 getPoint (x, y, point); 00690 Eigen::Affine3f transformation = getTransformationToViewerCoordinateFrame (point); 00691 00692 if (isObserved (x-radius, y) && isObserved (x+radius, y)) 00693 { 00694 Eigen::Vector3f transformed_left; 00695 if (isMaxRange (x-radius, y)) 00696 transformed_left = Eigen::Vector3f (0.0f, 0.0f, -1.0f); 00697 else 00698 { 00699 Eigen::Vector3f left; 00700 getPoint (x-radius, y, left); 00701 transformed_left = - (transformation * left); 00702 //std::cout << PVARN (transformed_left[1]); 00703 transformed_left[1] = 0.0f; 00704 transformed_left.normalize (); 00705 } 00706 00707 Eigen::Vector3f transformed_right; 00708 if (isMaxRange (x+radius, y)) 00709 transformed_right = Eigen::Vector3f (0.0f, 0.0f, 1.0f); 00710 else 00711 { 00712 Eigen::Vector3f right; 00713 getPoint (x+radius, y, right); 00714 transformed_right = transformation * right; 00715 //std::cout << PVARN (transformed_right[1]); 00716 transformed_right[1] = 0.0f; 00717 transformed_right.normalize (); 00718 } 00719 angle_change_x = transformed_left.dot (transformed_right); 00720 angle_change_x = (std::max) (0.0f, (std::min) (1.0f, angle_change_x)); 00721 angle_change_x = acosf (angle_change_x); 00722 } 00723 00724 if (isObserved (x, y-radius) && isObserved (x, y+radius)) 00725 { 00726 Eigen::Vector3f transformed_top; 00727 if (isMaxRange (x, y-radius)) 00728 transformed_top = Eigen::Vector3f (0.0f, 0.0f, -1.0f); 00729 else 00730 { 00731 Eigen::Vector3f top; 00732 getPoint (x, y-radius, top); 00733 transformed_top = - (transformation * top); 00734 //std::cout << PVARN (transformed_top[0]); 00735 transformed_top[0] = 0.0f; 00736 transformed_top.normalize (); 00737 } 00738 00739 Eigen::Vector3f transformed_bottom; 00740 if (isMaxRange (x, y+radius)) 00741 transformed_bottom = Eigen::Vector3f (0.0f, 0.0f, 1.0f); 00742 else 00743 { 00744 Eigen::Vector3f bottom; 00745 getPoint (x, y+radius, bottom); 00746 transformed_bottom = transformation * bottom; 00747 //std::cout << PVARN (transformed_bottom[0]); 00748 transformed_bottom[0] = 0.0f; 00749 transformed_bottom.normalize (); 00750 } 00751 angle_change_y = transformed_top.dot (transformed_bottom); 00752 angle_change_y = (std::max) (0.0f, (std::min) (1.0f, angle_change_y)); 00753 angle_change_y = acosf (angle_change_y); 00754 } 00755 } 00756 00757 00758 //inline float RangeImage::getSurfaceChange (const PointWithRange& point, const PointWithRange& neighbor1, const PointWithRange& neighbor2) const 00759 //{ 00760 //if (!pcl_isfinite (point.range) || (!pcl_isfinite (neighbor1.range)&&neighbor1.range<0) || (!pcl_isfinite (neighbor2.range)&&neighbor2.range<0)) 00761 //return -std::numeric_limits<float>::infinity (); 00762 //if (pcl_isinf (neighbor1.range)) 00763 //{ 00764 //if (pcl_isinf (neighbor2.range)) 00765 //return 0.0f; 00766 //else 00767 //return acosf ( (Eigen::Vector3f (point.x, point.y, point.z)-getSensorPos ()).normalized ().dot ( (Eigen::Vector3f (neighbor2.x, neighbor2.y, neighbor2.z)-Eigen::Vector3f (point.x, point.y, point.z)).normalized ())); 00768 //} 00769 //if (pcl_isinf (neighbor2.range)) 00770 //return acosf ( (Eigen::Vector3f (point.x, point.y, point.z)-getSensorPos ()).normalized ().dot ( (Eigen::Vector3f (neighbor1.x, neighbor1.y, neighbor1.z)-Eigen::Vector3f (point.x, point.y, point.z)).normalized ())); 00771 00772 //float d1_squared = squaredEuclideanDistance (point, neighbor1), 00773 //d1 = sqrtf (d1_squared), 00774 //d2_squared = squaredEuclideanDistance (point, neighbor2), 00775 //d2 = sqrtf (d2_squared), 00776 //d3_squared = squaredEuclideanDistance (neighbor1, neighbor2); 00777 //float cos_surface_change = (d1_squared + d2_squared - d3_squared)/ (2.0f*d1*d2), 00778 //surface_change = acosf (cos_surface_change); 00779 //if (pcl_isnan (surface_change)) 00780 //surface_change = static_cast<float> (M_PI); 00781 ////std::cout << PVARN (point)<<PVARN (neighbor1)<<PVARN (neighbor2)<<PVARN (cos_surface_change)<<PVARN (surface_change)<<PVARN (d1)<<PVARN (d2)<<PVARN (d1_squared)<<PVARN (d2_squared)<<PVARN (d3_squared); 00782 00783 //return surface_change; 00784 //} 00785 00786 ///////////////////////////////////////////////////////////////////////// 00787 float 00788 RangeImage::getMaxAngleSize (const Eigen::Affine3f& viewer_pose, const Eigen::Vector3f& center, float radius) 00789 { 00790 return 2.0f * asinf (radius/ (viewer_pose.translation ()-center).norm ()); 00791 } 00792 00793 ///////////////////////////////////////////////////////////////////////// 00794 Eigen::Vector3f 00795 RangeImage::getEigenVector3f (const PointWithRange& point) 00796 { 00797 return Eigen::Vector3f (point.x, point.y, point.z); 00798 } 00799 00800 ///////////////////////////////////////////////////////////////////////// 00801 void 00802 RangeImage::get1dPointAverage (int x, int y, int delta_x, int delta_y, int no_of_points, PointWithRange& average_point) const 00803 { 00804 //std::cout << __PRETTY_FUNCTION__<<" called.\n"; 00805 //MEASURE_FUNCTION_TIME; 00806 float weight_sum = 1.0f; 00807 average_point = getPoint (x,y); 00808 if (pcl_isinf (average_point.range)) 00809 { 00810 if (average_point.range>0.0f) // The first point is max range -> return a max range point 00811 return; 00812 weight_sum = 0.0f; 00813 average_point.x = average_point.y = average_point.z = average_point.range = 0.0f; 00814 } 00815 00816 int x2=x, y2=y; 00817 Vector4fMap average_point_eigen = average_point.getVector4fMap (); 00818 //std::cout << PVARN (no_of_points); 00819 for (int step=1; step<no_of_points; ++step) 00820 { 00821 //std::cout << PVARC (step); 00822 x2+=delta_x; y2+=delta_y; 00823 if (!isValid (x2, y2)) 00824 continue; 00825 const PointWithRange& p = getPointNoCheck (x2, y2); 00826 average_point_eigen+=p.getVector4fMap (); average_point.range+=p.range; 00827 weight_sum += 1.0f; 00828 } 00829 if (weight_sum<= 0.0f) 00830 { 00831 average_point = unobserved_point; 00832 return; 00833 } 00834 float normalization_factor = 1.0f/weight_sum; 00835 average_point_eigen *= normalization_factor; 00836 average_point.range *= normalization_factor; 00837 //std::cout << PVARN (average_point); 00838 } 00839 00840 ///////////////////////////////////////////////////////////////////////// 00841 float 00842 RangeImage::getEuclideanDistanceSquared (int x1, int y1, int x2, int y2) const 00843 { 00844 if (!isObserved (x1,y1)||!isObserved (x2,y2)) 00845 return -std::numeric_limits<float>::infinity (); 00846 const PointWithRange& point1 = getPoint (x1,y1), 00847 & point2 = getPoint (x2,y2); 00848 if (pcl_isinf (point1.range) && pcl_isinf (point2.range)) 00849 return 0.0f; 00850 if (pcl_isinf (point1.range) || pcl_isinf (point2.range)) 00851 return std::numeric_limits<float>::infinity (); 00852 return squaredEuclideanDistance (point1, point2); 00853 } 00854 00855 ///////////////////////////////////////////////////////////////////////// 00856 float 00857 RangeImage::getAverageEuclideanDistance (int x, int y, int offset_x, int offset_y, int max_steps) const 00858 { 00859 float average_pixel_distance = 0.0f; 00860 float weight=0.0f; 00861 for (int i=0; i<max_steps; ++i) 00862 { 00863 int x1=x+i*offset_x, y1=y+i*offset_y; 00864 int x2=x+ (i+1)*offset_x, y2=y+ (i+1)*offset_y; 00865 float pixel_distance = getEuclideanDistanceSquared (x1,y1,x2,y2); 00866 if (!pcl_isfinite (pixel_distance)) 00867 { 00868 //std::cout << x<<","<<y<<"->"<<x2<<","<<y2<<": "<<pixel_distance<<"\n"; 00869 if (i==0) 00870 return pixel_distance; 00871 else 00872 break; 00873 } 00874 //std::cout << x<<","<<y<<"->"<<x2<<","<<y2<<": "<<sqrtf (pixel_distance)<<"m\n"; 00875 weight += 1.0f; 00876 average_pixel_distance += sqrtf (pixel_distance); 00877 } 00878 average_pixel_distance /= weight; 00879 //std::cout << x<<","<<y<<","<<offset_x<<","<<offset_y<<" => "<<average_pixel_distance<<"\n"; 00880 return average_pixel_distance; 00881 } 00882 00883 ///////////////////////////////////////////////////////////////////////// 00884 float 00885 RangeImage::getImpactAngleBasedOnLocalNormal (int x, int y, int radius) const 00886 { 00887 if (!isValid (x,y)) 00888 return -std::numeric_limits<float>::infinity (); 00889 const PointWithRange& point = getPoint (x, y); 00890 int no_of_nearest_neighbors = static_cast<int> (pow (static_cast<double> ( (radius + 1.0)), 2.0)); 00891 Eigen::Vector3f normal; 00892 if (!getNormalForClosestNeighbors (x, y, radius, point, no_of_nearest_neighbors, normal, 1)) 00893 return -std::numeric_limits<float>::infinity (); 00894 return deg2rad (90.0f) - acosf (normal.dot ( (getSensorPos ()-getEigenVector3f (point)).normalized ())); 00895 } 00896 00897 00898 ///////////////////////////////////////////////////////////////////////// 00899 bool 00900 RangeImage::getNormal (int x, int y, int radius, Eigen::Vector3f& normal, int step_size) const 00901 { 00902 VectorAverage3f vector_average; 00903 for (int y2=y-radius; y2<=y+radius; y2+=step_size) 00904 { 00905 for (int x2=x-radius; x2<=x+radius; x2+=step_size) 00906 { 00907 if (!isInImage (x2, y2)) 00908 continue; 00909 const PointWithRange& point = getPoint (x2, y2); 00910 if (!pcl_isfinite (point.range)) 00911 continue; 00912 vector_average.add (Eigen::Vector3f (point.x, point.y, point.z)); 00913 } 00914 } 00915 if (vector_average.getNoOfSamples () < 3) 00916 return false; 00917 Eigen::Vector3f eigen_values, eigen_vector2, eigen_vector3; 00918 vector_average.doPCA (eigen_values, normal, eigen_vector2, eigen_vector3); 00919 if (normal.dot ( (getSensorPos ()-vector_average.getMean ()).normalized ()) < 0.0f) 00920 normal *= -1.0f; 00921 return true; 00922 } 00923 00924 ///////////////////////////////////////////////////////////////////////// 00925 float 00926 RangeImage::getNormalBasedAcutenessValue (int x, int y, int radius) const 00927 { 00928 float impact_angle = getImpactAngleBasedOnLocalNormal (x, y, radius); 00929 if (pcl_isinf (impact_angle)) 00930 return -std::numeric_limits<float>::infinity (); 00931 float ret = 1.0f - static_cast<float> ( (impact_angle / (0.5f * M_PI))); 00932 //std::cout << PVARAC (impact_angle)<<PVARN (ret); 00933 return ret; 00934 } 00935 00936 ///////////////////////////////////////////////////////////////////////// 00937 bool 00938 RangeImage::getNormalForClosestNeighbors (int x, int y, int radius, const PointWithRange& point, 00939 int no_of_nearest_neighbors, Eigen::Vector3f& normal, int step_size) const 00940 { 00941 return getNormalForClosestNeighbors (x, y, radius, Eigen::Vector3f (point.x, point.y, point.z), no_of_nearest_neighbors, normal, NULL, step_size); 00942 } 00943 00944 ///////////////////////////////////////////////////////////////////////// 00945 bool 00946 RangeImage::getNormalForClosestNeighbors (int x, int y, Eigen::Vector3f& normal, int radius) const 00947 { 00948 if (!isValid (x,y)) 00949 return false; 00950 int no_of_nearest_neighbors = static_cast<int> (pow (static_cast<double> (radius + 1.0), 2.0)); 00951 return getNormalForClosestNeighbors (x, y, radius, getPoint (x,y).getVector3fMap (), no_of_nearest_neighbors, normal); 00952 } 00953 00954 namespace 00955 { // Anonymous namespace, so that this is only accessible in this file 00956 struct NeighborWithDistance 00957 { // local struct to help us with sorting 00958 float distance; 00959 const PointWithRange* neighbor; 00960 bool operator < (const NeighborWithDistance& other) const { return distance<other.distance;} 00961 }; 00962 } 00963 00964 ///////////////////////////////////////////////////////////////////////// 00965 bool 00966 RangeImage::getSurfaceInformation (int x, int y, int radius, const Eigen::Vector3f& point, int no_of_closest_neighbors, int step_size, 00967 float& max_closest_neighbor_distance_squared, 00968 Eigen::Vector3f& normal, Eigen::Vector3f& mean, Eigen::Vector3f& eigen_values, 00969 Eigen::Vector3f* normal_all_neighbors, Eigen::Vector3f* mean_all_neighbors, 00970 Eigen::Vector3f* eigen_values_all_neighbors) const 00971 { 00972 max_closest_neighbor_distance_squared=0.0f; 00973 normal.setZero (); mean.setZero (); eigen_values.setZero (); 00974 if (normal_all_neighbors!=NULL) 00975 normal_all_neighbors->setZero (); 00976 if (mean_all_neighbors!=NULL) 00977 mean_all_neighbors->setZero (); 00978 if (eigen_values_all_neighbors!=NULL) 00979 eigen_values_all_neighbors->setZero (); 00980 00981 int blocksize = static_cast<int> (pow (static_cast<double> ( (2.0 * radius + 1.0)), 2.0)); 00982 00983 PointWithRange given_point; 00984 given_point.x=point[0]; given_point.y=point[1]; given_point.z=point[2]; 00985 00986 std::vector<NeighborWithDistance> ordered_neighbors (blocksize); 00987 int neighbor_counter = 0; 00988 for (int y2=y-radius; y2<=y+radius; y2+=step_size) 00989 { 00990 for (int x2=x-radius; x2<=x+radius; x2+=step_size) 00991 { 00992 if (!isValid (x2, y2)) 00993 continue; 00994 NeighborWithDistance& neighbor_with_distance = ordered_neighbors[neighbor_counter]; 00995 neighbor_with_distance.neighbor = &getPoint (x2, y2); 00996 neighbor_with_distance.distance = squaredEuclideanDistance (given_point, *neighbor_with_distance.neighbor); 00997 ++neighbor_counter; 00998 } 00999 } 01000 no_of_closest_neighbors = (std::min) (neighbor_counter, no_of_closest_neighbors); 01001 01002 std::sort (ordered_neighbors.begin (), ordered_neighbors.begin () + neighbor_counter); // Normal sort seems to be the fastest method (faster than partial_sort) 01003 //std::stable_sort (ordered_neighbors, ordered_neighbors+neighbor_counter); 01004 //std::partial_sort (ordered_neighbors, ordered_neighbors+no_of_closest_neighbors, ordered_neighbors+neighbor_counter); 01005 01006 max_closest_neighbor_distance_squared = ordered_neighbors[no_of_closest_neighbors-1].distance; 01007 //float max_distance_squared = max_closest_neighbor_distance_squared; 01008 float max_distance_squared = max_closest_neighbor_distance_squared*4.0f; // Double the allowed distance value 01009 //max_closest_neighbor_distance_squared = max_distance_squared; 01010 01011 VectorAverage3f vector_average; 01012 //for (int neighbor_idx=0; neighbor_idx<no_of_closest_neighbors; ++neighbor_idx) 01013 int neighbor_idx; 01014 for (neighbor_idx=0; neighbor_idx<neighbor_counter; ++neighbor_idx) 01015 { 01016 if (ordered_neighbors[neighbor_idx].distance > max_distance_squared) 01017 break; 01018 //std::cout << ordered_neighbors[neighbor_idx].distance<<"\n"; 01019 vector_average.add (ordered_neighbors[neighbor_idx].neighbor->getVector3fMap ()); 01020 } 01021 01022 if (vector_average.getNoOfSamples () < 3) 01023 return false; 01024 //std::cout << PVARN (vector_average.getNoOfSamples ()); 01025 Eigen::Vector3f eigen_vector2, eigen_vector3; 01026 vector_average.doPCA (eigen_values, normal, eigen_vector2, eigen_vector3); 01027 Eigen::Vector3f viewing_direction = (getSensorPos ()-point).normalized (); 01028 if (normal.dot (viewing_direction) < 0.0f) 01029 normal *= -1.0f; 01030 mean = vector_average.getMean (); 01031 01032 if (normal_all_neighbors==NULL) 01033 return true; 01034 01035 // Add remaining neighbors 01036 for (int neighbor_idx2=neighbor_idx; neighbor_idx2<neighbor_counter; ++neighbor_idx2) 01037 vector_average.add (ordered_neighbors[neighbor_idx2].neighbor->getVector3fMap ()); 01038 01039 vector_average.doPCA (*eigen_values_all_neighbors, *normal_all_neighbors, eigen_vector2, eigen_vector3); 01040 //std::cout << PVARN (vector_average.getNoOfSamples ())<<".\n"; 01041 if (normal_all_neighbors->dot (viewing_direction) < 0.0f) 01042 *normal_all_neighbors *= -1.0f; 01043 *mean_all_neighbors = vector_average.getMean (); 01044 01045 //std::cout << viewing_direction[0]<<","<<viewing_direction[1]<<","<<viewing_direction[2]<<"\n"; 01046 01047 return true; 01048 } 01049 01050 ///////////////////////////////////////////////////////////////////////// 01051 float 01052 RangeImage::getSquaredDistanceOfNthNeighbor (int x, int y, int radius, int n, int step_size) const 01053 { 01054 const PointWithRange& point = getPoint (x, y); 01055 if (!pcl_isfinite (point.range)) 01056 return -std::numeric_limits<float>::infinity (); 01057 01058 int blocksize = static_cast<int> (pow (static_cast<double> (2.0 * radius + 1.0), 2.0)); 01059 std::vector<float> neighbor_distances (blocksize); 01060 int neighbor_counter = 0; 01061 for (int y2=y-radius; y2<=y+radius; y2+=step_size) 01062 { 01063 for (int x2=x-radius; x2<=x+radius; x2+=step_size) 01064 { 01065 if (!isValid (x2, y2) || (x2==x&&y2==y)) 01066 continue; 01067 const PointWithRange& neighbor = getPointNoCheck (x2,y2); 01068 float& neighbor_distance = neighbor_distances[neighbor_counter++]; 01069 neighbor_distance = squaredEuclideanDistance (point, neighbor); 01070 } 01071 } 01072 std::sort (neighbor_distances.begin (), neighbor_distances.begin () + neighbor_counter); // Normal sort seems to be 01073 // the fastest method (faster than partial_sort) 01074 n = (std::min) (neighbor_counter, n); 01075 return neighbor_distances[n-1]; 01076 } 01077 01078 01079 ///////////////////////////////////////////////////////////////////////// 01080 bool 01081 RangeImage::getNormalForClosestNeighbors (int x, int y, int radius, const Eigen::Vector3f& point, int no_of_nearest_neighbors, 01082 Eigen::Vector3f& normal, Eigen::Vector3f* point_on_plane, int step_size) const 01083 { 01084 Eigen::Vector3f mean, eigen_values; 01085 float used_squared_max_distance; 01086 bool ret = getSurfaceInformation (x, y, radius, point, no_of_nearest_neighbors, step_size, used_squared_max_distance, 01087 normal, mean, eigen_values); 01088 01089 if (ret) 01090 { 01091 if (point_on_plane != NULL) 01092 *point_on_plane = (normal.dot (mean) - normal.dot (point))*normal + point; 01093 } 01094 return ret; 01095 } 01096 01097 01098 ///////////////////////////////////////////////////////////////////////// 01099 float 01100 RangeImage::getCurvature (int x, int y, int radius, int step_size) const 01101 { 01102 VectorAverage3f vector_average; 01103 for (int y2=y-radius; y2<=y+radius; y2+=step_size) 01104 { 01105 for (int x2=x-radius; x2<=x+radius; x2+=step_size) 01106 { 01107 if (!isInImage (x2, y2)) 01108 continue; 01109 const PointWithRange& point = getPoint (x2, y2); 01110 if (!pcl_isfinite (point.range)) 01111 continue; 01112 vector_average.add (Eigen::Vector3f (point.x, point.y, point.z)); 01113 } 01114 } 01115 if (vector_average.getNoOfSamples () < 3) 01116 return false; 01117 Eigen::Vector3f eigen_values; 01118 vector_average.doPCA (eigen_values); 01119 return eigen_values[0]/eigen_values.sum (); 01120 } 01121 01122 01123 ///////////////////////////////////////////////////////////////////////// 01124 template <typename PointCloudTypeWithViewpoints> Eigen::Vector3f 01125 RangeImage::getAverageViewPoint (const PointCloudTypeWithViewpoints& point_cloud) 01126 { 01127 Eigen::Vector3f average_viewpoint (0,0,0); 01128 int point_counter = 0; 01129 for (unsigned int point_idx=0; point_idx<point_cloud.points.size (); ++point_idx) 01130 { 01131 const typename PointCloudTypeWithViewpoints::PointType& point = point_cloud.points[point_idx]; 01132 if (!pcl_isfinite (point.vp_x) || !pcl_isfinite (point.vp_y) || !pcl_isfinite (point.vp_z)) 01133 continue; 01134 average_viewpoint[0] += point.vp_x; 01135 average_viewpoint[1] += point.vp_y; 01136 average_viewpoint[2] += point.vp_z; 01137 ++point_counter; 01138 } 01139 average_viewpoint /= point_counter; 01140 01141 return average_viewpoint; 01142 } 01143 01144 ///////////////////////////////////////////////////////////////////////// 01145 bool 01146 RangeImage::getViewingDirection (int x, int y, Eigen::Vector3f& viewing_direction) const 01147 { 01148 if (!isValid (x, y)) 01149 return false; 01150 viewing_direction = (getPoint (x,y).getVector3fMap ()-getSensorPos ()).normalized (); 01151 return true; 01152 } 01153 01154 ///////////////////////////////////////////////////////////////////////// 01155 void 01156 RangeImage::getViewingDirection (const Eigen::Vector3f& point, Eigen::Vector3f& viewing_direction) const 01157 { 01158 viewing_direction = (point-getSensorPos ()).normalized (); 01159 } 01160 01161 ///////////////////////////////////////////////////////////////////////// 01162 Eigen::Affine3f 01163 RangeImage::getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point) const 01164 { 01165 Eigen::Affine3f transformation; 01166 getTransformationToViewerCoordinateFrame (point, transformation); 01167 return transformation; 01168 } 01169 01170 ///////////////////////////////////////////////////////////////////////// 01171 void 01172 RangeImage::getTransformationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const 01173 { 01174 Eigen::Vector3f viewing_direction = (point-getSensorPos ()).normalized (); 01175 getTransformationFromTwoUnitVectorsAndOrigin (Eigen::Vector3f (0.0f, -1.0f, 0.0f), viewing_direction, point, transformation); 01176 } 01177 01178 ///////////////////////////////////////////////////////////////////////// 01179 void 01180 RangeImage::getRotationToViewerCoordinateFrame (const Eigen::Vector3f& point, Eigen::Affine3f& transformation) const 01181 { 01182 Eigen::Vector3f viewing_direction = (point-getSensorPos ()).normalized (); 01183 getTransformationFromTwoUnitVectors (Eigen::Vector3f (0.0f, -1.0f, 0.0f), viewing_direction, transformation); 01184 } 01185 01186 ///////////////////////////////////////////////////////////////////////// 01187 inline void 01188 RangeImage::setAngularResolution (float angular_resolution) 01189 { 01190 angular_resolution_x_ = angular_resolution_y_ = angular_resolution; 01191 angular_resolution_x_reciprocal_ = angular_resolution_y_reciprocal_ = 1.0f / angular_resolution; 01192 } 01193 01194 ///////////////////////////////////////////////////////////////////////// 01195 inline void 01196 RangeImage::setAngularResolution (float angular_resolution_x, float angular_resolution_y) 01197 { 01198 angular_resolution_x_ = angular_resolution_x; 01199 angular_resolution_x_reciprocal_ = 1.0f / angular_resolution_x_; 01200 angular_resolution_y_ = angular_resolution_y; 01201 angular_resolution_y_reciprocal_ = 1.0f / angular_resolution_y_; 01202 } 01203 01204 ///////////////////////////////////////////////////////////////////////// 01205 inline void 01206 RangeImage::setTransformationToRangeImageSystem (const Eigen::Affine3f& to_range_image_system) 01207 { 01208 to_range_image_system_ = to_range_image_system; 01209 to_world_system_ = to_range_image_system_.inverse (); 01210 } 01211 01212 ///////////////////////////////////////////////////////////////////////// 01213 inline void 01214 RangeImage::getAngularResolution (float& angular_resolution_x, float& angular_resolution_y) const 01215 { 01216 angular_resolution_x = angular_resolution_x_; 01217 angular_resolution_y = angular_resolution_y_; 01218 } 01219 01220 ///////////////////////////////////////////////////////////////////////// 01221 template <typename PointCloudType> void 01222 RangeImage::integrateFarRanges (const PointCloudType& far_ranges) 01223 { 01224 float x_real, y_real, range_of_current_point; 01225 for (typename PointCloudType::const_iterator it = far_ranges.points.begin (); it != far_ranges.points.end (); ++it) 01226 { 01227 //if (!isFinite (*it)) // Check for NAN etc 01228 //continue; 01229 Vector3fMapConst current_point = it->getVector3fMap (); 01230 01231 this->getImagePoint (current_point, x_real, y_real, range_of_current_point); 01232 01233 int floor_x = static_cast<int> (pcl_lrint (floor (x_real))), 01234 floor_y = static_cast<int> (pcl_lrint (floor (y_real))), 01235 ceil_x = static_cast<int> (pcl_lrint (ceil (x_real))), 01236 ceil_y = static_cast<int> (pcl_lrint (ceil (y_real))); 01237 01238 int neighbor_x[4], neighbor_y[4]; 01239 neighbor_x[0]=floor_x; neighbor_y[0]=floor_y; 01240 neighbor_x[1]=floor_x; neighbor_y[1]=ceil_y; 01241 neighbor_x[2]=ceil_x; neighbor_y[2]=floor_y; 01242 neighbor_x[3]=ceil_x; neighbor_y[3]=ceil_y; 01243 01244 for (int i=0; i<4; ++i) 01245 { 01246 int x=neighbor_x[i], y=neighbor_y[i]; 01247 if (!isInImage (x, y)) 01248 continue; 01249 PointWithRange& image_point = getPoint (x, y); 01250 if (!pcl_isfinite (image_point.range)) 01251 image_point.range = std::numeric_limits<float>::infinity (); 01252 } 01253 } 01254 } 01255 01256 } // namespace end 01257 #endif 01258