$search

remove_duplicate_views_service.cpp

Go to the documentation of this file.

/*
 * normals_vfh_db_estimator_service.cpp
 *
 *  Created on: Jan 25, 2011
 *      Author: aaldoma
 */

#include "ros/ros.h"
#include <ros/message_traits.h>
#include <ros/serialization.h>
#include "sensor_msgs/PointCloud2.h"
#include "geometry_msgs/Pose.h"
#include "vfh_recognizer_db/remove_duplicate_views.h"
#include <pcl/common/common.h>
#include "household_objects_database/database_view.h"
#include "household_objects_database/database_vfh.h"
#include "household_objects_database/database_vfh_orientation.h"
#include "household_objects_database/objects_database.h"
#include "household_objects_database/database_helper_classes.h"
#include <fstream>
#include <iostream>
#include <pcl_ros/point_cloud.h>
#include <vfh_recognizer_db/vfh_recognition.h>
#include <pcl/registration/registration.h>
#include <pcl/registration/icp.h>
#include <pcl/segmentation/segment_differences.h>

using namespace std;
namespace ser = ros::serialization;


void
getVFHFromView (boost::shared_ptr<household_objects_database::DatabaseView> & view, pcl::PointCloud<
    pcl::VFHSignature308> & vfh_signature, household_objects_database::ObjectsDatabase & database)
{
  //database->getViewFromVFHId (vfh_id, view);
  boost::shared_ptr < household_objects_database::DatabaseVFH > vfh;
  database.getVFHFromView (vfh, view);

  //deserealize into vfh_signature
  boost::shared_array < uint8_t > bufferRead (new uint8_t[vfh->vfh_descriptor_.data ().size ()]);
  memcpy (&bufferRead[0], &(vfh->vfh_descriptor_.data ()[0]), vfh->vfh_descriptor_.data ().size ());
  ser::IStream streamIn (bufferRead.get (), vfh->vfh_descriptor_.data ().size ());
  ser::deserialize (streamIn, vfh_signature);
}

void
getPointCloudFromView (pcl::PointCloud<pcl::PointNormal> & cloud, boost::shared_ptr<
    household_objects_database::DatabaseView> & view, household_objects_database::ObjectsDatabase & database)
{
  //database->getViewFromVFHId (vfh_id, view);
  database.loadFromDatabase (&view->view_point_cloud_data_);
  boost::shared_array < uint8_t > bufferRead (new uint8_t[view->view_point_cloud_data_.data ().size ()]);
  memcpy (&bufferRead[0], &(view->view_point_cloud_data_.data ()[0]), view->view_point_cloud_data_.data ().size ());
  ser::IStream streamIn (bufferRead.get (), view->view_point_cloud_data_.data ().size ());
  ser::deserialize (streamIn, cloud);
}

