Reg : Using Realsense Pointcloud as input
dummyjohn77 opened this issue · 5 comments
Hi,
I was hoping to use a realsense pointcloud as input for LoCUS. Can you please tell me if this would be possible.
I have converted the realsense's depth cloud to a pointcloud using http://wiki.ros.org/depth_image_proc. I have also made the necessary TF changes. However, I keep running into the error
[pcl::MultithreadedGeneralizedIterativeClosestPoint::" "computeCovariances] Number or points in cloud (%lu) is less " "than k_correspondences_ (%lu)!\n"
which is in line 70 of gicp.hpp.
Can you please tell me if there should be parameter change to incorporate the use of realsense?
Hmm, this very likely means that the point cloud that you input to locus doesn't have size for some reason. Can you please check that the point cloud size that you are inputting to the locus and whether this point cloud has more points that the correspondence that you are looking for? What's your k_corresponences_ in odometry and localization module? Maybe those numbers are too large?
Thank you for the prompt response.
I am able to visualize the input pointcloud of locus in rviz. I hope this would suffice to confirm that the pointcloud is valid and has points. The LOCUS error outputs that it has 4,8 or 16 points while the k_correspondences_ is fixed at 20, in both odometry and localization.
Real_Sense_Issue.mp4
Added to this, LOCUS runs for a few iterations and running odom_to_map_tf gives us an set of odometry outputs. The error pops up after a few iterations of LOCUS (PFA: command prompt Video ).
Also can you please tell me how to edit the k_correspondences_. I tried searching the config files, but couldnt find the parameter
I was trying to upload the launch file when I accidentally closed the issue. Sorry for any confusion caused
<arg name="robot_namespace" default="uav1"/>
<arg name="rgbd_enabled" default="false"/>
<arg name="use_gdb" default="true"/>
<arg name="nodelet_manager" value="nodelet_manager"/>
<arg name="launch_prefix" value="gdb -ex run --args" if="$(arg use_gdb)"/>
<arg name="launch_prefix" value="" unless="$(arg use_gdb)"/>
<arg name="nodelet_args" value="--no-bond"/>
<arg name="respawn" value="false" if="$(arg use_gdb)"/>
<arg name="respawn" value="true" unless="$(arg use_gdb)"/>
<arg name="robot_type" default="f550"/>
<arg name="pc_topic" default="velodyne_points" unless="$(arg rgbd_enabled)"/>
<arg name="pc_topic" default="rgbd_velodyne_points" if="$(arg rgbd_enabled)"/>
<arg name="number_of_velodynes" default="1" />
<arg name="b_use_multiple_pc" value="$(eval arg('number_of_velodynes') > 1)"/>
<arg name="pc_input" value="locus_input"/>
<!-- 0:TOP, 1:FRONT, 2:REAR -->
<arg name="pc_trans_in_0" default="$(arg pc_topic)/transformed"/>
<arg name="pc_trans_in_1" default="velodyne_front/$(arg pc_topic)/transformed"/>
<arg name="pc_trans_in_2" default="velodyne_rear/$(arg pc_topic)/transformed"/>
<group ns="$(arg robot_namespace)">
<node pkg="tf" type="static_transform_publisher" name="base_link_broadcaster" args="0 0 0 0 0 0 1 $(arg robot_namespace)/fcu $(arg robot_namespace)/base_link 100" if="$(eval robot_namespace.startswith('uav'))" />
<node pkg="tf" type="static_transform_publisher" name="velodyne_link_broadcaster" args="0 0 0 0 0 0 1 $(arg robot_namespace)/rgbd_down/link $(arg robot_namespace)/velodyne 100" if="$(arg rgbd_enabled)" />
<!-- <node pkg="tf" type="static_transform_publisher" name="vimu_broadcaster" args="0 0 0 0 0 0 1 $(arg robot_namespace)/fcu $(arg robot_namespace)/vn100 100" if="$(eval robot_namespace.startswith('uav'))" /> -->
<node pkg="tf" type="static_transform_publisher" name="arch_broadcaster" args="0 0 0 0 0 0 1 $(arg robot_namespace)/fcu $(arg robot_namespace)/arch 100" if="$(eval robot_namespace.startswith('uav'))" />
<node pkg="tf" type="static_transform_publisher" name="vimu_broadcaster" args="0 0 0 0 0 0 1 $(arg robot_namespace)/fcu $(arg robot_namespace)/vn100 100" if="$(eval robot_namespace.startswith('uav'))" />
<node pkg="tf" type="static_transform_publisher" name="imu_broadcaster" args="0 0 0 0 0 0 1 $(arg robot_namespace)/fcu $(arg robot_namespace)/imu 100" if="$(eval robot_namespace.startswith('uav'))" />
<!-- Load parameters -->
<rosparam file="$(find locus)/config/body_filter_params_$(arg robot_type).yaml"
subst_value="true"/>
<!-- Load robot description -->
<!-- <include file="$(find locus)/launch/robot_description.launch">
<arg name="robot_namespace" value="$(arg robot_namespace)"/>
</include> -->
<node
pkg="locus"
name="locus"
type="locus_node"
output="screen">
<remap from="~LIDAR_TOPIC" to="$(arg pc_input)"/>
<remap from="~ODOMETRY_TOPIC" to="mavros/local_position/odom"/>
<remap from="~IMU_TOPIC" to="mavros/imu/data"/>
<remap from="~POSE_TOPIC" to="not_currently_used"/>
<remap from="~SPACE_MONITOR_TOPIC" to="localizer_space_monitor/xy_cross_section"/>
<!-- For Sim -->
<!-- <remap from="~ODOMETRY_TOPIC" to="wheel_odom"/> -->
<remap from="/diagnostics" to="hero/diagnostics"/>
<param name="robot_name" value="$(arg robot_namespace)"/>
<param name="tf_prefix" value="$(arg robot_namespace)"/>
<param name="b_integrate_interpolated_odom" value="true" if="$(eval robot_namespace.startswith('spot'))"/>
<param name="b_integrate_interpolated_odom" value="false" unless="$(eval robot_namespace.startswith('spot'))"/>
<rosparam file="$(find locus)/config/lo_settings.yaml" />
<param name="b_pub_odom_on_timer" value="false" />
<rosparam file="$(find locus)/config/lo_frames_uav.yaml" subst_value="true"/>
<rosparam file="$(find point_cloud_filter)/config/parameters.yaml"/>
<rosparam file="$(find point_cloud_odometry)/config/parameters.yaml"/>
<rosparam file="$(find point_cloud_localization)/config/parameters.yaml" unless="$(eval robot_type == 'spot')"/>
<rosparam file="$(find point_cloud_mapper)/config/parameters.yaml"/>
<param name="localization/num_threads" value="1" unless="$(eval robot_namespace.startswith('husky'))" />
<param name="icp/num_threads" value="1" unless="$(eval robot_namespace.startswith('husky'))" />
<param name="mapper/num_threads" value="1" unless="$(eval robot_namespace.startswith('husky'))" />
</node>
<node pkg="locus" name="sensors_health_monitor" type="sensors_health_monitor.py" output="screen" if="$(eval number_of_velodynes > 1)">
<remap from="failure_detection" to="point_cloud_merger_lo/failure_detection"/>
<remap from="resurrection_detection" to="point_cloud_merger_lo/resurrection_detection"/>
</node>
<node pkg="nodelet"
type="nodelet"
name="transform_points_base_link"
args="standalone pcl/PassThrough">
<remap from="~input" to="$(arg pc_topic)"/>
<remap from="~output" to="$(arg pc_trans_in_0)"/>
<rosparam subst_value="true">
filter_field_name: z
filter_limit_min: -100
filter_limit_max: 100
output_frame: $(arg robot_namespace)/base_link
</rosparam>
</node>
<node pkg="nodelet"
type="nodelet"
name="$(arg nodelet_manager)"
launch-prefix="$(arg launch_prefix)"
args="manager"
respawn="$(arg respawn)"/>
<node pkg="nodelet"
type="nodelet"
name="body_filter"
args="load point_cloud_filter/BodyFilter $(arg nodelet_manager) $(arg nodelet_args)"
respawn="$(arg respawn)">
<remap from="~input" to="$(arg pc_topic)/transformed" unless="$(arg b_use_multiple_pc)"/>
</node>
<node pkg="nodelet" type="nodelet" name="voxel_grid" args="load point_cloud_filter/CustomVoxelGrid $(arg nodelet_manager)" output="screen" respawn="true">
<remap from="~input" to="body_filter/output" />
<rosparam subst_value="true">
filter_field_name: y
filter_limit_min: -10
filter_limit_max: 10
filter_limit_negative: False
leaf_size: 0.2
output_frame: $(arg robot_namespace)/base_link
</rosparam>
</node>
<node pkg="nodelet"
type="nodelet"
name="normal_computation"
args="load point_cloud_filter/NormalComputation $(arg nodelet_manager) $(arg nodelet_args)"
respawn="$(arg respawn)">
<remap from="~input" to="voxel_grid/output"/>
<remap from="~output" to="$(arg pc_input)" />
<param name="num_threads" value="4" if="$(eval robot_namespace.startswith('husky'))" />
<param name="num_threads" value="1" unless="$(eval robot_namespace.startswith('husky'))" />
</node>
<!-- <node pkg="tf2_ros" type="static_transform_publisher" name="base_to_velolink" args="0 0 0 0 0 0 $(arg robot_namespace)/base_link $(arg robot_namespace)/velodyne_link" />
<node pkg="tf2_ros" type="static_transform_publisher" name="base_to_velo" args="0 0 0 0 0 0 $(arg robot_namespace)/base_link $(arg robot_namespace)/velodyne" /> -->
</group>
Hi,
An update
locus_input_rate.mp4
Your advise of K_correpondences worked. I reduced it to as low as 2, to ensure that the algorithm is running (but erroneous).
I also captured an rviz depiction of the input pointcloud. As you can see from the video, the scans are coming in at irregular intervals. The average rate of the topic was 4.724.
Can you please tell me if it is possible to tune the rate of LOCUS's expected input rate. Please do let me know if you think some other workaround is possible
Hi,
I managed to get the code running as expected.
The issue was specific to the nodelet point_cloud_filter/CustomVoxelGrid. The nodelet was carrying out extreme downsampling inconsistently, which resulted in the input cloud being sparse.
I switched to pcl/VoxelGrid and the issue was resolved.
Can you please let me know if there is a reason for the use of point_cloud_filter/CustomVoxelGrid instead of pcl/VoxelGrid