Computing PoseWithCovariance's 6x6 matrix

asked 2014-07-01 09:33:43 -0500

Mainframe
1 ●1 ●2 ●2

Greetings,

I'm currently developing in Unity3D a projection mapping application to be used with a projector mounted on a robot, displaying Augmented Reality content in any (planar) surface. The application will require the robot's positioning which will be published in a ROS topic. Unfortunately I don't have access to the robot yet, however I do know the type of message chosen to represent its location: PoseWithCovariance msg (docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovariance.html)

My experience with ROS is extremely limited and the end goal is to convert the positional information contained within PoseWithCovariance's 6x6 matrix and convert it to its respective 3D position and orientation in a referential defined within Unity3D. The values in Unity3D are simply the position (x,y,z) and rotation around the 3 axis. My problem lies with the conversion of the matrix into those values. Unfortunately the raw message definition alone linked earlier doesn't include an example of some sort or additional info on how to convert the matrix values.

Any help would be greatly appreciated!

edit retag flag offensive close merge delete

add a comment

3

answered 2014-07-02 01:04:37 -0500

ahendrix

47576 ●179 ●367 ●662 http://namniart.com/

A PoseWithCovariance consists of: an XYZ position, a Quaternion representing the orientation, and a 6x6 matrix representing the positional and angular uncertainty.

The XYZ position is simple. Note that the units should be in meters.

Quaternions are an alternate method of representing orientation, which are more mathematically stable than euler angles (roll, pitch and yaw) or rotation matrices. You can use the tf library to create a quaternion given the Euler anlges:

geometry_msgs::Quaternion   tf::createQuaternionMsgFromRollPitchYaw (double roll, double pitch, double yaw)

The 6x6 covariance matrix represents the position and orientation covariance between the x, y, z, roll, pitch and yaw variables. If you don't have this or don't care about it for your application, you can leave it as zeroes.

edit flag offensive delete link

Comments

The only information the application needs is (most accurate) localization. After reading the covariance definition I guess the 6x6 matrix will not be needed. I am however confused about PoseWithCovariance Message definition: it states that "# In order, the parameters are: # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)" with no W variable typically present in the Quaternion, which from what I've read so far represents the amount of rotation so it would have to be present right?

Mainframe ( 2014-07-02 06:14:33 -0500 )edit

add a comment

1

answered 2014-07-01 22:23:16 -0500

sai
1925 ●58 ●75 ●86

For this,

you have to define all the variables in the message type and publish it.

Example Here is sample code snippet, you may have to extend it if I have missed any.

Assuming the the variables x,y,z represent the 3D position.

geometry_msgs::PoseWithCovarianceStampedConstPtr& msg;

msg->pose.pose.position.x = x;

msg->pose.pose.position.y = y;

msg->pose.pose.position.z = z;

Assuming that convert the 3D angles into quarternion representation gives (theta1, theta2, theta3, theta4)

msg->pose.pose.orientation.x = theta1;

msg->pose.pose.orientation.y = theta2;

msg->pose.pose.orientation.z = theta3;

msg->pose.pose.orientation.w = theta4;

Then you have properly assign a header to this msg

msg->header = another_time_Stamped_msg.header;

or create a header by assigning a time and a frame id, for example

msg->header.frame_id = "\my_frame_id";

msg->header.stamp = ros::Time current_time;

I hope this solves the issue.

edit flag offensive delete link

Comments

My doubts are not so much regarding ROS inner works (someone else is working on that), I just need to convert the values from a PoseWithCovariance msg into positional information, rotation included. Unity3D supports both euler and quaternion angles, chances are I'll end up doing a quaternion->euler conversion, at least at early stages of the project to help debugging the application possible issues that may arise.

Mainframe ( 2014-07-02 06:21:41 -0500 )edit

add a comment

Computing PoseWithCovariance's 6x6 matrix

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Computing PoseWithCovariance's 6x6 matrix edit

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

Computing PoseWithCovariance's 6x6 matrix