Gazebo | Ignition | Community
Ask Your Question
2

Gazebo 1.5: libgazebo_openni_kinect.so: pointcloud data is rotated and flipped?

asked 2013-09-05 08:49:19 -0500

psei gravatar image

updated 2013-09-05 09:36:45 -0500

Hi there!

I set up a gazebo simulation with the Clearpath Husky A200 robot, on which I mounted a kinect camera at the front bumper and a hokuyo laserscan on top of the rail.

Here is the situation: gazebo situation

For the hokuyo laser scan I use the "libgazebo_ros_laser.so" plugin. It works fine and provides the scan data on the "/scan" topic.

For the kinnect camera I am using the "libgazebo_openni_kinect.so", which provides "/kinect/image_raw", "/kinect/image_depth" and "/kinect/pointcloud" topics.

The camera can see this (screenshots taken from RVIZ):

"/kinect/image_raw": image raw

"/kinect/image_depth": image depth

So far, everything is looking good. So here comes the thing, when I tell RVIZ to show the "pointcloud" topic, which comes from the same camera, the siuation looks like this.

RIVZ pointcloud:

rviz

The red points indicate the edges of the trash dumpster, which is recognized correctly by the hokuyo laser scan.

But why the hell is the point cloud data coming from the kinect flipped and projected into the sky? I would have expected it to be overlaying with the red laserscan data... I tried the whole day rotating the data but could not find a solution. Does anyone know what to do?

Best regards psei

edit retag flag offensive close merge delete

7 Answers

Sort by » oldest newest most voted
0

answered 2013-09-09 09:18:53 -0500

psei gravatar image

updated 2013-10-15 05:41:34 -0500

I solved my problem. Anyone who is experiencing the same problem can try this:

Update your "libgazebo_ros_depth_camera.cpp" or "libgazebo_ros_openni_kinnect.cpp" as following:

#include "pcl/common/eigen.h"
#include "pcl/common/transforms.h"

then, look for the line "pcl::toROSMsg(point_cloud, point_cloud_msg);" in function

bool GazeboRosDepthCamera::FillPointCloudHelper( sensormsgs::PointCloud2 &pointcloudmsg, uint32t rowsarg, uint32t colsarg, uint32t steparg, void* dataarg)

right before that, transform the cloud as following:

pcl::PointCloud<pcl::pointxyzrgb> transformed_cloud;
double rotx = 0.0;
double roty = M_PI/2.0;
double rotz = -M_PI/2.0;
Eigen::Matrix4f rotMatrixX;
Eigen::Matrix4f rotMatrixY;
Eigen::Matrix4f rotMatrixZ;

rotMatrixX <<
1.0, 0.0, 0.0, 0.0,

0.0, cos(rotx), -sin(rotx), 0.0,
0.0, sin(rotx), cos(rotx), 0.0,
0.0, 0.0, 0.0, 1.0;

rotMatrixY <<
cos(roty), 0.0, sin(roty), 0.0,

0.0, 1.0, 0.0, 0.0,
-sin(roty), 0.0, cos(roty), 0.0,
0.0, 0.0, 0.0, 1.0;

rotMatrixZ <<
cos(rotz), -sin(rotz), 0.0, 0.0,
sin(rotz), cos(rotz), 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0;

pcl::transformPointCloud(point_cloud,transformed_cloud, rotMatrixX * rotMatrixY * rotMatrixZ);
pcl::toROSMsg(transformed_cloud, point_cloud_msg);

edit flag offensive delete link more

Comments

There is an other solution, using TF from ROS. you can publish a rotation transform to kinect_frame.

barcelosandre gravatar imagebarcelosandre ( 2013-09-09 17:38:55 -0500 )edit

I am experiencing the same issue. However, since I installed Gazebo using the Ubuntu Groovy repository, I do not have the source of the plugins. I checked it out and tried to compile, but then I enter dependency hell. Could you care to post a link to your recompiled libgazebo_ros_openni_kinect.so so that I can use your binary instead of rolling my own? Thanks!

MadEgg gravatar imageMadEgg ( 2013-09-12 09:27:04 -0500 )edit

hi i need correct libgazebo_ros_openni_kinect.so file but the these files not available at above links. can u give me these files cz i don't find its cpp file in gazebo plugins. Thnaks

