narf_descriptor_visualization.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 <iostream>
using namespace std;

#include "pcl/point_cloud.h"
#include "pcl/io/pcd_io.h"
#include "pcl_visualization/range_image_visualizer.h"
#include "pcl/range_image/range_image.h"
#include <pcl/features/narf.h>
//#include "pcl/point_types.h"
//#include <pcl/object_recognition/object_model_list.h>
//#include <pcl/object_recognition/false_positives_filter.h>
//#include <pcl/features/point_correspondences.h>
//#include <pcl/features/pose_estimation_from_correspondences.h>
//#include <pcl/features/range_image_border_extractor.h>
//#include <interest_points/range_image_interest_point_detector.h>
//#include <pcl/common/common_headers.h>

using namespace Eigen;
using namespace pcl;
using namespace pcl_visualization;

float angular_resolution = 0.5f;
int use_rotation_invariance = 1;
float support_size = 0.3f;
int descriptor_size = 36;
RangeImage::CoordinateFrame coordinate_frame = RangeImage::CAMERA_FRAME;
bool setUnseenToMaxRange = false;

typedef PointXYZ PointType;

void printUsage(const char* progName)
{
  cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
       << "Options:\n"
       << "-------------------------------------------\n"
       << "-r <float>   angular resolution in degrees (default "<<angular_resolution<<")\n"
       << "-s <float>   support size (diameter of the inclosed sphere) of the features in meters (default "<<support_size<<")\n"
       << "-d <int>     descriptor size (default "<<descriptor_size<<")\n"
       << "-c <int>     coordinate frame of the input point cloud (default "<<(int)coordinate_frame<<")\n"
       << "-i <0/1>     use rotation invariant descriptor (default "<<use_rotation_invariance<<")\n"
       << "-m           set unseen pixels to max range\n"
       << "-h           this help\n"
       << "\n\n";
}

