VISION_POSITION_ESTIMATE not appearing in QGroundControl

Subodh_Mishra · September 5, 2018, 9:47pm

Hey @anbello thanks for your response. But can you be a little more clear and specific? Can you please tell me the parameters you have set?

anbello · September 6, 2018, 12:55pm

Hi @Subodh_Mishra
I have the following parameters on my quad:
AHRS_EKF_TYPE 2
EKF2_ENABLE 1
EKF3_ENABLE 0
GPS_TYPE 0
EK2_GPS_TYPE 3
COMPASS_USE 0
VISO_TYPE 0

my quad stream the video from a camera via WiFi like your, only it is DIY:

little quad (130mm) with RevoMini FC with Arducopter 3.6 rc7
on the quad there is a Raspberry Pi 0 with Raspberry Cam streaming video to Desktop PC
on Desktop PC there is ROS Kinetic with mavros and aruco_mapping
the video is streamed with gstreamer and on the PC the node gscam get this stream and publish camera/image_raw and camera/camera_info topics
on the PC aruco_mapping (slightly modified by me) subscribe to above topics and publish a camera_pose message to the mavros/vision_pose/pose topic.

[Edit]
I forget to say that i send a SET_GPS_GLOBAL_ORIGIN and a SET_HOME_POSITION messages (similar to this) before sending vision position estimate.

When all this is running and I have the quad on my hand (disarmed) with camera pointing to a marker I can see /mavros/vision_pose/pose and /mavros/local_position/pose that moves in a coherent manner.

When I try to arm and fly over the marker if I switch to Loiter mode the quad drift and is not stable.

One problem I see is a not so good video quality and when I fly over the marker the marker itself is detected intermittently and not in a smooth an continuous manner as needed. I have to work to improve the quality of the streamed video and perhaps to redo a camera calibration procedure.

Another thing that I did not understand well is if I’m using the correct transformation when I send the message /mavros/vision_pose/pose, I am talking about the ENU NED stuff as explained here under " Asserting on Reference Frames".

Subodh_Mishra · September 6, 2018, 2:35pm

Thanks @anbello, I am going to try this and get back to you in a day or two.

Subodh_Mishra · September 6, 2018, 2:45pm

Hey @anbello, Do you arm your quad using the controller/transmitter or in using ros commands for mavros?

Subodh_Mishra · September 6, 2018, 3:04pm

Don’t you think this should be set to 3, which means no GPS?

anbello · September 6, 2018, 3:21pm

Yes it’s a typo I mean 3
now I edit the previous post with the right value, thanks

anbello · September 6, 2018, 3:23pm

I arm with the controller

Subodh_Mishra · September 6, 2018, 7:08pm

What mode are you in when you arm your quadrotor?

anbello · September 6, 2018, 9:56pm

I arm the quadrotor in ALTHOLD mode

Subodh_Mishra · September 7, 2018, 3:16pm

Thanks for the response @anbello. Even I arm it in the ALT HOLD mode., But I use Qgroundcontrol to do this. After arming (actually you can do this without arming) I give the takeoff command from Qgroundcontrol and then the motors start working(I have removed the propellors for safety) as if its trying to takeoff and the same time my mode automatically switches to guided mode. Does it happen with you?

I had one more doubt, what did you mean when you said " with camera pointing to a marker I can see /mavros/vision_pose/pose and /mavros/local_position/pose that moves in a coherent manner."? Do these two topics publish exactly the same values?

anbello · September 7, 2018, 4:08pm

Not exactly the same values but near values.
The values coming from /mavros/vision_pose/pose varies quickly (little errors in the estimation from aruco marker), those coming from /mavros/local_position/pose are smoothed from the EKF.
When I will have the time I will post a video to clarify better.

Subodh_Mishra · September 7, 2018, 4:53pm

Thanks @anbello. Please do! For me these values are different. I dont understand why.

rmackay9 · September 8, 2018, 2:54am

Subodh,

It’s normal for the vehicle to switch to Guided when the takeoff command is issues from QGC. It’s actually QGC that is commanding the vehicle to go into Guided mode.

anbello · September 9, 2018, 2:59pm

Here a video of a test of my system:

After starting video streaming on Raspberry Pi 0 on my quadcopter (connected to desktop PC via WiFi) I started ROS, aruco_mapping and mavros on PC and moved the quadcopter over the marker with my hand.
The two axes systems represent the /mavros/vision_pose/pose and /mavros/local_position/pose topics, the same topics are echoed on terminals.
The /mavros/vision_pose/pose move quickly and with more disturbances respect /mavros/local_position/pose that is smoothed by EKF.
The two poses have enough near values, only when I rotate yaw left respect to initial heading I see near angles in term of RPY but different values for the quaternions (there are two values of quaternions that give the same RPY angles).

Subodh_Mishra · September 10, 2018, 2:06pm

Really interesting!! I am planning to achieve something like this. I think I am close but not quite there. I think I need to use the aruco_mapping software instead of my own.

Subodh_Mishra · September 10, 2018, 3:03pm

@anbello I am going to post my video too. Looks somewhat similar. One thing I wanted to know is how and where do you feed the extrinsic calibration b/w the camera center and the the FCU center(or IMU center)? So that these two aforementioned topics can coincide. My result shows some offset betweem them. I also direct this question to @rmackay9. Thanks!

Subodh_Mishra · September 10, 2018, 4:40pm

Hey @anbello here is a link to the video I recorded. The axes with the label “Quadrotor_pose” is /mavros/vision_position/pose which is generated by aruco marker based pose estimation and the unlabelled axes is /mavros/localposition/pose. There is so offset between them probably because lack of extrinsic params between the camera and IMU and also as you can see the aruco marker based pose estimation is jittery when compared to the ekf o/p.

anbello · September 11, 2018, 3:32pm

Hi @Subodh_Mishra I looked at your video, it seems to me that you are almost there. I saw the offset you are talking about but I have no explanation for that, I didn’t anything to have my two poses almost near.
I think there should be some parameters to set the position and orientation of the camera as there is for VISO_* parameters (useful only for the VISION_POSITION_DELTA message) but there aren’t.
In the lack of these parameters it should be possible to transform the pose object with ROS function before sending the message.

Subodh_Mishra · September 11, 2018, 5:44pm

Hey @anbello, thanks for the inputs. I wanted to know if you are able to hover over this aruco marker autonomously?

Subodh_Mishra · September 11, 2018, 6:24pm

Hi @anbello, I also wanted to know how did you make aruco_mapping work? In my case it does see the aruco marker but does not do any pose estimation. Actually I am confused bw the camera matrix and the projection matrix in the calibration file provided to the aruco_mapping. Shouldn’t they be the same?

Besides this I set the marker size as the size of the aruco marker (as it is obvious) and for me it is 14.1 cm so I set it 0.141. Also I set the correct image height and width. Still doesn’t work.

What version of ROS, OpenCV are you using?