M.Ahmed gravatar imageM.Ahmed ( 2016-01-22 04:31:09 -0500 )edit

Hi, I also need your compiled files. Could you please repost them, they are not available anymore. Thank you! Also, could you please tell us the location folder of the ones we need to replace?

Filippo Sanfilippo gravatar imageFilippo Sanfilippo ( 2016-05-07 06:35:15 -0500 )edit

Any updates on this? Could you please repost your .so files?

Filippo Sanfilippo gravatar imageFilippo Sanfilippo ( 2016-05-13 11:49:29 -0500 )edit

I cannot imagine that this is the correct way to deal with this. A similar question is posted here: https://answers.ros.org/question/53784/simulated-kinect-rotation-around-x-bug/?answer=55968#post-id-55968

felixvd gravatar imagefelixvd ( 2018-07-11 04:16:42 -0500 )edit

Hey all, I cannot find the line pcl::toROSMsg(point_cloud, point_cloud_msg); in both plugins and I have the same issue. Can someone please upload the updated version of these plugins, so that the problem is solved because the two links are not working. Thank you

user90 gravatar imageuser90 ( 2018-07-20 01:40:05 -0500 )edit

There is an amazing solution by barcelosandre and you only need to update the urdf file you have. Instead of setting <framename>kinect_link</framename> to be connected directly to the sensor, set an additional kinect_optical_link whose joint is rotated in RPY terms by (-1.57, 0, -1.57).The code can be found here https://paste.ubuntu.com/p/bFJPHVDNVQ/

vib2810 gravatar imagevib2810 ( 2021-01-11 02:59:54 -0500 )edit
0

answered 2013-09-09 17:43:06 -0500

barcelosandre gravatar image

There is an other solution, using TF from ROS. you can publish a rotation transform to kinect_frame. An example to launch file is:

<node pkg="tf" type="static_transform_publisher" name="ime_slam_camera_tf" args="0 0 0 -1.58 0 -1.58 /pioneer3dx_gazebo/base_frame /camera/camera_frame 100"/>

edit flag offensive delete link more

Comments

Hi barcelosandre, thanks for your answer. I will try that and give you feedback.

psei gravatar imagepsei ( 2013-09-10 02:28:06 -0500 )edit

Hmm... in my case the transformation would be from /base_link to /kinect_link, but since I am using URDF to build my robot in Gazebo, this TF is already been published....so I cant try your solution. I am also asking myself if that static TF you suppose would affect the /image_raw or /image_depth topic, that also comes from the kinect frame.

psei gravatar imagepsei ( 2013-09-10 02:42:07 -0500 )edit

Instead of going into the controllers or using a static tf publisher, you could try doing the same rotations by adding an additional joint and rotating them by the same amount. For example, instead of setting <framename>kinect_link</framename> to be connected directly to the sensor, set an additional kinect_optical_link whose joint is rotated in RPY terms by (-1.57, 0, -1.57). Then put this as your frameName for the sensor. Soo.. now <framename>kinect_optical_link</framename>

M.Otubela gravatar imageM.Otubela ( 2017-03-13 11:39:58 -0500 )edit

@M.Otubela Your solution is apt. i used it and it worked like magic. using a different TF for just the sensor nailed it. No hassles of editing source code etc. 10 stars.

afroRoboticist gravatar imageafroRoboticist ( 2019-03-30 21:19:09 -0500 )edit

Yes this is an amazing solution The code can be found here https://paste.ubuntu.com/p/bFJPHVDNVQ/

vib2810 gravatar imagevib2810 ( 2021-01-11 02:00:54 -0500 )edit
0

answered 2020-04-13 14:21:55 -0500

Fookii gravatar image

The reason for this issue is that the gazebo plugin is not publishing /tf transforms like real openni driver do, "link" refers to physical setup, where frame is for the images. In openni_wrapper, 2 frame transforms will be published with < camera_link > as parent, but not in gazebo.

Another quick solution would be create a empty "link" in urdf and set a joint to them with rotation, see this https://answers.ros.org/question/2183...

edit flag offensive delete link more
0

answered 2016-10-29 22:31:05 -0500

hi,I got the same problem,how to fix it ?

