ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How to transform point clouds from multiple Gazebo Kinects in the world coordinates?

asked 2022-01-05 05:29:46 -0500

Emna gravatar image

updated 2022-02-18 13:23:10 -0500

lucasw gravatar image

I'm trying to capture a scene with four static Gazebo Kinects from different positions. each camera is defined by its own group in the main launch file. The problem is that the four generated point clouds are correctly displayed in Rviz, which means that the tf_static between each camera and the world frame is correct. However, when I tried to transform the point clouds from the different topics to open3d point clouds and save them, I got them superposed i.e from the same field of view! My questions are the following? 1- what is wrong (or missed) in my configuration that caused these misaligned point clouds? 2- How can I merge and publish all the point cloud messages in one topic?

Here are some screenshots on the target scene, the displayed point clouds in Rviz and the superposed open3d point clouds saved from the topics

Here is my lunch file

<?xml version="1.0"?>
<launch>
    <arg name="recordimages" default="false" />

    <!-- configuration of the simulation -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
            <arg name="world_name" value="$(find gazebo_sensors_tutorial)/worlds/plane_cube.world"/>
            <arg name="paused" value="false" />
            <arg name="gui" value="true" />
    </include>

        <node name="spawn_camera_model_1" pkg="gazebo_ros" type="spawn_model" args="-urdf -param camera_1/robot_description -model camera_1" respawn="false" output="screen" />
        <node name="spawn_camera_model_2" pkg="gazebo_ros" type="spawn_model" args="-urdf -param camera_2/robot_description -model camera_2" respawn="false" output="screen" />
        <node name="spawn_camera_model_3" pkg="gazebo_ros" type="spawn_model" args="-urdf -param camera_3/robot_description -model camera_3" respawn="false" output="screen" />
        <node name="spawn_camera_model_4" pkg="gazebo_ros" type="spawn_model" args="-urdf -param camera_4/robot_description -model camera_4" respawn="false" output="screen" />

        <!-- rviz configuration -->
    <node name="chess_rviz" pkg="rviz" type="rviz" respawn="false" args="-d $(find gazebo_sensors_tutorial)/config/four_kinects.rviz" output="screen"/>

        <group ns="camera_1">
            <param name="publish_frequency" value="100"/>
            <param name="robot_description" command="$(find xacro)/xacro $(find gazebo_sensors_tutorial)/robot/camera_1.urdf.xacro" />
            <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
        </group>


    <group ns="camera_2">
            <param name="publish_frequency" value="100"/>
            <param name="robot_description" command="$(find xacro)/xacro $(find gazebo_sensors_tutorial)/robot/camera_2.urdf.xacro" />
            <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
        </group>

        <group ns="camera_3">
            <param name="publish_frequency" value="100"/>
            <param name="robot_description" command="$(find xacro)/xacro $(find gazebo_sensors_tutorial)/robot/camera_3.urdf.xacro" />
            <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
        </group>

        <group ns="camera_4">
            <param name="publish_frequency" value="100"/>
            <param name="robot_description" command="$(find xacro)/xacro $(find gazebo_sensors_tutorial)/robot/camera_4.urdf.xacro" />
            <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
        </group>

</launch>

and here is an example of a camera_X.urdf.xacro file

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="camera_1">
  <xacro:include filename="$(find gazebo_sensors_tutorial)/robot/sensors/kinect_camera.urdf.xacro"/>
  <link name="world" />
  <!-- sets camera frame w.r.t. to world frame-->
  <xacro:kinect_camera name="camera_1" parent="world" > ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2022-01-05 13:13:03 -0500

Airuno2L gravatar image

It doesn't look like you ever transform the point clouds in your python script. You can use something like this:

from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
cloud_out = do_transform_cloud(cloud_in, transform)

You can build the transform using the pose of your kinect origins and inputting them into tf.transformations.compose_matrix()

What's happening is that the point cloud data is on disc with respect to a (0,0,0) origin. The reason it looks right in RVIZ is because it has knowledge of the TF tree.

edit flag offensive delete link more

Comments

Actually, you shouldn't need to compose the matrix, it already exists so you should be able to grab it off the TF tree somehow.

Airuno2L gravatar image Airuno2L  ( 2022-01-05 13:21:34 -0500 )edit

Was this ever answered successfully? I have the exact same issue and it is driving me mad

garethbyers gravatar image garethbyers  ( 2022-05-26 10:33:33 -0500 )edit

I think my answer is correct, you just need to transform the point cloud using the TF of the Kinect.

Airuno2L gravatar image Airuno2L  ( 2022-05-26 11:16:11 -0500 )edit

What goes in the transform part of the method? I cannot find any reliable sources and could use some help

garethbyers gravatar image garethbyers  ( 2022-05-31 11:25:00 -0500 )edit

The tf frame that has the location and orientation of the kinect sensor.

Airuno2L gravatar image Airuno2L  ( 2022-06-05 08:27:02 -0500 )edit

Question Tools

Stats

Asked: 2022-01-05 05:29:46 -0500

Seen: 385 times

Last updated: Jan 05 '22