localization.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, Andreas ten Pas
 *  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.
 *
 *  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.
 */

#ifndef LOCALIZATION_H_
#define LOCALIZATION_H_

// system dependencies
#include <iostream>
#include <math.h>
#include <set>
#include <string>
#include <time.h>

// PCL dependencies
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/segmentation/sac_segmentation.h>

// project dependencies
#include <agile_grasp/grasp_hypothesis.h>
#include <agile_grasp/hand_search.h>
#include <agile_grasp/handle.h>
#include <agile_grasp/handle_search.h>
#include <agile_grasp/learning.h>
#include <agile_grasp/plot.h>

typedef pcl::PointCloud<pcl::PointXYZRGBA> PointCloud;


class Localization
{
public:

        Localization() : num_threads_(1), plotting_mode_(1), plots_camera_sources_(false), cloud_(new PointCloud)
        { }
        
        Localization(int num_threads, bool filters_boundaries, int plotting_mode) :
                        num_threads_(num_threads), filters_boundaries_(filters_boundaries), 
      plotting_mode_(plotting_mode), plots_camera_sources_(false), 
      cloud_(new PointCloud)
        { }
        
        std::vector<Handle> findHandles(const std::vector<GraspHypothesis>& hand_list, int min_inliers, double min_length);
        
        std::vector<GraspHypothesis> predictAntipodalHands(const std::vector<GraspHypothesis>& hand_list,
                        const std::string& svm_filename);
        
        std::vector<GraspHypothesis> localizeHands(const PointCloud::Ptr& cloud_in, int size_left,
                        const std::vector<int>& indices, bool calculates_antipodal, bool uses_clustering);
        
        std::vector<GraspHypothesis> localizeHands(const std::string& pcd_filename_left,
                        const std::string& pcd_filename_right, bool calculates_antipodal = false, 
      bool uses_clustering = false);
        
        std::vector<GraspHypothesis> localizeHands(const std::string& pcd_filename_left,
                        const std::string& pcd_filename_right, const std::vector<int>& indices, 
      bool calculates_antipodal =       false, bool uses_clustering = false);
        
        void setCameraTransforms(const Eigen::Matrix4d& cam_tf_left, const Eigen::Matrix4d& cam_tf_right)
        {
                cam_tf_left_ = cam_tf_left;
                cam_tf_right_ = cam_tf_right;
        }
        
        const Eigen::Matrix4d& getCameraTransform(bool is_left)
        {
                if (is_left)
                        return cam_tf_left_;
                else
                        return cam_tf_right_;
        }
        
        void setWorkspace(const Eigen::VectorXd& workspace)
        {
                workspace_ = workspace;
        }
        
        void setNumSamples(int num_samples)
        {
                num_samples_ = num_samples;
        }
        
        void setNeighborhoodRadiusHands(double nn_radius_hands)
        {
                nn_radius_hands_ = nn_radius_hands;
        }
        
        void setNeighborhoodRadiusTaubin(double nn_radius_taubin)
        {
                nn_radius_taubin_ = nn_radius_taubin;
        }
        
        void setFingerWidth(double finger_width)
        {
                finger_width_ = finger_width;
        }
        
        void setHandDepth(double hand_depth)
        {
                hand_depth_ = hand_depth;
        }
        
        void setHandOuterDiameter(double hand_outer_diameter)
        {
                hand_outer_diameter_ = hand_outer_diameter;
        }
        
        void setInitBite(double init_bite)
        {
                init_bite_ = init_bite;
        }
        
        void setHandHeight(double hand_height)
        {
                hand_height_ = hand_height;
        }
                
        void createVisualsPub(ros::NodeHandle& node, double marker_lifetime, const std::string& frame)
        {
                plot_.createVisualPublishers(node, marker_lifetime);
                visuals_frame_ = frame;
        }
        
  static const int NO_PLOTTING = 0; 
  static const int PCL_PLOTTING = 1; 
  static const int PCL_PLOTTING_FINGERS = 2; 
  static const int RVIZ_PLOTTING = 3; 


private:

        struct UniqueVectorComparator
        {
                bool operator ()(const Eigen::Vector3i& a, const Eigen::Vector3i& b)
                {
                        for (int i = 0; i < a.size(); i++)
                        {
                                if (a(i) != b(i))
                                {
                                        return a(i) < b(i);
                                }
                        }

                        return false;
                }
        };
        
        void voxelizeCloud(const PointCloud::Ptr& cloud_in, const Eigen::VectorXi& pts_cam_source_in,
                        PointCloud::Ptr& cloud_out, Eigen::VectorXi& pts_cam_source_out, double cell_size);
        
        void filterWorkspace(const PointCloud::Ptr& cloud_in, const Eigen::VectorXi& pts_cam_source_in,
                        PointCloud::Ptr& cloud_out, Eigen::VectorXi& pts_cam_source_out);
        
        std::vector<GraspHypothesis> filterHands(const std::vector<GraspHypothesis>& hand_list);
        
        Eigen::Vector3i floorVector(const Eigen::Vector3d& a);

        Plot plot_; 
        Eigen::Matrix4d cam_tf_left_, cam_tf_right_; 
        Eigen::VectorXd workspace_; 
        Eigen::Matrix3Xd cloud_normals_; 
        PointCloud::Ptr cloud_; 
        int num_threads_; 
        int num_samples_; 
        double nn_radius_taubin_; 
        double nn_radius_hands_; 
        double finger_width_; 
        double hand_outer_diameter_; 
        double hand_depth_; 
        double hand_height_; 
        double init_bite_; 
  bool plots_camera_sources_; 
        bool filters_boundaries_; 
        int plotting_mode_; 
        std::string visuals_frame_; 
};

#endif