localization.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2015, Andreas ten Pas
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *
00018  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00019  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00020  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00021  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00022  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00023  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00024  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00026  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00027  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00028  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029  *  POSSIBILITY OF SUCH DAMAGE.
00030  */
00031 
00032 #ifndef LOCALIZATION_H_
00033 #define LOCALIZATION_H_
00034 
00035 // system dependencies
00036 #include <iostream>
00037 #include <math.h>
00038 #include <set>
00039 #include <string>
00040 #include <time.h>
00041 
00042 // PCL dependencies
00043 #include <pcl/filters/extract_indices.h>
00044 #include <pcl/filters/voxel_grid.h>
00045 #include <pcl/io/pcd_io.h>
00046 #include <pcl/point_types.h>
00047 #include <pcl/segmentation/sac_segmentation.h>
00048 
00049 // project dependencies
00050 #include <agile_grasp/grasp_hypothesis.h>
00051 #include <agile_grasp/hand_search.h>
00052 #include <agile_grasp/handle.h>
00053 #include <agile_grasp/handle_search.h>
00054 #include <agile_grasp/learning.h>
00055 #include <agile_grasp/plot.h>
00056 
00057 typedef pcl::PointCloud<pcl::PointXYZRGBA> PointCloud;
00058 
00059 
00068 class Localization
00069 {
00070 public:
00071 
00075         Localization() : num_threads_(1), plotting_mode_(1), plots_camera_sources_(false), cloud_(new PointCloud)
00076         { }
00077         
00084         Localization(int num_threads, bool filters_boundaries, int plotting_mode) :
00085                         num_threads_(num_threads), filters_boundaries_(filters_boundaries), 
00086       plotting_mode_(plotting_mode), plots_camera_sources_(false), 
00087       cloud_(new PointCloud)
00088         { }
00089         
00097         std::vector<Handle> findHandles(const std::vector<GraspHypothesis>& hand_list, int min_inliers, double min_length);
00098         
00104         std::vector<GraspHypothesis> predictAntipodalHands(const std::vector<GraspHypothesis>& hand_list,
00105                         const std::string& svm_filename);
00106         
00115         std::vector<GraspHypothesis> localizeHands(const PointCloud::Ptr& cloud_in, int size_left,
00116                         const std::vector<int>& indices, bool calculates_antipodal, bool uses_clustering);
00117         
00126         std::vector<GraspHypothesis> localizeHands(const std::string& pcd_filename_left,
00127                         const std::string& pcd_filename_right, bool calculates_antipodal = false, 
00128       bool uses_clustering = false);
00129         
00139         std::vector<GraspHypothesis> localizeHands(const std::string& pcd_filename_left,
00140                         const std::string& pcd_filename_right, const std::vector<int>& indices, 
00141       bool calculates_antipodal =       false, bool uses_clustering = false);
00142         
00148         void setCameraTransforms(const Eigen::Matrix4d& cam_tf_left, const Eigen::Matrix4d& cam_tf_right)
00149         {
00150                 cam_tf_left_ = cam_tf_left;
00151                 cam_tf_right_ = cam_tf_right;
00152         }
00153         
00159         const Eigen::Matrix4d& getCameraTransform(bool is_left)
00160         {
00161                 if (is_left)
00162                         return cam_tf_left_;
00163                 else
00164                         return cam_tf_right_;
00165         }
00166         
00171         void setWorkspace(const Eigen::VectorXd& workspace)
00172         {
00173                 workspace_ = workspace;
00174         }
00175         
00180         void setNumSamples(int num_samples)
00181         {
00182                 num_samples_ = num_samples;
00183         }
00184         
00189         void setNeighborhoodRadiusHands(double nn_radius_hands)
00190         {
00191                 nn_radius_hands_ = nn_radius_hands;
00192         }
00193         
00198         void setNeighborhoodRadiusTaubin(double nn_radius_taubin)
00199         {
00200                 nn_radius_taubin_ = nn_radius_taubin;
00201         }
00202         
00207         void setFingerWidth(double finger_width)
00208         {
00209                 finger_width_ = finger_width;
00210         }
00211         
00216         void setHandDepth(double hand_depth)
00217         {
00218                 hand_depth_ = hand_depth;
00219         }
00220         
00225         void setHandOuterDiameter(double hand_outer_diameter)
00226         {
00227                 hand_outer_diameter_ = hand_outer_diameter;
00228         }
00229         
00234         void setInitBite(double init_bite)
00235         {
00236                 init_bite_ = init_bite;
00237         }
00238         
00243         void setHandHeight(double hand_height)
00244         {
00245                 hand_height_ = hand_height;
00246         }
00247                 
00255         void createVisualsPub(ros::NodeHandle& node, double marker_lifetime, const std::string& frame)
00256         {
00257                 plot_.createVisualPublishers(node, marker_lifetime);
00258                 visuals_frame_ = frame;
00259         }
00260         
00262   static const int NO_PLOTTING = 0; 
00263   static const int PCL_PLOTTING = 1; 
00264   static const int PCL_PLOTTING_FINGERS = 2; 
00265   static const int RVIZ_PLOTTING = 3; 
00266 
00267 
00268 private:
00269 
00273         struct UniqueVectorComparator
00274         {
00281                 bool operator ()(const Eigen::Vector3i& a, const Eigen::Vector3i& b)
00282                 {
00283                         for (int i = 0; i < a.size(); i++)
00284                         {
00285                                 if (a(i) != b(i))
00286                                 {
00287                                         return a(i) < b(i);
00288                                 }
00289                         }
00290 
00291                         return false;
00292                 }
00293         };
00294         
00303         void voxelizeCloud(const PointCloud::Ptr& cloud_in, const Eigen::VectorXi& pts_cam_source_in,
00304                         PointCloud::Ptr& cloud_out, Eigen::VectorXi& pts_cam_source_out, double cell_size);
00305         
00314         void filterWorkspace(const PointCloud::Ptr& cloud_in, const Eigen::VectorXi& pts_cam_source_in,
00315                         PointCloud::Ptr& cloud_out, Eigen::VectorXi& pts_cam_source_out);
00316         
00322         std::vector<GraspHypothesis> filterHands(const std::vector<GraspHypothesis>& hand_list);
00323         
00329         Eigen::Vector3i floorVector(const Eigen::Vector3d& a);
00330 
00331         Plot plot_; 
00332         Eigen::Matrix4d cam_tf_left_, cam_tf_right_; 
00333         Eigen::VectorXd workspace_; 
00334         Eigen::Matrix3Xd cloud_normals_; 
00335         PointCloud::Ptr cloud_; 
00336         int num_threads_; 
00337         int num_samples_; 
00338         double nn_radius_taubin_; 
00339         double nn_radius_hands_; 
00340         double finger_width_; 
00341         double hand_outer_diameter_; 
00342         double hand_depth_; 
00343         double hand_height_; 
00344         double init_bite_; 
00345   bool plots_camera_sources_; 
00346         bool filters_boundaries_; 
00347         int plotting_mode_; 
00348         std::string visuals_frame_; 
00349 };
00350 
00351 #endif


agile_grasp
Author(s): Andreas ten Pas
autogenerated on Sat Jun 8 2019 20:08:27