range_image_planar.cpp
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 the copyright holder(s) 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 
00037 #include <iostream>
00038 using std::cout;
00039 using std::cerr;
00040 
00041 #include <pcl/range_image/range_image_planar.h>
00042 
00043 namespace pcl 
00044 {
00046   RangeImagePlanar::RangeImagePlanar () : RangeImage (), focal_length_x_ (0.0f), focal_length_y_ (0.0f),
00047                                           focal_length_x_reciprocal_ (0.0f), focal_length_y_reciprocal_ (0.0f),
00048                                           center_x_ (0.0f), center_y_ (0.0f)
00049   {
00050   }
00051 
00053   RangeImagePlanar::~RangeImagePlanar ()
00054   {
00055   }
00056 
00058   void
00059   RangeImagePlanar::setDisparityImage (const float* disparity_image, int di_width, int di_height,
00060                                        float focal_length, float base_line, float desired_angular_resolution)
00061   {
00062     //MEASURE_FUNCTION_TIME;
00063     reset ();
00064     
00065     float original_angular_resolution = atanf (0.5f * static_cast<float> (di_width) / static_cast<float> (focal_length)) / (0.5f * static_cast<float> (di_width));
00066     int skip = 1;
00067     if (desired_angular_resolution >= 2.0f*original_angular_resolution)
00068       skip = static_cast<int> (pcl_lrint (floor (desired_angular_resolution / original_angular_resolution)));
00069 
00070     setAngularResolution (original_angular_resolution * static_cast<float> (skip));
00071     width  = di_width / skip;
00072     height = di_height / skip;
00073     focal_length_x_ = focal_length_y_ = focal_length / static_cast<float> (skip);
00074     focal_length_x_reciprocal_ = focal_length_y_reciprocal_ = 1.0f / focal_length_x_;
00075     center_x_ = static_cast<float> (di_width)  / static_cast<float> (2 * skip);
00076     center_y_ = static_cast<float> (di_height) / static_cast<float> (2 * skip);
00077     points.resize (width*height);
00078     
00079     //cout << PVARN (*this);
00080     
00081     float normalization_factor = static_cast<float> (skip) * focal_length_x_ * base_line;
00082     for (int y=0; y < static_cast<int> (height); ++y)
00083     {
00084       for (int x=0; x < static_cast<int> (width); ++x)
00085       {
00086         PointWithRange& point = getPointNoCheck (x,y);
00087         float disparity = disparity_image[ (y*skip)*di_width + x*skip];
00088         if (disparity <= 0.0f)
00089         {
00090           //std::cout << disparity << ", "<<std::flush;
00091           point = unobserved_point;
00092           continue;
00093         }
00094         point.z = normalization_factor / disparity;
00095         point.x = (static_cast<float> (x) - center_x_) * point.z * focal_length_x_reciprocal_;
00096         point.y = (static_cast<float> (y) - center_y_) * point.z * focal_length_y_reciprocal_;
00097         point.range = point.getVector3fMap ().norm ();
00098         //cout << point<<std::flush;
00099         
00101         //PointWithRange test_point1;
00102         //calculate3DPoint (float (x), float (y), point.range, test_point1);
00103         //if ( (point.getVector3fMap ()-test_point1.getVector3fMap ()).norm () > 1e-5)
00104           //cerr << "Something's wrong here...\n";
00105         
00106         //float image_x, image_y;
00107         //getImagePoint (point.getVector3fMap (), image_x, image_y);
00108         //if (fabsf (image_x-x)+fabsf (image_y-y)>1e-4)
00109           //cerr << PVARC (x)<<PVARC (y)<<PVARC (image_x)<<PVARN (image_y);
00110       }
00111     }
00112   }
00113   
00115   void
00116   RangeImagePlanar::setDepthImage (const float* depth_image, int di_width, int di_height,
00117                                    float di_center_x, float di_center_y,
00118                                    float di_focal_length_x, float di_focal_length_y,
00119                                    float desired_angular_resolution)
00120   {
00121     //MEASURE_FUNCTION_TIME;
00122     reset ();
00123     
00124     float original_angular_resolution = asinf (0.5f*static_cast<float> (di_width)/static_cast<float> (di_focal_length_x)) / (0.5f*static_cast<float> (di_width));
00125     int skip = 1;
00126     if (desired_angular_resolution >= 2.0f*original_angular_resolution)
00127       skip = static_cast<int> (pcl_lrint (floor (desired_angular_resolution / original_angular_resolution)));
00128 
00129     setAngularResolution (original_angular_resolution * static_cast<float> (skip));
00130     width  = di_width / skip;
00131     height = di_height / skip;
00132     focal_length_x_ = di_focal_length_x / static_cast<float> (skip);
00133     focal_length_x_reciprocal_ = 1.0f / focal_length_x_;
00134     focal_length_y_ = di_focal_length_y / static_cast<float> (skip);
00135     focal_length_y_reciprocal_ = 1.0f / focal_length_y_;
00136     center_x_ = static_cast<float> (di_center_x) / static_cast<float> (skip);
00137     center_y_ = static_cast<float> (di_center_y) / static_cast<float> (skip);
00138     points.resize (width * height);
00139     
00140     for (int y=0; y < static_cast<int> (height); ++y)
00141     {
00142       for (int x=0; x < static_cast<int> (width); ++x)
00143       {
00144         PointWithRange& point = getPointNoCheck (x, y);
00145         float depth = depth_image[ (y*skip)*di_width + x*skip];
00146         if (depth <= 0.0f || !pcl_isfinite (depth))
00147         {
00148           point = unobserved_point;
00149           continue;
00150         }
00151         point.z = depth;
00152         point.x = (static_cast<float> (x) - center_x_) * point.z * focal_length_x_reciprocal_;
00153         point.y = (static_cast<float> (y) - center_y_) * point.z * focal_length_y_reciprocal_;
00154         point.range = point.getVector3fMap ().norm ();
00155       }
00156     }
00157   }
00158 
00159 
00161   void
00162   RangeImagePlanar::setDepthImage (const unsigned short* depth_image, int di_width, int di_height,
00163                                    float di_center_x, float di_center_y,
00164                                    float di_focal_length_x, float di_focal_length_y,
00165                                    float desired_angular_resolution)
00166   {
00167     //MEASURE_FUNCTION_TIME;
00168     reset ();
00169     
00170     float original_angular_resolution = asinf (0.5f*static_cast<float> (di_width)/static_cast<float> (di_focal_length_x)) / (0.5f*static_cast<float> (di_width));
00171     int skip = 1;
00172     if (desired_angular_resolution >= 2.0f*original_angular_resolution)
00173       skip = static_cast<int> (pcl_lrint (floor (desired_angular_resolution/original_angular_resolution)));
00174 
00175     setAngularResolution (original_angular_resolution * static_cast<float> (skip));
00176     width  = di_width / skip;
00177     height = di_height / skip;
00178     focal_length_x_ = di_focal_length_x / static_cast<float> (skip);
00179     focal_length_x_reciprocal_ = 1.0f / focal_length_x_;
00180     focal_length_y_ = di_focal_length_y / static_cast<float> (skip);
00181     focal_length_y_reciprocal_ = 1.0f / focal_length_y_;
00182     center_x_ = static_cast<float> (di_center_x) / static_cast<float> (skip);
00183     center_y_ = static_cast<float> (di_center_y) / static_cast<float> (skip);
00184     points.resize (width * height);
00185     
00186     for (int y = 0; y < static_cast<int> (height); ++y)
00187     {
00188       for (int x = 0; x < static_cast<int> (width); ++x)
00189       {
00190         PointWithRange& point = getPointNoCheck (x, y);
00191         float depth = depth_image[ (y*skip)*di_width + x*skip] * 0.001f;
00192         if (depth <= 0.0f || !pcl_isfinite (depth))
00193         {
00194           point = unobserved_point;
00195           continue;
00196         }
00197         point.z = depth;
00198         point.x = (static_cast<float> (x) - center_x_) * point.z * focal_length_x_reciprocal_;
00199         point.y = (static_cast<float> (y) - center_y_) * point.z * focal_length_y_reciprocal_;
00200         point.range = point.getVector3fMap ().norm ();
00201       }
00202     }
00203   }
00204   
00206   void 
00207   RangeImagePlanar::getHalfImage (RangeImage& half_image) const
00208   {
00209     //std::cout << __PRETTY_FUNCTION__ << " called.\n";
00210     if (typeid (*this) != typeid (half_image))
00211     {
00212       std::cerr << __PRETTY_FUNCTION__<<": Given range image is not a RangeImagePlanar!\n";
00213       return;
00214     }
00215     RangeImagePlanar& ret = * (static_cast<RangeImagePlanar*> (&half_image));
00216     
00217     ret.focal_length_x_ = focal_length_x_/2;
00218     ret.focal_length_x_reciprocal_ = 1.0f/ret.focal_length_x_;
00219     ret.focal_length_y_ = focal_length_y_/2;
00220     ret.focal_length_y_reciprocal_ = 1.0f/ret.focal_length_y_;
00221     ret.center_x_ = center_x_/2;
00222     ret.center_y_ = center_y_/2;
00223     BaseClass::getHalfImage (ret);
00224   }
00225 
00227   void 
00228   RangeImagePlanar::getSubImage (int sub_image_image_offset_x, int sub_image_image_offset_y, int sub_image_width,
00229                                  int sub_image_height, int combine_pixels, RangeImage& sub_image) const
00230   {
00231     std::cerr << __PRETTY_FUNCTION__ << ": Warning, not tested properly!\n";
00232     
00233     if (typeid (*this) != typeid (sub_image))
00234     {
00235       std::cerr << __PRETTY_FUNCTION__<<": Given range image is not a RangeImagePlanar!\n";
00236       return;
00237     }
00238     RangeImagePlanar& ret = * (static_cast<RangeImagePlanar*> (&sub_image));
00239     
00240     ret.focal_length_x_ = focal_length_x_ / static_cast<float> (combine_pixels);
00241     ret.focal_length_x_reciprocal_ = 1.0f / ret.focal_length_x_;
00242     ret.focal_length_y_ = focal_length_x_ / static_cast<float> (combine_pixels);
00243     ret.focal_length_y_reciprocal_ = 1.0f/ret.focal_length_y_;
00244     ret.center_x_ = center_x_/2 - static_cast<float> (sub_image_image_offset_x);
00245     ret.center_y_ = center_y_/2 - static_cast<float> (sub_image_image_offset_y);
00246     BaseClass::getSubImage (sub_image_image_offset_x, sub_image_image_offset_y, sub_image_width,
00247                            sub_image_height, combine_pixels, ret);
00248     ret.image_offset_x_ = ret.image_offset_y_ = 0;
00249   }
00250 
00252   void
00253   RangeImagePlanar::copyTo (RangeImage& other) const
00254   {
00255     bool ERROR_GIVEN_RANGE_IMAGE_IS_NOT_A_RangeImagePlanar = typeid (*this) == typeid (other);
00256     if (!ERROR_GIVEN_RANGE_IMAGE_IS_NOT_A_RangeImagePlanar) {
00257       std::cerr << PVARC(typeid (*this).name())<<PVARN(typeid (other).name());
00258     }
00259     assert (ERROR_GIVEN_RANGE_IMAGE_IS_NOT_A_RangeImagePlanar);
00260     *static_cast<RangeImagePlanar*> (&other) = *this;
00261   }
00262 
00263 }  // namespace end
00264 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:32:01