[ros-users] tf and quaternions

[Rodrigo Baravalle]

Thanks Stuart!

I have changed the code and I have succesfully adjusted the orientation.

Best Regards,
Rodrigo

2011/1/6 Stuart Glaser <sglaser at willowgarage.com>:
> Hi Rodrigo,
>
> It's very unlikely that this code will produce a valid quaternion:
>
>> transform = tf::Transform(tf::Quaternion(stransform.getRotation()[0],
>>                                         stransform.getRotation()[1],
>>                                         stransform.getRotation()[2]-0.41,
>>                                         stransform.getRotation()[3]),
>>                tf::Vector3(bswisstransform.getOrigin()));
>
> Quaternions are required to be normalized, and adding values to the
> elements of the quaternions is not a valid operation.
>
> Instead, you should construct an additional quaternion that represents
> the offset of the gyroscope and compose it with the gyroscope's
> rotation.  Something like this:
>
> tf::Quaternion gyroscope_offset(tf::Vector3(0,0,1), M_PI/4.0);  // Axis/angle
> tf::Quaternion robot_rotation = stransform * gyroscope_offset.inverse();
>
> That should give you the rotation of the robot in the world.  Do test
> it carefully, as it's very easy to mess up the order when composing
> transforms.
>
> -Stu
>
> On Thu, Jan 6, 2011 at 7:18 AM, Rodrigo Baravalle <rbaravalle at gmail.com> wrote:
>> Hello everyone!
>>
>> I have already made a question about tf and synchronization which was
>> succesfully answered, so I will tell again briefly what I am doing:
>>
>> I am currently working on building a 3D map of the environment of a
>> robot. I am using a swissranger SR4000 and a xsens MTI for the
>> rotation of the camera. The camera is attached to the robot a bit
>> displaced from the center. More precisely, 80cm back and 1.4m above,
>> so, when the robot turns, the camera describes a circle around the
>> center of the robot. For that, I have made this transformation:
>>
>> ...
>> swissranger.header.frame_id = "base_link";
>> swissranger.child_frame_id = "swissranger";
>> swissranger.transform.translation.x = x_scan_offset_-0.8; // offset in
>> x for the camera
>> swissranger.transform.translation.y = y_scan_offset_;
>> swissranger.transform.translation.z = 1.4; // height at which the
>> camera is placed
>> ...
>>
>> I have a 2D map of the same environment, and I use that map for
>> localization with amcl. When a new message from amcl arrives, I took a
>> pointcloud from the swissranger. I also use the transformation from tf
>> between /map and /swissranger to get the actual x-y-z of the
>> swissranger in the map:
>>
>> ...
>> tlistener.lookupTransform("/map", pc_current.header.frame_id,
>>                               pc_current.header.stamp, bswisstransform);
>>
>> ...
>>
>> my first question is: does this sentence represent the actual position
>> of the swissranger in the map?:
>>
>> bswisstransform.getOrigin();
>>
>> I also have the values of the gyroscopes in a transformation called
>> stransform. I publish it as a transformation between "/map" and a new
>> frame which I named "/rot_swissranger", because I only need the
>> values. It is ok to do that in this way?
>>
>> Finally, I want to use the position of the swissranger + the values of
>> the gyroscopes of the xsens to build a transformation in this way:
>>
>>
>
>>
>> The "0.41" value is there because the gyroscope is put in a different
>> orientation to the direction that the robot moves. The problem is that
>> it works well (or at least, not too bad) when it moves horizontally or
>> vertically, but, when it turns, the point clouds are not transformed
>> properly, it seems that the rotation is greater than desired, but when
>> the robot reachs again an horizontal or vertical position the clouds
>> appear in a more correct position again.
>>
>> I read in the xsens manual that the quaternions returned are unit
>> vectors, so I try to normalize the resultant quaternion in this way:
>>
>> double a,b,c,d;
>> a = stransform.getRotation()[0];
>> b = stransform.getRotation()[1];
>> c = stransform.getRotation()[2]-0.41;
>> d = sqrt(1 - a*a - b*b - c*c);
>>
>> transform_rotation2 = tf::Transform(tf::Quaternion(a,b,c,d),
>>            tf::Vector3(bswisstransform.getOrigin()));
>>
>> But this is worst. So, the d value seems of great importance. Which is
>> the mistake in the values of the gyroscopes I am using?
>>
>> Thanks in advance.
>> _______________________________________________
>> ros-users mailing list
>> ros-users at code.ros.org
>> https://code.ros.org/mailman/listinfo/ros-users
>>
>
>
>
> --
> Stuart Glaser
> sglaser -at- willowgarage -dot- com
> www.willowgarage.com
> _______________________________________________
> ros-users mailing list
> ros-users at code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users
>