range_image_planar.cpp

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#include <iostream>
using std::cout;
using std::cerr;

#include <pcl/range_image/range_image_planar.h>

namespace pcl 
{

RangeImagePlanar::RangeImagePlanar () : RangeImage(), focal_length_x_(0.0f), focal_length_y_(0.0f),
                                        focal_length_x_reciprocal_(0.0f), focal_length_y_reciprocal_(0.0f),
                                        center_x_(0.0f), center_y_(0.0f)
{
}

RangeImagePlanar::~RangeImagePlanar ()
{
}

void
RangeImagePlanar::setDisparityImage (const float* disparity_image, int di_width, int di_height,
                                     float focal_length, float base_line, float desired_angular_resolution)
{
  //MEASURE_FUNCTION_TIME;
  reset ();
  
  float original_angular_resolution = asinf (0.5f*float(di_width)/float(focal_length)) / (0.5f*float(di_width));
  int skip = 1;
  if (desired_angular_resolution >= 2.0f*original_angular_resolution)
  {
    skip = lrint (floor (desired_angular_resolution/original_angular_resolution));
  }
  //std::cout << PVARN (skip);
  setAngularResolution (original_angular_resolution * skip);
  width  = di_width / skip;
  height = di_height / skip;
  focal_length_x_ = focal_length_y_ = focal_length / skip;
  focal_length_x_reciprocal_ = focal_length_y_reciprocal_ = 1.0f / focal_length_x_;
  center_x_ = float(di_width)  / float(2*skip);
  center_y_ = float(di_height) / float(2*skip);
  points.resize(width*height);
  
  //cout << PVARN (*this);
  
  float normalization_factor = skip*focal_length_x_*base_line;
  for (int y=0; y<(int)height; ++y)
  {
    for (int x=0; x<(int)width; ++x)
    {
      PointWithRange& point = getPointNoCheck(x,y);
      float disparity = disparity_image[(y*skip)*di_width + x*skip];
      if (disparity <= 0.0f)
      {
        //std::cout << disparity << ", "<<std::flush;
        point = unobserved_point;
        continue;
      }
      point.z = normalization_factor / disparity;
      point.x = (x-center_x_)*point.z * focal_length_x_reciprocal_;
      point.y = (y-center_y_)*point.z * focal_length_y_reciprocal_;
      point.range = point.getVector3fMap().norm();
      //cout << point<<std::flush;
      
      //PointWithRange test_point1;
      //calculate3DPoint (float (x), float (y), point.range, test_point1);
      //if ((point.getVector3fMap ()-test_point1.getVector3fMap ()).norm () > 1e-5)
        //cerr << "Something's wrong here...\n";
      
      //float image_x, image_y;
      //getImagePoint(point.getVector3fMap (), image_x, image_y);
      //if (fabsf (image_x-x)+fabsf (image_y-y)>1e-4)
        //cerr << PVARC (x)<<PVARC (y)<<PVARC (image_x)<<PVARN (image_y);
    }
  }
}


void
RangeImagePlanar::setDepthImage (const float* depth_image, int di_width, int di_height,
                                 float di_center_x, float di_center_y,
                                 float di_focal_length_x, float di_focal_length_y,
                                 float desired_angular_resolution)
{
  //MEASURE_FUNCTION_TIME;
  reset ();
  
  float original_angular_resolution = asinf (0.5f*float(di_width)/float(di_focal_length_x)) / (0.5f*float(di_width));
  int skip = 1;
  if (desired_angular_resolution >= 2.0f*original_angular_resolution)
  {
    skip = lrint (floor (desired_angular_resolution/original_angular_resolution));
  }
  //std::cout << PVARN (skip);
  setAngularResolution (original_angular_resolution * skip);
  width  = di_width / skip;
  height = di_height / skip;
  focal_length_x_ = di_focal_length_x / skip;
  focal_length_x_reciprocal_ = 1.0f / focal_length_x_;
  focal_length_y_ = di_focal_length_y / skip;
  focal_length_y_reciprocal_ = 1.0f / focal_length_y_;
  center_x_ = float(di_center_x) / float(skip);
  center_y_ = float(di_center_y) / float(skip);
  points.resize(width*height);
  
  //cout << PVARN (*this);
  
  for (int y=0; y<(int)height; ++y)
  {
    for (int x=0; x<(int)width; ++x)
    {
      PointWithRange& point = getPointNoCheck(x,y);
      float depth = depth_image[(y*skip)*di_width + x*skip];
      if (depth <= 0.0f || !pcl_isfinite(depth))
      {
        //std::cout << depth << ", "<<std::flush;
        point = unobserved_point;
        continue;
      }
      point.z = depth;
      point.x = (x-center_x_)*point.z * focal_length_x_reciprocal_;
      point.y = (y-center_y_)*point.z * focal_length_y_reciprocal_;
      point.range = point.getVector3fMap().norm();
    }
  }
}

void 
RangeImagePlanar::getHalfImage(RangeImage& half_image) const
{
  //std::cout << __PRETTY_FUNCTION__ << " called.\n";
  if (typeid(*this) != typeid(half_image))
  {
    std::cerr << __PRETTY_FUNCTION__<<": Given range image is not a RangeImagePlanar!\n";
    return;
  }
  RangeImagePlanar& ret = *((RangeImagePlanar*)&half_image);
  
  ret.focal_length_x_ = focal_length_x_/2;
  ret.focal_length_x_reciprocal_ = 1.0f/ret.focal_length_x_;
  ret.focal_length_y_ = focal_length_y_/2;
  ret.focal_length_y_reciprocal_ = 1.0f/ret.focal_length_y_;
  ret.center_x_ = center_x_/2;
  ret.center_y_ = center_y_/2;
  BaseClass::getHalfImage(ret);
}

void 
RangeImagePlanar::getSubImage(int sub_image_image_offset_x, int sub_image_image_offset_y, int sub_image_width,
                        int sub_image_height, int combine_pixels, RangeImage& sub_image) const
{
  std::cerr << __PRETTY_FUNCTION__ << ": Warning, not tested properly!\n";
  
  if (typeid(*this) != typeid(sub_image))
  {
    std::cerr << __PRETTY_FUNCTION__<<": Given range image is not a RangeImagePlanar!\n";
    return;
  }
  RangeImagePlanar& ret = *((RangeImagePlanar*)&sub_image);
  
  ret.focal_length_x_ = focal_length_x_ / combine_pixels;
  ret.focal_length_x_reciprocal_ = 1.0f/ret.focal_length_x_;
  ret.focal_length_y_ = focal_length_x_ / combine_pixels;
  ret.focal_length_y_reciprocal_ = 1.0f/ret.focal_length_y_;
  ret.center_x_ = center_x_/2 - sub_image_image_offset_x;
  ret.center_y_ = center_y_/2 - sub_image_image_offset_y;
  BaseClass::getSubImage(sub_image_image_offset_x, sub_image_image_offset_y, sub_image_width,
                         sub_image_height, combine_pixels, ret);
  ret.image_offset_x_ = ret.image_offset_y_ = 0;
}

}  // namespace end

---

pcl
Author(s): See http://pcl.ros.org/authors for the complete list of authors.
autogenerated on Fri Jan 11 09:55:23 2013