Point Cloud Library (PCL)  1.3.1
range_image.hpp
Go to the documentation of this file.
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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines