Go to the documentation of this file.
35 #ifndef JSK_PCL_ROS_KINFU_H_
36 #define JSK_PCL_ROS_KINFU_H_
38 #include <pcl/console/parse.h>
39 #include <pcl/gpu/kinfu_large_scale/kinfu.h>
40 #include <pcl/gpu/kinfu_large_scale/marching_cubes.h>
41 #include <pcl/gpu/kinfu_large_scale/raycaster.h>
42 #include <pcl/gpu/containers/initialization.h>
43 #include <pcl/surface/texture_mapping.h>
44 #include <pcl/point_cloud.h>
45 #include <pcl/point_types.h>
46 #include <pcl/common/angles.h>
48 #include <dynamic_reconfigure/server.h>
52 #include <jsk_topic_tools/connection_based_nodelet.h>
55 #include <std_srvs/Empty.h>
56 #include <sensor_msgs/Image.h>
57 #include <sensor_msgs/PointCloud2.h>
58 #include <jsk_rviz_plugins/OverlayText.h>
62 #include "jsk_pcl_ros/KinfuConfig.h"
71 void paint3DView (
const KinfuTracker::View& rgb24, KinfuTracker::View& view,
float colors_weight = 0.5
f);
78 class Kinfu:
public jsk_topic_tools::ConnectionBasedNodelet
85 typedef jsk_pcl_ros::KinfuConfig
Config;
94 void initKinfu(
const sensor_msgs::CameraInfo::ConstPtr& caminfo_msg);
95 void update(
const sensor_msgs::CameraInfo::ConstPtr& caminfo_msg,
96 const sensor_msgs::Image::ConstPtr& depth_msg);
97 void update(
const sensor_msgs::CameraInfo::ConstPtr& caminfo_msg,
98 const sensor_msgs::Image::ConstPtr& depth_msg,
99 const sensor_msgs::Image::ConstPtr& rgb_msg);
106 pcl::PolygonMesh
createPolygonMesh(
const jsk_recognition_msgs::BoundingBox& box_msg,
107 const std::string& ground_frame_id);
109 const std::vector<cv::Mat> textures,
110 pcl::texture_mapping::CameraVector cameras);
113 jsk_recognition_msgs::SaveMesh::Request&
req, jsk_recognition_msgs::SaveMesh::Response&
res);
161 #endif // JSK_PCL_ROS_KINFU_H_
pcl::gpu::kinfuLS::MarchingCubes::Ptr marching_cubes_
boost::shared_ptr< pcl::gpu::kinfuLS::KinfuTracker > kinfu_
virtual void configCallback(Config &config, uint32_t level)
pcl::TextureMesh convertToTextureMesh(const pcl::PolygonMesh &triangles, const std::vector< cv::Mat > textures, pcl::texture_mapping::CameraVector cameras)
pcl::texture_mapping::CameraVector cameras_
boost::shared_ptr< tf::TransformListener > tf_listener_
message_filters::sync_policies::ExactTime< sensor_msgs::CameraInfo, sensor_msgs::Image > SyncPolicy
ros::Publisher pub_rendered_image_
void initKinfu(const sensor_msgs::CameraInfo::ConstPtr &caminfo_msg)
pcl::gpu::kinfuLS::KinfuTracker::View colors_device_
std::vector< cv::Mat > textures_
ros::Publisher pub_depth_
message_filters::Subscriber< sensor_msgs::Image > sub_depth_
pcl::gpu::kinfuLS::RayCaster::Ptr raycaster_
bool saveMeshCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
bool saveMeshWithContextCallback(jsk_recognition_msgs::SaveMesh::Request &req, jsk_recognition_msgs::SaveMesh::Response &res)
ros::Publisher pub_cloud_
std::string fixed_frame_id_
void paint3DView(const KinfuTracker::View &rgb24, KinfuTracker::View &view, float colors_weight=0.5f)
ros::ServiceServer srv_save_mesh_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
bool resetCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
pcl::PolygonMesh createPolygonMesh()
ros::Publisher pub_status_
tf::TransformBroadcaster tf_broadcaster_
void update(const sensor_msgs::CameraInfo::ConstPtr &caminfo_msg, const sensor_msgs::Image::ConstPtr &depth_msg)
jsk_pcl_ros::KinfuConfig Config
ros::ServiceServer srv_save_mesh_with_context_
boost::mutex mutex
global mutex.
message_filters::Subscriber< sensor_msgs::Image > sub_color_
ros::Publisher pub_camera_pose_
message_filters::sync_policies::ApproximateTime< sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::Image > SyncPolicyWithColor
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_camera_info_
Eigen::Affine3f odom_init_to_kinfu_origin_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
ros::ServiceServer srv_reset_
virtual void unsubscribe()
boost::shared_ptr< message_filters::Synchronizer< SyncPolicyWithColor > > sync_with_color_
jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jan 7 2025 04:05:45