Ask Your Question
0

What is the message of geometry_msgs/PoseWithCovariance

asked 2019-10-03 09:31:17 -0500

kane_choigo gravatar image

updated 2019-10-03 09:32:44 -0500

Hi, I'm using ROS kinetic in ubuntu 16.04 and still think I'm new to ROS.

Anyway, I'm making some packages above on people tracking so I need to estimate people's state and their covariance in 2D circumstance.

And I found the ROS message of geometry_msgs/PoseWithCovariance and geometry_msgs/TwistWithCovariance.

These covariance terms have 36-length of a list, so I think they correspond to [x, y, z, roll, pitch, yaw] terms row and column each other.

The problem is I have no idea how to access them. When I'm gonna get the value of (row 3, col 4), where is it? Is it CUSTOMIZED_MSG_NAME.covarianve[15] ?

In other words, when I inspect from CUSTOMIZED_MSG_NAME.covarianve[0] to [5], is these values correspond to the all first row of the covariance matrix(list in ROS message) ?? or column ??

Thanks in advance :)

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2019-10-03 11:36:44 -0500

updated 2019-10-04 04:55:27 -0500

gvdhoorn gravatar image

Hello !

The documentation for this message is here:

# This represents a pose in free space with uncertainty.
Pose pose

# Row-major representation of the 6x6 covariance matrix
# The orientation parameters use a fixed-axis representation.
# In order, the parameters are:
# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
float64[36] covariance

So covariance is Row-Major order so you can access it with 2 methods :

CUSTOMIZED_MSG_NAME.covariance[0..35]

or

CUSTOMIZED_MSG_NAME.covariance[0*6+0..5*6+5]  (for 6*6 matrix only)

CUSTOMIZED_MSG_NAME.covariance[0..5][0..5]

edit flag offensive delete link more

Comments

Hello!

Thanks for your quick response. So, is that means there would NOT be any other problems when I access it like ' CUSTOMIZED_MSG_NAME.covariance[2][3] = VARIABLE_1'? Is it NOT necessary to write it like 'CUSTOMIZED_MSG_NAME.covariance[15] '?

kane_choigo gravatar imagekane_choigo ( 2019-10-03 19:08:31 -0500 )edit

And i'm sorry but no, you cannot actually use the [][] on cpp std::vector/boost::array or python tuple (the deserialized type http://wiki.ros.org/msg)

Actually only the first version is ok. what you can do is :

CUSTOMIZED_MSG_NAME.covariance[X*N+Y], where N is the size of your square matrix (6 here), X the row and Y the column.(0-indexed) See the other answers

lmathieu gravatar imagelmathieu ( 2019-10-04 02:31:54 -0500 )edit
1

where N is the size of your square matrix (6 here)

Just to be accurate : N is the columns number, the size of the matrix is 36

Delb gravatar imageDelb ( 2019-10-04 02:54:43 -0500 )edit
1

answered 2019-10-03 11:28:49 -0500

lucascoelho gravatar image

Considering that the first row and column are 0, row 3, column 4 (roll pitch covariance) will be index 29. If you are considering that first row and column are 1, then row 3, column 4 (z roll covariance) will be index 21.

ROS uses a row/column indexation of columns to vectors. So 0 to 5 corresponds to the first row.

edit flag offensive delete link more

Comments

Not to be zealous, I just want to correct you because confusions with this are easy to make but hard to debug sometimes :

Considering that the first row and column are 0

It's not a consideration. The first index of an array is 0 so the first row and column are 0 too (which is matching with the formulas below).

I don't know how you got the value 29 and 21, you probably have also considered that the first index is 1, but you can have x (row number) and y (column number) from those formulas :

y = index % columns_number = 29 % 6 = 5
x = (index -  index % columns_number)/columns_number = (29 - y)/6 = 4

So 29 is actually row 4 column 5 and 21 is row 3 column 3. Index is calculated as @lmathieu said in his comment :

index = x * columns_number + y
Delb gravatar imageDelb ( 2019-10-04 03:38:43 -0500 )edit

Indeed, my indexes are all wrong! Sorry for that. But my consideration about the first index is because on linear algebra textbooks and Matlab, the first index is 1, so I wanted to make things clear before proceeding.

lucascoelho gravatar imagelucascoelho ( 2019-10-04 13:00:57 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-10-03 09:31:17 -0500

Seen: 32 times

Last updated: Oct 04