kinfu.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2017, Kentaro Wada and JSK Lab
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/o2r other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Kentaro Wada and JSK Lab nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #ifndef JSK_PCL_ROS_KINFU_H_
00036 #define JSK_PCL_ROS_KINFU_H_
00037 
00038 #include <pcl/console/parse.h>
00039 #include <pcl/gpu/kinfu_large_scale/kinfu.h>
00040 #include <pcl/gpu/kinfu_large_scale/marching_cubes.h>
00041 #include <pcl/gpu/kinfu_large_scale/raycaster.h>
00042 #include <pcl/gpu/containers/initialization.h>
00043 #include <pcl/surface/texture_mapping.h>
00044 #include <pcl/point_cloud.h>
00045 #include <pcl/point_types.h>
00046 #include <pcl/common/angles.h>
00047 
00048 #include <dynamic_reconfigure/server.h>
00049 #include <message_filters/subscriber.h>
00050 #include <message_filters/sync_policies/exact_time.h>
00051 #include <message_filters/sync_policies/approximate_time.h>
00052 #include <jsk_topic_tools/connection_based_nodelet.h>
00053 #include <jsk_recognition_utils/pcl_conversion_util.h>
00054 #include <pcl_conversions/pcl_conversions.h>
00055 #include <std_srvs/Empty.h>
00056 #include <sensor_msgs/Image.h>
00057 #include <sensor_msgs/PointCloud2.h>
00058 #include <jsk_rviz_plugins/OverlayText.h>
00059 #include <jsk_pcl_ros/tf_listener_singleton.h>
00060 #include <tf/transform_broadcaster.h>
00061 
00062 #include "jsk_pcl_ros/KinfuConfig.h"
00063 
00064 // defined in pcl/gpu/kinfu_large_scale/src/kinfu.cpp
00065 namespace pcl
00066 {
00067   namespace gpu
00068   {
00069     namespace kinfuLS
00070     {
00071       void paint3DView (const KinfuTracker::View& rgb24, KinfuTracker::View& view, float colors_weight = 0.5f);
00072     }
00073   }
00074 }
00075 
00076 namespace jsk_pcl_ros
00077 {
00078   class Kinfu: public jsk_topic_tools::ConnectionBasedNodelet
00079   {
00080   public:
00081     typedef message_filters::sync_policies::ExactTime<
00082       sensor_msgs::CameraInfo, sensor_msgs::Image> SyncPolicy;
00083     typedef message_filters::sync_policies::ApproximateTime<
00084       sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::Image> SyncPolicyWithColor;
00085     typedef jsk_pcl_ros::KinfuConfig Config;
00086 
00087     Kinfu(): ConnectionBasedNodelet(), frame_idx_(0) {}
00088     ~Kinfu() {}
00089   protected:
00090     virtual void onInit();
00091     virtual void subscribe();
00092     virtual void unsubscribe();
00093 
00094     void initKinfu(const sensor_msgs::CameraInfo::ConstPtr& caminfo_msg);
00095     void update(const sensor_msgs::CameraInfo::ConstPtr& caminfo_msg,
00096                 const sensor_msgs::Image::ConstPtr& depth_msg);
00097     void update(const sensor_msgs::CameraInfo::ConstPtr& caminfo_msg,
00098                 const sensor_msgs::Image::ConstPtr& depth_msg,
00099                 const sensor_msgs::Image::ConstPtr& rgb_msg);
00100     bool resetCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
00101 
00102     boost::shared_ptr <dynamic_reconfigure::Server<Config> > srv_;
00103     virtual void configCallback(Config &config, uint32_t level);
00104 
00105     pcl::PolygonMesh createPolygonMesh();
00106     pcl::PolygonMesh createPolygonMesh(const jsk_recognition_msgs::BoundingBox& box_msg,
00107                                        const std::string& ground_frame_id);
00108     pcl::TextureMesh convertToTextureMesh(const pcl::PolygonMesh& triangles,
00109                                           const std::vector<cv::Mat> textures,
00110                                           pcl::texture_mapping::CameraVector cameras);
00111     bool saveMeshCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
00112     bool saveMeshWithContextCallback(
00113       jsk_recognition_msgs::SaveMesh::Request& req, jsk_recognition_msgs::SaveMesh::Response& res);
00114 
00115     boost::shared_ptr<pcl::gpu::kinfuLS::KinfuTracker> kinfu_;
00116     pcl::gpu::kinfuLS::MarchingCubes::Ptr marching_cubes_;
00117     pcl::gpu::kinfuLS::KinfuTracker::View colors_device_;
00118     pcl::gpu::kinfuLS::RayCaster::Ptr raycaster_;
00119     std::vector<cv::Mat> textures_;
00120     pcl::texture_mapping::CameraVector cameras_;
00121 
00122     int device_;
00123     bool auto_reset_;
00124     bool integrate_color_;
00125     bool slam_;
00126     std::string fixed_frame_id_;
00127     int n_textures_;
00128 
00129     int frame_idx_;
00130     std::string save_dir_;
00131 
00132     boost::mutex mutex_;
00133 
00134     boost::shared_ptr<tf::TransformListener> tf_listener_;
00135     Eigen::Affine3f odom_init_to_kinfu_origin_;
00136 
00137     message_filters::Subscriber<sensor_msgs::CameraInfo> sub_camera_info_;
00138     message_filters::Subscriber<sensor_msgs::Image> sub_depth_;
00139     message_filters::Subscriber<sensor_msgs::Image> sub_color_;
00140     boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > sync_;
00141     boost::shared_ptr<message_filters::Synchronizer<SyncPolicyWithColor> > sync_with_color_;
00142 
00143     ros::Publisher pub_camera_pose_;
00144     ros::Publisher pub_cloud_;
00145     ros::Publisher pub_depth_;
00146     ros::Publisher pub_rendered_image_;
00147     ros::Publisher pub_status_;
00148 
00149     tf::TransformBroadcaster tf_broadcaster_;
00150 
00151     ros::ServiceServer srv_reset_;
00152     ros::ServiceServer srv_save_mesh_;
00153     ros::ServiceServer srv_save_mesh_with_context_;
00154 
00155   private:
00156   };
00157 
00158 }  // namespace jsk_pcl_ros
00159 
00160 #endif  // JSK_PCL_ROS_KINFU_H_


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:49