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