int main (int argc, char** argv)
{
  // --------------------------------------
  // -----Parse Command Line Arguments-----
  // --------------------------------------
  for (char c; (c = getopt(argc, argv, "r:s:d:c:i:mh")) != -1; ) {
    switch (c) {
      case 'r':
      {
        angular_resolution = strtod(optarg, NULL);
        cout << PVARN(angular_resolution);
        break;
      }
      case 's':
      {
        support_size = strtod(optarg, NULL);
        cout << PVARN(support_size);
        break;
      }
      case 'c':
      {
        coordinate_frame = (RangeImage::CoordinateFrame)strtol(optarg, NULL, 0);
        cout << "Using coordinate frame "<<coordinate_frame<<".\n";
        break;
      }
      case 'i':
      {
        use_rotation_invariance = (RangeImage::CoordinateFrame)strtol(optarg, NULL, 0);
        cout << (use_rotation_invariance ? "U":"Not u")<<"sing rotational invariant descriptor.\n";
        break;
      }
      case 'm':
      {
        setUnseenToMaxRange = true;
        cout << "Unseen pixels will be considered max ranges.\n";
        break;
      }
      case 'h':
      {
        printUsage(argv[0]);
        exit(0);
      }
    }
  }
  angular_resolution = deg2rad(angular_resolution);
  // --------------------------------------
  
  PointCloud<PointType> point_cloud;
  PointCloud<PointWithViewpoint> far_ranges;
  
  // -----------------------
  // -----Read pcd file-----
  // -----------------------
  Eigen::Affine3f scene_sensor_pose(Eigen::Affine3f::Identity());
  if (optind < argc)
  {
    sensor_msgs::PointCloud2 point_cloud_data;
    if (pcl::io::loadPCDFile(argv[optind], point_cloud_data) == -1)
    {
      ROS_ERROR_STREAM("Was not able to open file \""<<argv[optind]<<"\".\n");
      printUsage(argv[0]);
      exit(0);
    }
    fromROSMsg(point_cloud_data, point_cloud);
    RangeImage::extractFarRanges(point_cloud_data, far_ranges);
    if (pcl::getFieldIndex(point_cloud_data, "vp_x")>=0)
    {
      //cout << "Scene point cloud has viewpoint information.\n";
      PointCloud<PointWithViewpoint> tmp_pc;  fromROSMsg(point_cloud_data, tmp_pc);
      Eigen::Vector3f average_viewpoint = RangeImage::getAverageViewPoint(tmp_pc);
      scene_sensor_pose = Eigen::Translation3f(average_viewpoint) * scene_sensor_pose;
    }
  }
  else
  {
    cout << "\nNo *.pcd file for scene given.\n\n";
    printUsage(argv[0]);
    exit(0);
  }
  // ------------------------------------------------------------------

  // Create a range image from the above point cloud
  float noise_level = 0.25*support_size;
  RangeImage scene_range_image;
  scene_range_image.createFromPointCloud(point_cloud, angular_resolution, deg2rad(360.0f), deg2rad(180.0f), scene_sensor_pose,
                                         coordinate_frame, noise_level, 0.0f, 0);
  //cout << PVARN(scene_range_image);
  
  // Extract NARF features:
  cout << "Now extracting NARFs in every image point.\n";
  vector<vector<Narf*> > narfs;
  narfs.resize(scene_range_image.points.size());
  int last_percentage=-1;
  for (unsigned int y=0; y<scene_range_image.height; ++y)
  {
    for (unsigned int x=0; x<scene_range_image.width; ++x)
    {
      int index = y*scene_range_image.width+x;
      int percentage = (100*index) / scene_range_image.points.size();
      if (percentage > last_percentage)
      {
        cout << percentage<<"% "<<std::flush;
        last_percentage = percentage;
      }
      Narf::extractFromRangeImageAndAddToList(scene_range_image, x, y, descriptor_size, support_size, use_rotation_invariance, narfs[index]);
      //cout << "Extracted "<<narfs[index].size()<<" features for pixel "<<x<<","<<y<<".\n";
    }
  }
  cout << "100%\n";
  cout << "Done.\n\n Now you can click on points in the image to visualize how the descriptor is extracted and see the descriptor distances to every other point..\n";
  
  //---------------------
  // -----Show image-----
  // --------------------
  RangeImageVisualizer scene_range_image_widget("Scene range image");
  scene_range_image_widget.setRangeImage(scene_range_image);
  scene_range_image_widget.visualize_selected_point = true;

  //--------------------
  // -----Main loop-----
  //--------------------
  while(scene_range_image_widget.isShown()) {
    ImageWidgetWX::spinOnce();  // process GUI events
    usleep(100000);
    
    if (!scene_range_image_widget.mouse_click_happened)
      continue;
    scene_range_image_widget.mouse_click_happened = false;
    float clicked_pixel_x_f = scene_range_image_widget.last_clicked_point_x,
          clicked_pixel_y_f = scene_range_image_widget.last_clicked_point_y;
    int clicked_pixel_x, clicked_pixel_y;
    scene_range_image.real2DToInt2D(clicked_pixel_x_f, clicked_pixel_y_f, clicked_pixel_x, clicked_pixel_y);
    if (!scene_range_image.isValid(clicked_pixel_x, clicked_pixel_y))
      continue;
      //Vector3f clicked_3d_point;
      //scene_range_image.getPoint(clicked_pixel_x, clicked_pixel_y, clicked_3d_point);
    
    static ImageWidgetWX surface_patch_widget, descriptor_widget;
    surface_patch_widget.show(false);
    descriptor_widget.show(false);
    
    int selected_index = clicked_pixel_y*scene_range_image.width + clicked_pixel_x;
    Narf narf;
    if (!narf.extractFromRangeImage(scene_range_image, clicked_pixel_x, clicked_pixel_y,
                                                                         descriptor_size, support_size))
    {
      continue;
    }
    
    int surface_patch_pixel_size = narf.getSurfacePatchPixelSize();
    float surface_patch_world_size = narf.getSurfacePatchWorldSize();
    surface_patch_widget.setFloatImage(narf.getSurfacePatch(), surface_patch_pixel_size, surface_patch_pixel_size,
                                       "Descriptor's surface patch", -0.5f*surface_patch_world_size, 0.5f*surface_patch_world_size, true);
    float surface_patch_rotation = narf.getSurfacePatchRotation();
    float patch_middle = 0.5f*(float(surface_patch_pixel_size-1));
    float angle_step_size = deg2rad(360.0f)/narf.getDescriptorSize();
    float cell_size = surface_patch_world_size/float(surface_patch_pixel_size),
          cell_factor = 1.0f/cell_size,
          max_dist = 0.5f*surface_patch_world_size,
          line_length = cell_factor*(max_dist-0.5f*cell_size);
    for (int descriptor_value_idx=0; descriptor_value_idx<narf.getDescriptorSize(); ++descriptor_value_idx)
    {
      float angle = descriptor_value_idx*angle_step_size + surface_patch_rotation;
      surface_patch_widget.markLine(patch_middle, patch_middle, patch_middle+line_length*sinf(angle), patch_middle+line_length*-cosf(angle));
    }
    vector<float> rotations, strengths;
    narf.getRotations(rotations, strengths);
    float radius = 0.5f*surface_patch_pixel_size;
    for (unsigned int i=0; i<rotations.size(); ++i)
    {
      surface_patch_widget.markLine(radius-0.5, radius-0.5, radius-0.5f + 2.0f*radius*sinf(rotations[i]),
                                                radius-0.5f - 2.0f*radius*cosf(rotations[i]), wxRED_PEN);
    }
    
    descriptor_widget.setFloatImage(narf.getDescriptor(), narf.getDescriptorSize(), 1, "Descriptor", -0.1f, 0.3f, true);

    //===================================
    //=====Compare with all features=====
    //===================================
    const vector<Narf*>& narfs_of_selected_point = narfs[selected_index];
    if (narfs_of_selected_point.empty())
      continue;
    
    static ImageWidgetWX descriptor_distances_widget;
    descriptor_distances_widget.show(false);
    float* descriptor_distance_image = new float[scene_range_image.points.size()];
    for (unsigned int point_index=0; point_index<scene_range_image.points.size(); ++point_index)
    {
      float& descriptor_distance = descriptor_distance_image[point_index];
      descriptor_distance = INFINITY;
      vector<Narf*>& narfs_of_current_point = narfs[point_index];
      if (narfs_of_current_point.empty())
        continue;
      for (unsigned int i=0; i<narfs_of_selected_point.size(); ++i)
      {
        for (unsigned int j=0; j<narfs_of_current_point.size(); ++j)
        {
          descriptor_distance = min(descriptor_distance, narfs_of_selected_point[i]->getDescriptorDistance(*narfs_of_current_point[j]));
        }
      }
    }
    descriptor_distances_widget.setFloatImage(descriptor_distance_image, scene_range_image.width, scene_range_image.height, "descriptor distances", -INFINITY, INFINITY, true);
    delete[] descriptor_distance_image;
  }
}