Locate_Server.cpp

Go to the documentation of this file.

#include "ros/ros.h"
#include "narf_recognition/LocateObjects.h"
#include <iostream>
#include <exception>
using namespace std;

#include <narf_recognition/object_recognition_helper.h>
#include <boost/thread/thread.hpp>
#include "pcl/console/parse.h"

#include "pcl/visualization/range_image_visualizer.h"
#include "pcl/visualization/pcl_visualizer.h"

#include "Eigen/Geometry"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"

#include "pcl/io/pcd_io.h"
#include "pcl/range_image/range_image.h"
#include "pcl/features/narf.h"
//#include "object_recognition/object_model_list.h"
#include "pcl/common/point_correspondence.h"
#include "pcl/common/poses_from_matches.h"
#include "pcl/features/range_image_border_extractor.h"
#include "pcl/keypoints/narf_keypoint.h"
#include "pcl/range_image/range_image_planar.h"
#include "sensor_msgs/Image.h"
#include "pcl/common/file_io.h"
//#include "stereo_msgs/DisparityImage.h"
//#include "sensor_msgs/CameraInfo.h"

#include "pcl/common/common_headers.h"

#include "ros/ros.h"

#include <pcl/visualization/cloud_viewer.h>
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/foreach.hpp>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include "pcl/visualization/pcl_visualizer.h"
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/console/parse.h>
#include <cstdlib>
#include <iostream>
#include <boost/thread/thread.hpp>
#include "pcl/range_image/range_image.h"
#include "pcl/features/range_image_border_extractor.h"
#include <pcl_ros/impl/transforms.hpp>
#include "narf_recognition/Locate_Server.h"

using namespace pcl;
using namespace pcl::visualization;

Locate_Server::Locate_Server(ros::NodeHandle& n) :
                nh(n) {
        noise_level_factor = 0.1; // Factor regarding average model radius
        support_size_factor = 0.5f; // Factor regarding average model radius

        show_model_views = 1;
        show_view_simulation_results = 0;
        show_validation_points = 0;
        show_found_objects = 1;
        show_feature_match_positions = 0;
        show_interest_points = 0;
        print_timings = true;

        cloud_received = false;
        got_new_data = false;

//      ObjectModelList& object_models =
//                      object_recognition_helper.getIntermediateElements().object_models;

        viewer = new pcl::visualization::PCLVisualizer("3D Viewer");
        ObjectRecognitionHelper::Parameters& orh_params =
                        object_recognition_helper.getParameters();
        FalsePositivesFilter& false_positives_filter =
                        object_recognition_helper.getFalsePositivesFilter();
        FalsePositivesFilter::Parameters& fpf_params =
                        false_positives_filter.getParameters();

        orh_params.view_sampling_first_layer = 0;
        orh_params.view_sampling_last_layer = 0;

        set_unseen_to_max_range = true;
        fpf_params.do_icp = false;
        max_no_of_threads = 2;
        angular_resolution = deg2rad(0.3);
        use_rotation_invariance = true;

        vector<RangeImage> view_simulation_results;
        if (show_view_simulation_results)
                false_positives_filter.setViewSimulationImagesPtr(
                                &view_simulation_results);

        sub = nh.subscribe<sensor_msgs::PointCloud2>("/camera/rgb/points", 1,
                        &Locate_Server::cloud_cb_, this);

        XmlRpc::XmlRpcValue file_list;

        std::vector<std::string> filenames;
        std::string key;
        key.assign("locate_objects_server/filenames");
        n.getParam(key, file_list);

        for (unsigned int i = 0; i < file_list.size(); i++) {
                filenames.push_back(file_list[i]);
                std::cout << filenames[i] << std::endl;
        }
        std::cout << "got filenames. \n";
        if (got_new_data) {
                getModels(filenames);
        }

        got_new_data = false;

        ros::Rate r(10);
        while (!cloud_received) {
                ros::spinOnce();
                r.sleep();
        }

        service = n.advertiseService("locate_objects", &Locate_Server::locate, this);

        ROS_INFO("Ready to locate objects.");



}

Locate_Server::~Locate_Server() {
        delete viewer;
}
void Locate_Server::getModels(std::vector<string> model_filenames) {
        if (model_filenames.size() == 0) {
                cout << "No*.pcd file for model given.\n\n";
                exit(0);
        }

        object_models =
                        object_recognition_helper.getIntermediateElements().object_models;
        if (!object_recognition_helper.addObjectModelsFromPcdFiles(
                        model_filenames)) {

                exit(1);
        }

        float average_model_radius = object_models.getAverageModelRadius();
        float support_size = support_size_factor * average_model_radius;
        float noise_level = noise_level_factor * average_model_radius;
        cout << "\n-----\n" << "The average object model radius is "
                        << average_model_radius << "m.\n" << "  => using support size "
                        << support_size << "m.\n" << "  => using noise level "
                        << noise_level << "m.\n" << "-----\n\n";
        orh_params.support_size = support_size;
        orh_params.noise_level = noise_level;

}

void Locate_Server::cloud_cb_(const sensor_msgs::PointCloud2 msg) {

        PointCloud<PointXYZ> point_cloud;
        PointCloud<PointWithViewpoint> far_ranges;
        fromROSMsg(msg, point_cloud);
//      point_cloud.sensor_origin_ = msg.cloud_msg_sensor_origin;
//      point_cloud.sensor_orientation_ = cloud_msg_sensor_orientation;
        RangeImage::extractFarRanges(msg, far_ranges);
        if (!object_recognition_helper.createRangeImageFromPointCloud(point_cloud,
                        &far_ranges)) {
                std::cout << "Failed to create Range Image From Point Cloud. \n";
        }
        got_new_data = true;
        cloud_received = true;
}

