00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id:$ 00005 * 00006 * Copyright (C) Brno University of Technology 00007 * 00008 * This file is part of software developed by dcgm-robotics@FIT group. 00009 * 00010 * Author: Vit Stancl (stancl@fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: dd/mm/2012 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00026 */ 00027 00028 #pragma once 00029 #ifndef OCTOMAP_FILTER_RAYCAST_H_ 00030 #define OCTOMAP_FILTER_RAYCAST_H_ 00031 00032 #include "octomap_filter_base.h" 00033 #include <tf/transform_listener.h> 00034 #include <sensor_msgs/CameraInfo.h> 00035 #include <image_geometry/pinhole_camera_model.h> 00036 #include <pcl/filters/voxel_grid.h> 00037 00038 namespace srs_env_model 00039 { 00040 00041 class COcFilterRaycast : public COcTreeFilterBase 00042 { 00043 public: 00045 COcFilterRaycast(const std::string & octree_frame_id, ERunMode mode = FILTER_ALLWAYS); 00046 00048 virtual void init(ros::NodeHandle & node_handle); 00049 00051 void setCloud(tPointCloudConstPtr cloud); 00052 00054 virtual void writeLastRunInfo(); 00055 00056 protected: 00058 virtual void filterInternal( tButServerOcTree & tree ); 00059 00061 void cameraInfoCB(const sensor_msgs::CameraInfo::ConstPtr &cam_info); 00062 00064 octomap::point3d getSensorOrigin(const std_msgs::Header& sensor_header); 00065 00067 bool inSensorCone(const cv::Point2d& uv) const; 00068 00070 bool isOccludedMap(const octomap::point3d& sensor_origin, const octomap::point3d& p, double resolution, tButServerOcTree & tree) const; 00071 00073 void computeBBX(const std_msgs::Header& sensor_header, octomap::point3d& bbx_min, octomap::point3d& bbx_max); 00074 00075 protected: 00077 std_msgs::Header m_sensor_header; 00078 00080 bool m_bFilterInitialized; 00081 00083 tf::TransformListener m_tfListener; 00084 00086 int m_camera_stereo_offset_left, m_camera_stereo_offset_right; 00087 00089 cv::Size m_camera_size; 00090 00092 image_geometry::PinholeCameraModel m_camera_model; 00093 00095 bool m_bCamModelInitialized; 00096 00098 boost::mutex m_lockCamera; 00099 00101 boost::mutex m_lockData; 00102 00104 std::string m_camera_info_topic; 00105 00107 ros::Subscriber m_ciSubscriber; 00108 00110 tPointCloudConstPtr m_cloudPtr; 00111 00113 long m_numLeafsRemoved; 00114 00116 long m_numLeafsOutOfCone; 00117 00118 octomap::point3d m_sensor_origin; 00119 00121 ros::Publisher marker_pub_; 00122 00124 ros::Publisher grid_pub_; 00125 00127 pcl::VoxelGrid<tPclPoint> m_vgfilter; 00128 00130 tPointCloud::Ptr m_filtered_cloud; 00131 00133 float m_miss_speedup; 00134 00135 }; // class COcFilterRaycast 00136 00137 } // namespace srs_env_model 00138 00139 #endif /* OCTOMAP_FILTER_RAYCAST_H_ */