search

thien94 / vision_to_mavros

A collection of ROS and non-ROS (Python) code that converts data from vision-based system (external localization system like fiducial tags, VIO, SLAM, or depth image) to corresponding mavros topics or MAVLink messages that can be consumed by a flight control stack (with working and tested examples for ArduPilot).
https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html
GNU General Public License v3.0
242 stars 146 forks source link

Custom Camera orientation #52

Closed dookei closed 2 years ago

dookei commented 2 years ago

Hi @thien94 , Great work! I am trying to use this package with a custom camera orientation Facing up with 53° or 37° as bellow. I have tested it with the facing up and USB connector to the front( r = -1.5707963, p = 1.5707963). That works fine. So I assumed that by rolling 127°/−2,21656815 rad would be just fine. But I get the position all wrong. When I move the drone one meter to the front I get the bellow output. I am also trying to add the camera offset(x,y,z) and scale_correction on the ROS package.

rostopic echo /mavros/vision_pose/pose frame_id: "t265_odom_frame" pose: position: x: 0.8028973528494894 y: -0.9401906632686671 z: 0.0489788104758632 orientation: x: -0.01121105751439569 y: -0.0016881932020644808 z: -0.27386077336080583 w: 0.9617025210569757

rostopic echo /t265/odom/sample

frame_id: "t265_odom_frame" child_frame_id: "t265_pose_frame" pose: pose: position: x: -0.6849033236503601 y: -0.56478351354599 z: 0.016267837956547737 orientation: x: 0.03361372649669647 y: -0.6984394788742065 z: 0.030689600855112076 w: 0.7142203450202942

image

Upfacing Upfacing, USB port to the back : r = 1.5707963, p = 1.5707963, y = 0, gamma = -1.5707963 Upfacing, USB port to the front : r = -1.5707963, p = 1.5707963, y = 0, gamma = -1.5707963 Upfacing, USB port to the right : r = 0, p = 1.5707963, y = 0, gamma = -1.5707963 Upfacing, USB port to the left : r = 3.1415926, p = 1.5707963, y = 0, gamma = -1.5707963

    Upfacing, USB port -37° to the front :   r = -2.21656815,    p = 1.5707963, y = 0,  gamma = -1.5707963

image

Am I missing something here ? Any guidance would be much appreciated. Thanks.

thien94 commented 2 years ago

I suspect that with this configuration, the parameters might not be this straightforward

Upfacing, USB port -37° to the front : r = -2.21656815, p = 1.5707963, y = 0, gamma = -1.5707963

It might be easier to understand and keep track if you can keep the known working configuration

Upfacing, USB port to the front : r = -1.5707963, p = 1.5707963, y = 0, gamma = -1.5707963

and append an additional rotation of -37° in this line (to the right, or end of the line) https://github.com/thien94/vision_to_mavros/blob/master/src/vision_to_mavros.cpp#L208

The addition rotation is probably around the z-axis, but you would have to test around to make sure which one works.

dookei commented 2 years ago

Thank you for your very fast reply. I tried like you recommended but not effect. This only generates rotation. The position suffers no effect. _offset = -0.6458 rad = -37 deg

        quat_offset = tf::createQuaternionFromRPY(0, 0, -_offset);
        quat_body = quat_rot_z * quat_cam * quat_cam_to_body_x  * quat_cam_to_body_y * quat_cam_to_body_z * quat_offset;
        quat_body.normalize();

image

_offset = 0.6458 rad = 37 deg image

Shouldn't be made also some changes here? https://github.com/thien94/vision_to_mavros/blob/52b0e81d4c525d0f07168910132a505451d5bd58/src/vision_to_mavros.cpp#L194-L196

thien94 commented 2 years ago

Shouldn't be made also some changes here?

Ah that's true, you also need to append the new rotation there.

dookei commented 2 years ago

So I now got the position correct. But somehow the Roll and pitch are still not working proper.

Added this for pose. Which was tested and is working fine.

        position_orig = transform.getOrigin();
        position_body.setX(  cos(gamma_world) * position_orig.getX() + sin(gamma_world) * position_orig.getY() );
        position_body.setY( -sin(gamma_world) * position_orig.getX() + cos(gamma_world) * position_orig.getY() );
        position_body.setZ(position_orig.getZ()) ;

        position_orig = transform.getOrigin();
        position_new.setX( position_body.getX()*cos(offset_roll) + sin(offset_roll) * position_body.getY() );
        position_new.setY( -sin(offset_roll) * position_body.getX() + cos(offset_roll) * position_body.getY() );
        position_new.setZ(position_body.getZ() ) ;

The drone is around 45°tilted to the front ( -45 pitch). But I get on roll and pitch rotations...Yaw seams to show ok. I forgot to mention I also have the camera with a big offset from the drone center around (-200; -200,+200) mm.

image

dookei commented 2 years ago

So made some modifications under https://github.com/thien94/vision_to_mavros/blob/master/src/vision_to_mavros.cpp#L208 as you proposed and now its working everything. I will only do the offset compensation and thats it :-) Many thanks for your fast help!

thien94 commented 2 years ago

Glad to know it's working!