Home | Tutorials | Wiki | Issues
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

5 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
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 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 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
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
Login/Signup to Answer

Question Tools

3 followers

Stats

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

Seen: 4,401 times

Last updated: Jan 16 '15