RGBDSLAM in Gazebo environment [closed]
Dear Felix Endres, dear ROS users
I would like to know if it's possible to use RGBDSLAM in outdoor simulated environment. I use a simulated kinect with the plugin provided in gazebo ros (gazebo_ros_openni_kinect). Then I launch rgbdslam.launch. I change some params to remap topics used by camera. This is my launch file:
<launch> <node pkg="rgbdslam" type="rgbdslam" name="rgbdslam" cwd="node" required="true" output="screen">
<param name="fixed_frame_name" value="/Robot0/map"/>
<param name="base_frame_name" value="/Robot0/base_link"/>
<!-- These are the default values of some important parameters -->
<param name="config/feature_extractor_type" value="ORB"/><!-- also available: SIFT, SIFTGPU, SURF, SURF128 (extended SURF), ORB. -->
<param name="config/feature_detector_type" value="ORB"/><!-- also available: SIFT, SURF, GFTT (good features to track), ORB. -->
<param name="config/detector_grid_resolution" value="3"/><!-- detect on a 3x3 grid (to spread ORB keypoints and parallelize SIFT and SURF) -->
<param name="config/max_keypoints" value="600"/><!-- Extract no more than this many keypoints -->
<param name="config/max_matches" value="300"/><!-- Keep the best n matches (important for ORB to set lower than max_keypoints) -->
<param name="config/min_sampled_candidates" value="4"/><!-- Frame-to-frame comparisons to random frames (big loop closures) -->
<param name="config/predecessor_candidates" value="4"/><!-- Frame-to-frame comparisons to sequential frames-->
<param name="config/neighbor_candidates" value="4"/><!-- Frame-to-frame comparisons to graph neighbor frames-->
<param name="config/ransac_iterations" value="100"/>
<param name="config/cloud_creation_skip_step" value="2"/><!-- subsample the images' pixels (in both, width and height), when creating the cloud (and therefore reduce memory consumption) -->
<param name="config/cloud_display_type" value="POINTS"/><!-- Show pointclouds as points (as opposed to TRIANGLE_STRIP) -->
<param name="config/pose_relative_to" value="largest_loop"/><!-- optimize only a subset of the graph: "largest_loop" = Everything from the earliest matched frame to the current one. Use "first" to optimize the full graph, "inaffected" to optimize only the frames that were matched (not those inbetween for loops) -->
<param name="config/backend_solver" value="pcg"/><!-- pcg is faster and good for continuous online optimization, cholmod and csparse are better for offline optimization (without good initial guess)-->
<param name="config/optimizer_skip_step" value="1"/><!-- optimize only every n-th frame -->
</node> </launch>
First error I note is because gazebo_ros plugin, if pointcloaud is not dense, automatically change width and height from structured to unstructured data (for example image_raw has width = 640 and height = 480, points come 307200 x 1). I've modified openni_listener.cpp to remove this error. So in the kinectCallbak
oid OpenNIListener::kinectCallback (const sensor_msgs::ImageConstPtr& visual_img_msg,
const sensor_msgs::ImageConstPtr& depth_img_msg,
const sensor_msgs::PointCloud2ConstPtr& sensor_point_cloud)
{
//Pointcloud reorganization into structured matrix sensor_msgs::PointCloud2::Ptr point_cloud = boost::make_shared<sensor_msgs::pointcloud2>(); if(sensor_point_cloud->height == 1 && sensor_point_cloud->width == visual_img_msg->width * visual_img_msg->height) { ROS_WARN_STREAM("Reorganization of the poincloud as structured"); pcl::PointCloud<pcl::pointxyzrgb> pcl_cloud; pcl::fromROSMsg(sensor_point_cloud, pcl_cloud); pcl_cloud.width = visual_img_msg->width; pcl_cloud.height = visual_img_msg->height; pcl::toROSMsg(pcl_cloud, *point_cloud); } else point_cloud = boost::make_shared<sensor_msgs::pointcloud2>(sensor_point_cloud); ... } Thi is the only modification to origin code I make. Now I can see images and feature estimated but I get some errors and /rgbdslam/online_clouds and /rgbdslam/batch_clouds are both empty.
Can you help me? Thanks a lot
Anna