void Locate_Server::visualize(
                PCLVisualizer& viewer,
                std::vector<PosesFromMatches::PoseEstimatesVector>& pose_estimates_per_model,
                std::vector<string>& point_clouds_in_viewer) {
        boost::shared_ptr<RangeImage>& scene_range_image_ptr =
                        object_recognition_helper.getIntermediateElements().scene_range_image_ptr;

        vector<boost::shared_ptr<ObjectModel::PointCloudType> > objects_in_scene; // For visualization
        for (unsigned int model_idx = 0; model_idx < object_models.size();
                        ++model_idx) {

                PosesFromMatches::PoseEstimatesVector& pose_estimates =
                                pose_estimates_per_model[model_idx];
                ObjectModel& object_model = *object_models[model_idx];

                for (unsigned int pose_estimate_idx = 0;
                                pose_estimate_idx < pose_estimates.size() && show_found_objects;
                                ++pose_estimate_idx) {
                        const PosesFromMatches::PoseEstimate& pose_estimate =
                                        pose_estimates[pose_estimate_idx];
                        objects_in_scene.push_back(
                                        boost::shared_ptr<ObjectModel::PointCloudType>(
                                                        new ObjectModel::PointCloudType));
                        object_model.getTransformedPointCloud(pose_estimate.transformation,
                                        *objects_in_scene.back());
                }
        }

//              ObjectModel& first_object_model = *object_models[0];
        if (objects_in_scene.size() > 0) {
                for (unsigned int i = 0; i < point_clouds_in_viewer.size(); ++i)
                        viewer.removePointCloud(point_clouds_in_viewer[i]);

                point_clouds_in_viewer.clear();

                //viewer.addCoordinateSystem (1.0f);
                PointCloudColorHandlerCustom<pcl::PointWithRange> color_handler_cloud(
                                scene_range_image_ptr, 0, 0, 0);
                point_clouds_in_viewer.push_back("scene_range_image");
                viewer.addPointCloud<PointWithRange>(scene_range_image_ptr,
                                color_handler_cloud, point_clouds_in_viewer.back());
                point_clouds_in_viewer.push_back("feature_match_positions");
                viewer.setPointCloudRenderingProperties(PCL_VISUALIZER_POINT_SIZE, 7,
                                point_clouds_in_viewer.back());
                for (unsigned int i = 0; i < objects_in_scene.size(); ++i) {
                        stringstream ss;
                        ss << "Found object " << i;
                        point_clouds_in_viewer.push_back(ss.str());
                        PointCloudColorHandlerRandom<ObjectModel::PointCloudType::PointType> color_handler(
                                        objects_in_scene[i]);
                        viewer.addPointCloud<ObjectModel::PointCloudType::PointType>(
                                        objects_in_scene[i], color_handler,
                                        point_clouds_in_viewer.back());
                }
        }

}

bool Locate_Server::locate(narf_recognition::LocateObjects::Request &req,
                narf_recognition::LocateObjects::Response &res) {

        std::cout << "started locating. \n";

        object_recognition_helper.extractModelFeatures();
        object_recognition_helper.getIntermediateElements().database_feature_sources;
        std::cout << "Got Model Features.\n";

        viewer->setBackgroundColor(1, 1, 1);

        // ------------------------------------------------------------------

        vector<string> point_clouds_in_viewer;
        RangeImageVisualizer scene_range_image_widget("scene range image");
        vector<RangeImageVisualizer*> view_simulation_result_widgets;
        vector<RangeImageVisualizer*> interest_points_widgets;
        vector<RangeImageVisualizer*> range_image_widgets;

        if (!got_new_data) {
                if (!cloud_received) {
                        std::cout << "No cloud received yet. \n";
                }
                return false;
//                              continue;
        }
        got_new_data = false;
        cout << "Got new data.\n";

        object_recognition_helper.extractSceneFeatures();
        object_recognition_helper.extractPoseEstimates();
        std::vector<PosesFromMatches::PoseEstimatesVector>& pose_estimates_per_model =
                        object_recognition_helper.filterFalsePositives();
        object_recognition_helper.printTimings();

        std::vector<PosesFromMatches::PoseEstimatesVector> poses =
                        object_recognition_helper.getIntermediateElements().pose_estimates_per_model;
        size_t numPoseEstimates = poses.size();

        for (unsigned int i = 0; i < numPoseEstimates; i++) {
                for (unsigned int j = 0; j < poses[i].size(); j++) {
                        Eigen::Affine3f eigen_pose = poses[i][j].transformation;

                        Eigen::Quaternionf rot(eigen_pose.rotation());
                        Eigen::Vector3f origin(eigen_pose.translation());

                        geometry_msgs::Pose pose;
                        pose.position.x = origin[0];
                        pose.position.y = origin[1];
                        pose.position.z = origin[2];

                        pose.orientation.w = rot.w();
                        pose.orientation.x = rot.x();
                        pose.orientation.y = rot.y();
                        pose.orientation.z = rot.z();
                        res.pos.push_back(pose);
                }
        }

        visualize(*viewer, pose_estimates_per_model, point_clouds_in_viewer);
        viewer->spinOnce(100);
//              }

        ROS_INFO("sending back response.");
        return true;

        //return false;
}

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

        Locate_Server locator(n);
        while (ros::ok() && !locator.viewer->wasStopped()) {
                locator.viewer->spinOnce(100);
                ros::spinOnce();
        }
        return 0;
}