What is the message of geometry_msgs/PoseWithCovariance

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 ?

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

Thanks in advance :)

edit retag close merge delete

Sort by » oldest newest most voted

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 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]

more

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 = VARIABLE_1'? Is it NOT necessary to write it like 'CUSTOMIZED_MSG_NAME.covariance '?

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

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 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.

more

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

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.