bool
getUniqueViews (vfh_recognizer_db::remove_duplicate_views::Request &req,
                vfh_recognizer_db::remove_duplicate_views::Response &res)
{

  vfh_recognizer_db::VFHRecognizerDB < flann::ChiSquareDistance > vfh_recognizer;
  //vfh_recognition::VFHRecognizer<flann::HistIntersectionDistance > vfh_recognizer;

  //store in the database
  household_objects_database::ObjectsDatabase database ("wgs36.willowgarage.com", "5432", "willow", "willow",
                                                        "household_objects");
  if (!database.isConnected ())
  {
    std::cerr << "Database failed to connect \n";
    return -1;
  }
  std::cerr << "Database connected successfully \n";

  //get views from the database with req.scaled_model_id

  std::vector < boost::shared_ptr<household_objects_database::DatabaseView> > views;
  database.getViewsFromScaledModelId (req.scaled_model_id.data, req.iteration.data, views);

  if (views.size() == 0) {
    ROS_WARN("Not removing duplicate views, model has no views");
    return false;
  }

  //build roll histograms
  std::vector<pcl::PointCloud<pcl::VFHSignature308>::Ptr> roll_histograms;
  roll_histograms.resize (views.size ());
  std::vector<Eigen::Vector4f> centroids;
  centroids.resize (views.size ());

  for (size_t i = 0; i < views.size (); i++)
  {
    boost::shared_ptr < household_objects_database::DatabaseView > view;
    view = views.at (i);

    //get point cloud from view
    pcl::PointCloud < pcl::PointNormal >::Ptr cloud_normals(new pcl::PointCloud<pcl::PointNormal> ());
    getPointCloudFromView (*cloud_normals, view, database);

    //compute centroid
    Eigen::Vector4f centroid;
    pcl::compute3DCentroid (*cloud_normals, centroid);
    Eigen::Vector3f ci3f (centroid[0], centroid[1],centroid[2]);

    //compute roll angle orientation
    pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_o_signature (new pcl::PointCloud<pcl::VFHSignature308> ());
    cv::Mat frequency_domain;
    vfh_recognizer.computeVFHRollOrientation (cloud_normals, vfh_o_signature, ci3f,frequency_domain);

    roll_histograms[i] = vfh_o_signature;
    centroids[i] = centroid;
  }

  //check for duplicates
  std::vector<bool> unique (views.size (), true);
  //PCLVisualizer vis ("VFH Cluster Classifier");
  //PCLVisualizer vis_diff ("Difference");

  for (size_t i = 0; i < views.size (); i++)
  {

    boost::shared_ptr < household_objects_database::DatabaseView > view_i;
    view_i = views.at (i);

    Eigen::Vector3f plane_normal;
    plane_normal[0] = -centroids.at(i)[0];
    plane_normal[1] = -centroids.at(i)[1];
    plane_normal[2] = -centroids.at(i)[2];
    Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
    plane_normal.normalize ();
    Eigen::Vector3f axis = plane_normal.cross (z_vector);
    double rotation = -asin (axis.norm ());
    axis.normalize ();
    Eigen::Affine3f transformRM (Eigen::AngleAxisf (rotation, axis));

    pcl::PointCloud<pcl::VFHSignature308>::Ptr roll_i_ptr;
    roll_i_ptr = roll_histograms[i]; //access the correct roll histogram

    pcl::PointCloud < pcl::PointNormal >::Ptr cloud_normals_i(new pcl::PointCloud<pcl::PointNormal> ());
    getPointCloudFromView (*cloud_normals_i, view_i, database);

    Eigen::Vector4f centroid_input (centroids.at(i)[0], centroids.at(i)[1], centroids.at(i)[2], 0);

    pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_o_signature_trash (new pcl::PointCloud<pcl::VFHSignature308> ());
    Eigen::Vector3f ci3f (centroids.at(i)[0], centroids.at(i)[1], centroids.at(i)[2]);
    cv::Mat frequency_domain_input;
    vfh_recognizer.computeVFHRollOrientation (cloud_normals_i, vfh_o_signature_trash, ci3f, frequency_domain_input);

    for (size_t j = (i+1); j < views.size (); j++) {
      //compare view i with the rest of the views and mark it as unique or not unique
      /*vis.removePointCloud ("cloud_i");
      vis.removePointCloud ("cloud_j");
      vis_diff.removePointCloud ("difference");*/

      boost::shared_ptr < household_objects_database::DatabaseView > view_j;
      view_j = views.at (j);

      pcl::PointCloud<pcl::VFHSignature308>::Ptr roll_j_ptr;
      roll_j_ptr = roll_histograms[j]; //access the correct roll histogram

      Eigen::Vector4f centroid_view (centroids.at(j)[0], centroids.at(j)[1], centroids.at(j)[2], 0);

      pcl::PointCloud < pcl::PointNormal >::Ptr cloud_normals_j(new pcl::PointCloud<pcl::PointNormal> ());
      getPointCloudFromView (*cloud_normals_j, view_j, database);

      pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_o_signature_trash (new pcl::PointCloud<pcl::VFHSignature308> ());
      Eigen::Vector3f ci3f (centroids.at(j)[0], centroids.at(j)[1], centroids.at(j)[2]);
      cv::Mat frequency_domain_view;
      vfh_recognizer.computeVFHRollOrientation (cloud_normals_j, vfh_o_signature_trash, ci3f, frequency_domain_view);

      //compute roll angle!
      int angle = 0;
      angle = vfh_recognizer.computeAngleRollOrientationFrequency (frequency_domain_input, frequency_domain_view, 90, centroid_view, centroid_input , *cloud_normals_j, *cloud_normals_i, 0, *vfh_o_signature_trash, *vfh_o_signature_trash);
      //TODO: Fix this!! computeAngleRollOrientationFrequency

      //std::cout << "Computed angle:" << angle << std::endl;
      plane_normal[0] = -centroids.at(j)[0];
      plane_normal[1] = -centroids.at(j)[1];
      plane_normal[2] = -centroids.at(j)[2];
      plane_normal.normalize ();
      axis = plane_normal.cross (z_vector);
      rotation = -asin (axis.norm ());
      axis.normalize ();
      Eigen::Affine3f transformRS (Eigen::AngleAxisf (rotation, axis));
      transformRS = transformRS.inverse(); //invert transformation
      //roll transformation

      Eigen::Affine3f transformRoll(Eigen::AngleAxisf((angle*M_PI/180), Eigen::Vector3f::UnitZ()));
      Eigen::Affine3f final_trans = transformRS * transformRoll * transformRM;

      pcl::PointCloud < pcl::PointNormal > cloud_xyz;
      pcl::PointCloud < pcl::PointNormal >::Ptr gridTransformed (new pcl::PointCloud < pcl::PointNormal > ());
      pcl::transformPointCloudWithNormals(*cloud_normals_j,*gridTransformed,final_trans);
      cloud_xyz = *gridTransformed;

      // Demean the cloud
      pcl::PointCloud < pcl::PointNormal >::Ptr cloud_xyz_demean(new pcl::PointCloud<pcl::PointNormal> ());
      Eigen::Vector4f diff = Eigen::Vector4f::Zero();

      Eigen::Vector3f centr(centroids.at(j)[0],centroids.at(j)[1],centroids.at(j)[2]);
      centr = final_trans * centr;

      diff[0] = -centroids.at(i)[0] + centr[0];
      diff[1] = -centroids.at(i)[1] + centr[1];
      diff[2] = -centroids.at(i)[2] + centr[2];

      pcl::demeanPointCloud (cloud_xyz, diff, *cloud_xyz_demean);

      //we can compare cloud_xyz_demean with cloud_normals_i
      pcl::IterativeClosestPoint < pcl::PointNormal, pcl::PointNormal > reg;
      reg.setInputCloud (cloud_xyz_demean);
      reg.setInputTarget (cloud_normals_i);
      double fscore = reg.getFitnessScore (0.005);

      //segment difference
      pcl::SegmentDifferences<pcl::PointNormal> seg;
      typedef pcl::KdTree<pcl::PointNormal>::Ptr KdTreePtr;
      KdTreePtr normals_tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointNormal> > (true);
      //normals_tree->setInputCloud (cloud_xyz_demean);
      seg.setDistanceThreshold (0.01*0.01); //1cm
      seg.setSearchMethod(normals_tree);
      seg.setInputCloud(cloud_normals_i);
      seg.setTargetCloud(cloud_xyz_demean);

      pcl::PointCloud < pcl::PointNormal > difference;
      seg.segment(difference);

      //std::cout << "Fitness score:" << fscore << std::endl;
      fscore = (double)(difference.points.size()) / (double)(cloud_normals_i->points.size());
      std::cout << "% of remaining points:" << fscore << " i:" << i << " j:" << j << " size diff:" << difference.points.size() << std::endl;
      if (fscore < 0.025) { //if less than 2.5% of the points have their nearest neighbor outside threshold, then view is not unique
        //std::cout << "View is not unique" << std::endl;
        unique[i] = false;
        break; //we have found at least one view after i that is the same
      }
    }
  }

  //remove duplicate views and descriptors from the database

  int unique_views = 0;
  for (size_t i = 0; i < unique.size (); i++)
  {
    if (!unique.at (i)) //if its not unique, delete it from database
    {

      std::cout << "View "<< i << " not unique" << std::endl;
      //get the view from the vfh_id
      boost::shared_ptr < household_objects_database::DatabaseView > view;
      view = views.at(i);

      //get roll histograms and delete them
      std::vector < boost::shared_ptr<household_objects_database::DatabaseVFHOrientation> > vfhOrientations;
      std::stringstream where;
      where << "view_id =" << view->view_id_.data ();
      std::string where_clause (where.str ());
      if (!database.getList (vfhOrientations, where_clause)) {
        ROS_WARN("NO VFH orientation found\n");
      } else {
        for (size_t j=0; j < vfhOrientations.size(); j++) {
          database.deleteFromDatabase(vfhOrientations[j].get());
        }
      }

      //get VFH histogram and delete them
      std::vector < boost::shared_ptr<household_objects_database::DatabaseVFH> > vfhs;
      if (!database.getList (vfhs, where_clause)) {
        ROS_WARN("NO VFH found\n");
      } else {
        for (size_t j=0; j < vfhOrientations.size(); j++) {
          database.deleteFromDatabase(vfhs[j].get());
        }
      }

      //delete view
      database.deleteFromDatabase(view.get());
    } else {
      unique_views++;
    }
  }

  std::cout << "Number of unique views:" << unique_views << std::endl;

  return true;
}

int
main (int argc, char **argv)
{
  ros::init (argc, argv, "remove_duplicate_views_server");
  ros::NodeHandle n;

  ros::ServiceServer service = n.advertiseService ("remove_duplicate_views", getUniqueViews);
  ros::spin ();

  return 0;
}

---

vfh_recognizer_db
Author(s): Aitor Aldoma
autogenerated on Tue Mar 5 15:26:31 2013