$search

create_descriptors_from_views.cpp

Go to the documentation of this file.

#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/normals_vfh_db_estimator.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>

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

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
household_objects_database::ObjectsDatabase * database;

void
getPointCloudFromView (pcl::PointCloud<pcl::PointNormal> & cloud, boost::shared_ptr<
    household_objects_database::DatabaseView> & view)
{
  //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);
}

int
main (int argc, char **argv)
{
  ros::init (argc, argv, "create_descriptors");
  database = new household_objects_database::ObjectsDatabase ("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";

  //read point cloud from database
  std::vector<boost::shared_ptr<household_objects_database::DatabaseView> > views;
  std::stringstream where;
  where << "iteration = " << atoi (argv[1]);
  std::string where_clause (where.str ());
  if (!database->getList (views, where_clause))
  {
    std::cout << "No views returned" << std::endl;
    return false;
  }

  int new_iteration = atoi (argv[2]);
  std::cout << "NUMBER OF VIEWS:" << views.size () << std::endl;

  //we can call the service with the views...
  for (size_t i = 0; i < views.size (); i++)
  {
    //read from raw views
    //pcl::PointCloud < pcl::PointXYZ >::Ptr cloud (new  pcl::PointCloud < pcl::PointXYZ > ());

    boost::shared_ptr< household_objects_database::DatabaseView> view;
    view = views.at(i);
    sensor_msgs::PointCloud2Ptr cloud(new sensor_msgs::PointCloud2());
    std::stringstream raw_path;
    raw_path << "/u/aaldoma/data/raw_views_iteration_17/view_" << view->view_id_.data () << ".pcd";
    std::cout << raw_path.str() << std::endl;
    pcl::io::loadPCDFile (raw_path.str(), *cloud);

    ros::NodeHandle n;
    ros::ServiceClient client = n.serviceClient<vfh_recognizer_db::normals_vfh_db_estimator>("normals_vfh_db_estimator");
    vfh_recognizer_db::normals_vfh_db_estimator srv;
    srv.request.view = *cloud;
    srv.request.transform = view->view_transform_.data ().pose_;
    srv.request.model_id.data = view->scaled_model_id_.data ();
    srv.request.iteration.data = new_iteration;

    if (client.call(srv))
    {
      ROS_INFO("Call succeeded");
    }
    else
    {
      ROS_ERROR("Failed to call service normals_vfh_db_estimator");
      return 1;
    }

  }

  return 0;
}

---

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