How to get geometry_msgs/Pose from position(x,y,z) like geometry_msgs/Point

Hi, all. Now I got the position (x,y,z) and velocity (vx,vy,vz) of an agent. Now both the positon and velocity are represented as geometry_msg/Point. Now I want to convert thme to geometry_msgs/Pose and geometry_msgs/Twist, respectively. That is, How to get quaternion of Pose from (x,y,z)? How to get linear and angular parts of Twist from (vx,vy,vz)?

Additionally, if Kalman filter or Particle filter are used for tracking the position and velocity of an agent, how to get the covariance of postion and velocity, respectively? With theirs covariance, both geometry_msgs/PoseWithCovariance and geometry_msgs/TwistWithCovariance can be calculated, respectively.

Thank you!

EDIT1: If kalman filter is used to track the movement of an agent, the position and velocity of the agent can be obtained from kalman tracker. Then how to get the covariance of the position from kalman filter? i.e. how to get the value of geometry_msgs/PoseWithCovariance.msg consisting of pose and covariance from the tracked position?

EDIT2: Thank you! From this wiki of kalman filter, is the P_(k/k-1) the covariance of kalman fitler? However, I still wonder how to get the covariance of particle filter.

edit retag close merge delete

The covariance is just tracked with a Kalman filter, so just fill a message with that after each step in your Kalman filter.

( 2015-02-13 03:56:47 -0500 )edit

Sort by ยป oldest newest most voted

To start, position data alone is insufficient to generate a pose. Does the orientation of the agent actually represent something? If so, you will need to understand and provide that information in order to generate anything useful. If the orientation of the agent is unimportant, just set orientation.x/y/z=0 and w=1.

more