How to change the rotation in the result of depth_image_proc/point_cloud_xyz?

asked 2020-06-17 20:46:17 -0500

Konye gravatar image

updated 2020-06-18 21:21:46 -0500

Hello guys, I am trying to use the depth_image_proc/point_cloud_xyz nodelet to convert my depth images into point cloud data.

Everything goes well except the rotation, which rotates my data in the top of my robot when it was displayed in rviz. Please see my screenshot followed:

image description

The data displays correct unless I set the urdf files with a extra rotation as:

rpy = "-1.57 0 4.71"

It gives a result like this:

image description

And I find that there is also a offset and a scaling ( about x7.0 ) in the data which is different to my laser scan and makes the true ground (horizon) under the map ground:

image description

And here is my camera info:

height: 270
width: 480
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [237.78233998783918, 0.0, 240.0, 0.0, 237.78233998783918, 135.0, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [237.78233998783918, 0.0, 240.0, 0.0, 0.0, 237.78233998783918, 135.0, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
x_offset: 0
y_offset: 0
height: 0
width: 0
do_rectify: False

How can I solve this problem? Thank you!

edit retag flag offensive close merge delete

Comments

1

Generally to manipulate pointclouds you have to use pcl library. to convert from rosmessage to pcl format there is some straightforward implementations: http://docs.ros.org/indigo/api/pcl_co...

Once you have the pointcloud as pcl data there is functions which should allow you to rotate easily. from (http://docs.ros.org/indigo/api/pcl_ro...) An example:

    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform.translation() << 0.0, 0.0, 0.0;
    transform.rotate(Eigen::AngleAxisf((rotx*M_PI) / 180, Eigen::Vector3f::UnitX()));
    transform.rotate(Eigen::AngleAxisf((roty*M_PI) / 180, Eigen::Vector3f::UnitY()));
    transform.rotate(Eigen::AngleAxisf((rotz*M_PI) / 180, Eigen::Vector3f::UnitZ()));
    pcl::transformPointCloud(*cloud_in, *cloud_out, transform);

Edit: i think is not exactly what you want. anyway i leave this here just in case it's useful for someone

Solrac3589 gravatar image Solrac3589  ( 2020-06-18 02:33:45 -0500 )edit

It's actually possible the OP is surprised by the fact that PointClouds and other image based sensors have their sensor data aligned with Z+. See REP-103: Standard Units of Measure and Coordinate Conventions - Suffix Frames.

As RViz follows REP-103 (with X+ forward, Y+ left, Z+ up), PointClouds end up "on the ceiling" or "above the robot".

The proper solution would then be to configure the TF tree correctly and make sure the camera / sensor frames are correctly oriented.

I would not use PCL to rotate anything, it introduces quite some overhead and should not be necessary if I understand the description by the OP correctly.

gvdhoorn gravatar image gvdhoorn  ( 2020-06-18 02:36:11 -0500 )edit

If I am not wrong, the implementation of the library you are using is based on opencv. from there (https://docs.opencv.org/2.4/modules/c...) is possible to see that R is the rotation matrix. Maybe just changing the values for the desired ones using an easy converter: https://www.andre-gaschler.com/rotati...

Solrac3589 gravatar image Solrac3589  ( 2020-06-18 02:40:34 -0500 )edit

you are right about you response @gvdhoorn. The best solution is what you say

Solrac3589 gravatar image Solrac3589  ( 2020-06-18 02:42:26 -0500 )edit

@Konye: please attach your screenshot to the question directly instead of linking to an offline site.

I've given you sufficient karma.

gvdhoorn gravatar image gvdhoorn  ( 2020-06-18 03:10:23 -0500 )edit

According to the message definition for CameraInfo, a R matrix is for steroeo cameras only and 

For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
#  also have R = the identity and P[1:3,1:3] = K.

(mentioned in P matrix). What I am using now is a mono camera in a simulation world and all the CameraInfo is setted manually based on the message definition and the opencv website you mentioned, so I set R as identity matrix. Maybe I should double check again, thank you.

Konye gravatar image Konye  ( 2020-06-18 03:12:36 -0500 )edit

Please look into your TF tree first.

Do not start rotating anything by hand or changing CameraInfo settings.

If you have a regular ROS setup with a proper TF tree, this should not be needed.

gvdhoorn gravatar image gvdhoorn  ( 2020-06-18 03:16:21 -0500 )edit

@gvdhoorn：I have updated my screenshot now, sorry for the inconvenience.

Konye gravatar image Konye  ( 2020-06-18 03:18:02 -0500 )edit
see more comments