$search

update_vfh_descriptors.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)
{

  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;
  }

  vfh_recognizer_db::VFHRecognizerDB < flann::ChiSquareDistance > vfh_recognizer;
  vfh_recognizer.setApplyVoxelGrid (false);
  vfh_recognizer.setApplyRadiusRemoval (false);

  std::cout << "NUMBER OF VIEWS:" << views.size () << std::endl;

  for (size_t i = 0; i < views.size (); i++)
  {
    pcl::PointCloud < pcl::PointNormal > cloud_xyz_normals;
    getPointCloudFromView (cloud_xyz_normals, views[i]);

    //remove old VFH
    std::vector<boost::shared_ptr<household_objects_database::DatabaseVFH> > vfhs;
    std::stringstream where;
    where << "view_id = " << views[i]->view_id_.data ();
    std::string where_clause (where.str ());
    if (database->getList (vfhs, where_clause))
    {

      if (vfhs.size () > 0)
      {
        std::cout << "Removing old vfh:" << i << std::endl;
        database->deleteFromDatabase (vfhs[0].get ());
      }
    }

    //compute VFH
    pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_signaturePtr (new pcl::PointCloud<pcl::VFHSignature308> ());
    pcl::PointCloud < pcl::VFHSignature308 > vfh_signature;
    vfh_recognizer.computeVFH (cloud_xyz_normals.makeShared (), vfh_signaturePtr);
    vfh_signature = *vfh_signaturePtr;

    household_objects_database::DatabaseVFH vfh_db;
    vfh_db.view_id_.data () = views[i]->view_id_.data ();
    vfh_db.iteration_.data () = views[i]->iteration_.data ();

    int msg_length_vfh = ser::serializationLength (vfh_signature);
    boost::shared_array<uint8_t> buffer_vfh (new uint8_t[msg_length_vfh]);
    ser::OStream stream_vfh (buffer_vfh.get (), msg_length_vfh);
    ser::serialize (stream_vfh, vfh_signature);

    //copy the buffer in the underlying view structure
    vfh_db.vfh_descriptor_.data ().resize (msg_length_vfh);
    memcpy (&vfh_db.vfh_descriptor_.data ()[0], &buffer_vfh[0], vfh_db.vfh_descriptor_.data ().size ());

    if (!database->insertIntoDatabase (&vfh_db))
      std::cerr << "VFH insertion failed\n";
    else
      std::cout << "Inserting new vfh:" << i << std::endl;

    if (!database->saveToDatabase (&vfh_db.vfh_descriptor_))
      std::cerr << "VFH descriptor insertion failed\n";

  }

  return 0;
}

---

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