<gazebo reference="kinect_link">
    <sensor type="depth" name="openni_camera_camera">       
        <always_on>1</always_on>
        <visualize>true</visualize>             
        <camera>
            <horizontal_fov>1.047</horizontal_fov>  
            <image>
                <width>640</width>
                <height>480</height>
                <format>R8G8B8</format>
            </image>
            <depth_camera>

            </depth_camera>
            <clip>
                <near>0.1</near>
                <far>100</far>
            </clip>
        </camera>
        <plugin name="kinect_controller" filename="libgazebo_ros_openni_kinect.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>10.0</updateRate>
        <cameraName>camera</cameraName>
        <frameName>kinect_link</frameName>                   
        <imageTopicName>rgb/image_raw</imageTopicName>
        <depthImageTopicName>depth/image_raw</depthImageTopicName>
        <pointCloudTopicName>depth/points</pointCloudTopicName>
        <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>              
        <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>            
        <pointCloudCutoff>0.4</pointCloudCutoff>                
        <hackBaseline>0.07</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
        <CxPrime>0.0</CxPrime>
        <Cx>0.0</Cx>
        <Cy>0.0</Cy>
        <focalLength>0.0</focalLength>
        </plugin>
    </sensor>
</gazebo>![image description](/upfiles/14777982551105122.jpg)(/upfiles/14777982463774798.jpg)(/upfiles/14777982365170525.jpg)
edit flag offensive delete link more
0

answered 2016-05-07 06:55:42 -0500

Filippo Sanfilippo gravatar image

I have reported this bug here: link text

edit flag offensive delete link more
0

answered 2015-01-16 14:59:48 -0500

carlos gravatar image

Hi, I know this is old, however, I'd like to mention that the point cloud data is fine. With Asus/Primesense/Kinect1 the point cloud is published in the "camerargbopticalframe" which is rotated with the transformation you mention with respect to the "cameralink".

In my case, I set <framename>${name}/camerargboptical_frame</framename> and then I have a launch file with the default axis transformations (you can get them using a real RGBD sensor and tf echoing the frames) like this.

<launch>

<!-- LAUNCH INTERFACE -->
<arg name="camera_name" default="head"/>

<!-- LAUNCH IMPLEMENTATION -->
<node pkg="tf" type="static_transform_publisher" name="hand_camera_calib" args="0.000 -0.020 0.000 0.000 0.000 0.000 1.000 $(arg camera_name)_link $(arg camera_name)/camera_link  100"/>

<node pkg="tf" type="static_transform_publisher" name="camera_depth_frame" args="0.000 -0.020 0.000 0.000 0.000 0.000 1.000  $(arg camera_name)/camera_link $(arg camera_name)/camera_depth_frame 100"/>

<node pkg="tf" type="static_transform_publisher" name="camera_depth_optical_frame" args="0.000 -0.020 0.000 -0.500 0.500 -0.500 0.500 $(arg camera_name)/camera_link $(arg camera_name)/camera_depth_optical_frame 100"/>

<node pkg="tf" type="static_transform_publisher" name="camera_rgb_frame" args="0.000 -0.045 0.000 0.000 0.000 0.000 1.000 $(arg camera_name)/camera_link $(arg camera_name)/camera_rgb_frame 100"/>

<node pkg="tf" type="static_transform_publisher" name="camera_rgb_optical_frame" args="0.000 -0.045 0.000 -0.500 0.500 -0.500 0.500  $(arg camera_name)/camera_link $(arg camera_name)/camera_rgb_optical_frame 100"/>

</launch>

And it works fine for me.

edit flag offensive delete link more
0

answered 2021-01-11 03:04:40 -0500

vib2810 gravatar image

updated 2021-01-11 09:37:16 -0500

Solution by barcelosandreworks like a charm. You have to change the transform between the camera and the robot. You can do so by writing a TF publisher node, but in most cases the ROBOT_STATE_PUBLISHER node will publish your tf for you according to your URDF. So you can modify the URDF, create a dummy link(kinect_optical_link) wrt the camera link rotated by RPY(-pi/2, 0, -pi/2) wrt the camera link and set this as your camera plugins frameName. Note: Don't change the gazebo "reference" tag. The code can be found here for better explanation: https://paste.ubuntu.com/p/bFJPHVDNVQ/

edit flag offensive delete link more

Question Tools

3 followers

Stats

Asked: 2013-09-05 08:49:19 -0500

Seen: 11,006 times

Last updated: Jan 11 '21