object_model.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.
 *
 */

/* \author Bastian Steder */


#include <narf_recognition/object_model.h>

#include <pcl/common/transformation_from_correspondences.h>
#include <pcl/common/transforms.h>
#include <iostream>
using std::cout;
using std::cerr;
using std::string;
using std::min;
using std::max;
using std::vector;
#include <cmath>

#include <pcl/common/common_headers.h>

#include <pcl/features/range_image_border_extractor.h>
#include <pcl/keypoints/narf_keypoint.h>

using Eigen::Vector3f;
using Eigen::Affine3f;

namespace pcl {

ObjectModel::ObjectModel()
{
  clear();
}

ObjectModel::~ObjectModel()
{
  clear();
}

void ObjectModel::clear(bool keep_point_cloud)
{
  id_ = 0;
  name_ = "";
  if (!keep_point_cloud)
    point_cloud_ = PointCloudType();
  clearViews();
  center_ = Vector3f(0.0f, 0.0f, 0.0f);
  radius_ = 0.0f;
  is_single_view_model_ = false;
}

void ObjectModel::clearViews()
{
  views_.clear();
  views_for_validation_points_.clear();
  validation_points_.clear();
}

void ObjectModel::updateCenterAndRadius()
{
  // Get bounding box
  Vector3f bb_min(INFINITY, INFINITY, INFINITY), bb_max(-INFINITY, -INFINITY, -INFINITY);
  for (unsigned int i=0; i<point_cloud_.points.size(); ++i)
  {
    const PointType& p = point_cloud_.points[i];
    bb_min[0]=min(bb_min[0], p.x);  bb_min[1]=min(bb_min[1], p.y);  bb_min[2]=min(bb_min[2], p.z);
    bb_max[0]=max(bb_max[0], p.x);  bb_max[1]=max(bb_max[1], p.y);  bb_max[2]=max(bb_max[2], p.z);
  }
  center_ = 0.5f * (bb_min+bb_max);
  
  radius_ = 0.0f;
  for (unsigned int i=0; i<point_cloud_.points.size(); ++i)
  {
    const PointType& p = point_cloud_.points[i];
    radius_ = max(radius_, (center_-Vector3f(p.x, p.y, p.z)).norm());
  }
  //cout << "Object center is ("<<center_[0]<<","<<center_[1]<<","<<center_[2]<<") with radius "<<radius_<<"m.\n";
}

void ObjectModel::sampleViews2D(int no_of_views, int first_layer, int last_layer,
                                float angular_resolution, float distance,
                                std::vector<RangeImage, Eigen::aligned_allocator<RangeImage> >* views)
{
  //cout << __PRETTY_FUNCTION__<<" was called.\n";
  views = (views==NULL ? &views_ : views);
  if (radius_ == 0.0f)
  {
    cerr << __PRETTY_FUNCTION__<<": radius_ is 0.0 => calling updateCenterAndRadius()\n";
    updateCenterAndRadius();
  }
  
  for (int layer=first_layer; layer<=last_layer; ++layer) {
    for (int view_index=0; view_index<no_of_views; ++view_index)
    {
      float angle = deg2rad(360.f)*float(view_index)/float(no_of_views) - deg2rad(180.f);
      Vector3f direction(cosf(angle), -0.5f*float(layer), sinf(angle));
      addSimulatedView(direction, angular_resolution, distance, views);
    }
  }
}

void ObjectModel::sampleViews3D(float sample_angular_resolution, float angular_resolution, float distance,
                                std::vector<RangeImage, Eigen::aligned_allocator<RangeImage> >* views)
{
  views = (views==NULL ? &views_ : views);
  if (radius_ == 0.0f)
  {
    cerr << __PRETTY_FUNCTION__<<": radius_ is 0.0 => calling updateCenterAndRadius()\n";
    updateCenterAndRadius();
  }
  
  int no_of_steps_y = lrint(deg2rad(180.0f)/sample_angular_resolution)+1;
  //cout << PVARN(no_of_steps_y);
  float step_size_y = deg2rad(180.0f)/float(no_of_steps_y-1);
  for (int step_y=0; step_y<no_of_steps_y; ++step_y)
  {
    float angle_y = step_y*step_size_y - deg2rad(90.0f);
    int no_of_steps_x = max(1, (int)(lrint(cosf(angle_y)*deg2rad(360.0f) / sample_angular_resolution)));
    //cout<<PVARN(no_of_steps_x);
    float step_size_x = deg2rad(360.0f)/float(no_of_steps_x);
    
    for (int step_x=0; step_x<no_of_steps_x; ++step_x)
    {
      float angle_x = step_x*step_size_x - deg2rad(180.0f);
      //cout << PVARAC(angle_x)<<PVARAN(angle_y);
      Vector3f direction(sinf(angle_x) * cosf(angle_y), sinf(angle_y), cosf(angle_x)*cosf(angle_y));
      //cout << "Sample direction for angles ("<<rad2deg(angle_x)<<","<<rad2deg(angle_y)<<") is ("<<direction[0]<<","<<direction[1]<<","<<direction[2]<<")\n";
      addSimulatedView(direction, angular_resolution, distance, views);
    }
  }
}

void ObjectModel::addSimulatedView(const Vector3f& direction, float angular_resolution, float distance,
                                   std::vector<RangeImage, Eigen::aligned_allocator<RangeImage> >* views)
{
  views = (views==NULL ? &views_ : views);
  if (distance <= 0.0f)
    distance = 5.0f*radius_;
  
  Vector3f viewpoint = center_ + (radius_+distance)*direction;
  float actual_distance = INFINITY;
  for (unsigned int i=0; i<point_cloud_.points.size(); ++i)
  {
    const PointType& p = point_cloud_.points[i];
    actual_distance = min(actual_distance, (viewpoint-Vector3f(p.x, p.y, p.z)).norm());
  }
  // Approximate method to get closer to the real distance
  viewpoint = viewpoint - (actual_distance-distance)*direction;
  //cout << "Viewpoint is ("<<viewpoint[0]<<","<<viewpoint[1]<<","<<viewpoint[2]<<").\n";
  Affine3f viewer_pose = getViewerPose(viewpoint);
  
  views->push_back(RangeImage());
  RangeImage& view = views->back();
  //cout << "Starting creating view for pose \n"<<viewer_pose<<" and point cloud of size "<<point_cloud_.points.size()<<"\n";
  createView(viewer_pose, angular_resolution, view);
}

float ObjectModel::getMaxAngleSize(const Eigen::Affine3f& viewer_pose) const
{
  return RangeImage::getMaxAngleSize(viewer_pose, center_, radius_);
}

void ObjectModel::addView(const Eigen::Affine3f& viewer_pose, float angular_resolution)
{
  //cout << PVARN(point_cloud_.points.size())<<PVARN(radius_);
  views_.push_back(RangeImage());
  createView(viewer_pose, angular_resolution, views_.back());
}

void ObjectModel::createView(const Affine3f& viewer_pose, float angular_resolution, RangeImage& view) const
{
  float noise_level = 0.05f*radius_;
  view.createFromPointCloud(point_cloud_, angular_resolution, deg2rad(360.0f), deg2rad(180.0f), viewer_pose, RangeImage::CAMERA_FRAME, noise_level, 0.0, 1);
  //view.createFromPointCloudWithKnownSize(point_cloud_, angular_resolution, center_, radius_, viewer_pose, RangeImage::CAMERA_FRAME, 0.0, 0.0, 1); // damn, doesn't work for objects with large radius
  view.setUnseenToMaxRange();
}

Affine3f ObjectModel::getViewerPose(const Vector3f& viewpoint) const
{
  Affine3f viewer_pose;
  Vector3f viewing_direction = (center_-viewpoint).normalized(),
           up_vector(0.0f, 1.0f, 0.0f);
  getTransformationFromTwoUnitVectorsAndOrigin(up_vector, viewing_direction, viewpoint, viewer_pose);
  viewer_pose = viewer_pose.inverse();
  return viewer_pose;
}

void ObjectModel::extractNARFsForCompleteSurface(unsigned int descriptor_size, float size_in_world, bool rotation_invariant, vector<vector<Narf*>*>& features) const
{
  freeFeatureListMemory(features);
  
  std::vector<float> rotations, strengths;
  for (unsigned int view_index=0; view_index<views_.size(); ++view_index)
  {
    const RangeImage& view = views_[view_index];
    //cout << PVARN(view);
    features.push_back(new vector<Narf*>);
    vector<Narf*>& features_of_current_view = *features.back();
    
    for (unsigned int y=0; y<view.height; ++y)
    {
      for (unsigned int x=0; x<view.width; ++x)
      {
        Narf::extractFromRangeImageAndAddToList(view, x, y, descriptor_size, size_in_world, rotation_invariant, features_of_current_view);
      }
    }
    //cout << PVARN(features_of_current_view.size());
  }
}

void ObjectModel::extractInterestPoints(int view_ix, float support_size, NarfKeypoint& interest_point_detector, 
                                        PointCloud<InterestPoint>& interest_points) const
{
  float original_support_size = interest_point_detector.getParameters().support_size;
  interest_point_detector.getParameters().support_size = support_size;
  interest_point_detector.setRangeImage(&views_[view_ix]);
  interest_points = interest_point_detector.getInterestPoints();
  //cout << "Resetting interest point detector.\n";
  interest_point_detector.setRangeImage(NULL); // Clear structure to prevent memory problems when view is not valid anymore
  interest_point_detector.getParameters().support_size = original_support_size;
}

void ObjectModel::extractNARFsForInterestPoints(unsigned int descriptor_size, float size_in_world,
                                               bool rotation_invariant, NarfKeypoint& interest_point_detector,
                                               vector<vector<Narf*>*>& features) const
{
  freeFeatureListMemory(features);
  
  std::vector<float> rotations, strengths;
  for (unsigned int view_index=0; view_index<views_.size(); ++view_index)
  {
    const RangeImage& view = views_[view_index];
    
    PointCloud<InterestPoint> interest_points;
    extractInterestPoints(view_index, size_in_world, interest_point_detector, interest_points);
    
    //cout << PVARN(view);
    features.push_back(new vector<Narf*>);
    vector<Narf*>& features_of_current_view = *features.back();
    
    Narf::extractForInterestPoints(view, interest_points, descriptor_size, size_in_world, rotation_invariant, features_of_current_view);
    //cout << PVARN(features_of_current_view.size());
  }
}


void ObjectModel::getTransformedPointCloud(const Affine3f& transformation, ObjectModel::PointCloudType& output) const
{
  pcl::transformPointCloud(point_cloud_, output, transformation);
}

void ObjectModel::updateValidationPoints(unsigned int no_of_surface_points, unsigned int no_of_border_points, float sample_resolution,
                                         NarfKeypoint* interest_point_detector, float support_size)
{
  //cout << __PRETTY_FUNCTION__ << " called.\n";
  //MEASURE_FUNCTION_TIME;
  if (is_single_view_model_)
  {
    views_for_validation_points_ = views_;
  }
  else
  {
    views_for_validation_points_.clear();
    sampleViews3D(sample_resolution, deg2rad(0.4f), -1.0f, &views_for_validation_points_);
  }
  
  validation_points_.clear();
  validation_points_.resize(views_for_validation_points_.size());
  
  for (unsigned int view_idx=0; view_idx<views_for_validation_points_.size(); ++view_idx)
  {
    //std::cout << view_idx<<" ";
    const RangeImage& view = views_for_validation_points_[view_idx];
    ValidationPoints& validation_points_for_view = validation_points_[view_idx];
    
    int width=view.width, height=view.height;
    vector<Eigen::Vector3f> surface_points, border_points;
    for (int x=0; x<width; ++x)
    {
      for (int y=0; y<height; ++y)
      {
        int index = y*width+x;
        const PointWithRange& point = view.getPoint(index);
        if (!pcl_isfinite(point.range))
          continue;
        bool is_border_point=false;
        for (int y2=y-1; y2<=y+1; ++y2)
        {
          for (int x2=(y2==y ? x-1 : x); x2<=(y2==y ? x+1 : x); x2+=2)
          {
            if (!view.isInImage(x2,y2))
              continue;
            const PointWithRange& point2 = view.getPoint(x2, y2);
            if (std::isinf(point2.range) && point2.range>0.0f)
            {
              is_border_point = true;
              break;
            }
          }
        }
        Eigen::Vector3f point_eigen;
        view.getPoint(x, y, point_eigen);
        if (is_border_point)
          border_points.push_back(point_eigen);
        else if (interest_point_detector == NULL)
          surface_points.push_back(point_eigen);
      }
    }
    //std::cout << view_idx<<" ";
    
    if (interest_point_detector != NULL)
    {
      PointCloud<InterestPoint> interest_points;
      float original_distance_for_additional_points = interest_point_detector->getParameters().distance_for_additional_points;
      interest_point_detector->getParameters().distance_for_additional_points = 0.0f;
      extractInterestPoints(view_idx, support_size, *interest_point_detector, interest_points);
      for (unsigned int i=0; i<interest_points.points.size(); ++ i)
        surface_points.push_back(interest_points.points[i].getVector3fMap());
      interest_point_detector->getParameters().distance_for_additional_points = original_distance_for_additional_points;
    }
    
    //cout << PVARC(surface_points.size())<<PVARN(border_points.size());
    getUniformlyDistributedPoints(surface_points, no_of_surface_points, validation_points_for_view.surface);
    getUniformlyDistributedPoints(border_points, no_of_border_points, validation_points_for_view.border);
  }
}


void ObjectModel::getUniformlyDistributedPoints(const vector<Eigen::Vector3f>& points, int no_of_points, vector<Eigen::Vector3f>& result)
{
  if (points.empty() || no_of_points==0)
    return;

  no_of_points = std::min(no_of_points, (int)points.size());
  
  std::multimap<float, Vector3f> distance_sorted_points1, distance_sorted_points2;
  std::multimap<float, Vector3f>* current_points=&distance_sorted_points1, * new_points=&distance_sorted_points2;
  result.push_back(points[0]);
  
  for (unsigned int i=1; i<points.size(); ++i)
    current_points->insert(std::make_pair(INFINITY, points[i]));
  
  while ((int)result.size() < no_of_points)
  {
    new_points->clear();
    for (std::multimap<float, Vector3f>::const_iterator it=current_points->begin(); it!=current_points->end(); ++it)
    {
      const Vector3f& point = it->second;
      float distance_to_last_point = (result.back()-point).squaredNorm();
      float distance = min(it->first, distance_to_last_point);
      if (distance > 0.0f)
        new_points->insert(std::make_pair(distance, point));
    }
    std::swap(current_points, new_points);
    //cout << PVARC(result.size())<<PVARN(current_points->size());
    result.push_back(current_points->rbegin()->second);
  }
}

Eigen::Affine3f ObjectModel::doFastICP(const Eigen::Affine3f& initial_guess, const RangeImage& range_image,
                                        int search_radius, int no_of_surface_points_to_use,
                                        int no_of_border_points_to_use, int num_iterations,
                                        float max_distance_start, float max_distance_end) const
{
  if (max_distance_start <= 0.0f)
    max_distance_start = 0.05f*radius_;
  if (max_distance_end <= 0.0f)
    max_distance_end = 0.5*max_distance_start;
  Affine3f model_viewer_pose = initial_guess.inverse()*range_image.getTransformationToWorldSystem();
  int view_idx = getClosestValidationPointViewIdx(model_viewer_pose);
  const ValidationPoints& validation_points_for_view = validation_points_[view_idx];
  
  //std::vector<Eigen::Vector3f> points;
  //points.resize(no_of_surface_points_to_use+no_of_border_points_to_use);
  //for (int i=0; i<min(no_of_surface_points_to_use,(int)validation_points_for_view.surface.size()); ++i)
    //points.push_back(validation_points_for_view.surface[i]);
  //for (int i=0; i<min(no_of_border_points_to_use,(int)validation_points_for_view.border.size()); ++i)
    //points.push_back(validation_points_for_view.border[i]);
  
  //return range_image.doIcp(points, initial_guess, search_radius, max_distance_start, max_distance_end, num_iterations);


  float max_distance = max_distance_start, 
        max_distance_reduction = (max_distance_start-max_distance_end)/float(num_iterations);
  Affine3f ret = initial_guess;
  no_of_surface_points_to_use = min(no_of_surface_points_to_use, (int)validation_points_for_view.surface.size());
  no_of_border_points_to_use  = min(no_of_border_points_to_use, (int)validation_points_for_view.border.size());
  
  
  TransformationFromCorrespondences transformation_from_correspondeces;
  for (int iteration=1; iteration<=num_iterations; ++iteration)
  {
    float max_distance_squared = max_distance*max_distance;
    transformation_from_correspondeces.reset();
    for (int point_idx=0; point_idx<no_of_surface_points_to_use+no_of_border_points_to_use; ++point_idx)
    {
      const Vector3f* point;
      if (point_idx<no_of_surface_points_to_use)
         point = &validation_points_for_view.surface[point_idx];
      else
         point = &validation_points_for_view.border[point_idx-no_of_surface_points_to_use];
      Vector3f transformed_point = ret * *point;
      int x,y;
      range_image.getImagePoint(transformed_point, x, y);
      float closest_distance = max_distance_squared;
      Vector3f closest_point(0,0,0);
      bool found_neighbor = false;
      for (int y2=y-search_radius; y2<=y+search_radius; ++y2)
      {
        for (int x2=x-search_radius; x2<=x+search_radius; ++x2)
        {
          const PointWithRange& neighbor = range_image.getPoint(x2, y2);
          if (!pcl_isfinite(neighbor.range))
            continue;
          float distance = (transformed_point-neighbor.getVector3fMap()).squaredNorm();
          if (distance < closest_distance)
          {
            closest_distance = distance;
            closest_point = neighbor.getVector3fMap();
            found_neighbor = true;
          }
        }
      }
      if (found_neighbor)
      {
        //cout << PVARN(closest_distance);
        transformation_from_correspondeces.add(*point, closest_point);
      }
    }
    //cout << PVARN(transformation_from_correspondeces.getNoOfSamples());
    //cout << PVARN(iteration);
    if (transformation_from_correspondeces.getNoOfSamples() < 3)
      return ret;
    // TODO: check
    ret = transformation_from_correspondeces.getTransformation();
    //cout << ret.matrix()<<"\n";
    
    max_distance -= max_distance_reduction;
  }
  //cout << PVARN(initial_guess.matrix())<<PVARN(ret.matrix());
  
  return ret;
}

Eigen::Affine3f ObjectModel::doSlowICP(const Eigen::Affine3f& initial_guess, const RangeImage& range_image,
                                       int search_radius, int num_iterations, float max_distance_start,
                                       float max_distance_end) const
{
  if (max_distance_start <= 0.0f)
    max_distance_start = 0.05f*radius_;
  if (max_distance_end <= 0.0f)
    max_distance_end = 0.5*max_distance_start;
  Affine3f viewer_pose = initial_guess.inverse()*range_image.getTransformationToWorldSystem();
  float angular_resolution = range_image.getAngularResolution();
  //float angular_resolution = 2.0f * range_image.getAngularResolution();
  RangeImage simulated_view;
  float noise_level = 0.1*radius_;
  simulated_view.createFromPointCloud(point_cloud_, angular_resolution, deg2rad(360.0f), deg2rad(180.0f), viewer_pose,
                                      RangeImage::CAMERA_FRAME, noise_level);
  RangeImage::VectorOfEigenVector3f points;
  for (unsigned int i=0; i<simulated_view.points.size(); ++i)
  //for (unsigned int i=0; i<point_cloud_.points.size(); ++i)
    if (simulated_view.isValid(i))
      points.push_back(simulated_view.getPoint(i).getVector3fMap());
  
  return range_image.doIcp(points, initial_guess, search_radius, max_distance_start, max_distance_end, num_iterations);
}

void ObjectModel::addNoise(float max_noise)
{
  double normalization_factor = double(2.0*max_noise) / double(RAND_MAX),
         offset = -max_noise;
  clear(true);  // Clear all but point_cloud_;
  
  for (unsigned int point_idx=0; point_idx<point_cloud_.points.size(); ++point_idx)
  {
    PointType& point = point_cloud_.points[point_idx];
    point.x += normalization_factor*double(rand()) + offset;
    point.y += normalization_factor*double(rand()) + offset;
    point.z += normalization_factor*double(rand()) + offset;
  }
}

float ObjectModel::getMaximumPlaneSize (float initial_max_plane_error) const
{
  float max_plane_size = 0.0f;
  std::vector<RangeImage::ExtractedPlane> planes;
  for (int view_idx=0; view_idx<int(views_.size()); ++view_idx)
  {
    const RangeImage& view = views_[view_idx];
    planes.clear ();
    view.extractPlanes (initial_max_plane_error, planes);
    for (int plane_idx=0; plane_idx<int(planes.size()); ++plane_idx)
      max_plane_size = std::max(max_plane_size, planes[plane_idx].maximum_extensions[2]);
  }
  return max_plane_size;
}

} // namespace end