VISION_POSITION_ESTIMATE not appearing in QGroundControl

rmackay9 · March 8, 2018, 11:36am

I just want people to know that adding support for the vision-position-estimate is actively being worked on. We’re making progress, here’s our latest branch which basically seems to work in the simulator but needs more testing.

So, it’s getting close!

jeff_null · March 21, 2018, 5:11pm

@Subodh_Mishra

Yes, I have the local_position plugin whitelisted. It is responsible for publishing the ‘/mavros/local_position/pose’ topic, which is the final, EKF-fused position estimate.
I’ve created a new plugin file (vision_delta_estimate.cpp), as well as updating the appropriate mavros_plugins.xml and CMakeLists.txt files. I’ll be publishing these on our official company’s project github page soon.

I publish the origin-setting mavlink message upon receipt of the first visual pose estimation, but doing it in initialize() should work as well. The complete code to send the message is:

  mavlink::common::msg::SET_GPS_GLOBAL_ORIGIN gpo;
  gpo.target_system = m_uas->get_tgt_system();
  gpo.latitude = 37.2343 * 1E7;
  gpo.longitude = -115.8067 * 1E7;
  gpo.altitude = 610.0 * 1E3; // roughly 610m above sea level
  
  UAS_FCU(m_uas)->send_message_ignore_drop(gpo);

Subodh_Mishra · March 21, 2018, 6:39pm

Thanks… please let me know the link of the github page here so that I can keep looking for the update. Infact I have made it work by doing everything you have mentioned here. Earlier it was not working because my EKF3 was disabled but now it does because I enabled the EKF3. My current problem is with sending deltas, I am not sure how to send correct delta values, I want to know the correct way of doing it.

jeff_null · March 21, 2018, 8:35pm

Calculating the deltas in the correct frame of reference took me quite a while. First, please note that in vision_pose_estimate.cpp, a rotation is added to the incoming pose estimate’s rotations, but NOT the positions. Specifically, on line 126-129:

auto position = ftf::transform_frame_enu_ned(Eigen::Vector3d(tr.translation()));
auto rpy = ftf::quaternion_to_rpy(
			ftf::transform_orientation_enu_ned(
			ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation()))));

For the deltas, remove the “ftf::transform_orientation_baselink_aircraft” call.

Then, calculating the deltas is fairly straightforward:

/// Position delta
Eigen::Vector3d delta_pos = position - prev_position;
// Rotate the position difference into the previous frame of reference
delta_pos = prev_rotation_inverse*delta_pos;

where ‘prev_rotation_inverse’ is the inverse of the previous frame’s rotation. Similarly:

/// Rotation delta
const Eigen::Affine3d cur_rotation = trs_rpy_to_matrix(Eigen::Vector3d(0,0,0), rpy);
const Eigen::Affine3d delta_rot = prev_rotation_inverse*cur_rotation;
const Eigen::Vector3d delta_rpy = quaternion_to_rpy(Eigen::Quaterniond(delta_rot.rotation()));

Put these values into the mavlink::ardupilotmega::msg::VISION_POSITION_DELTA message, and you’re good to go. (Don’t forget to put the time between your delta updates into that message’s ‘time_delta_usec’ field.

rmackay9 · March 22, 2018, 12:07am

As a side note, we have a PR which adds support for the vision-position messages to ArduPilot. It’s still being reviewed but I hope it will go into master by next Monday.

Subodh_Mishra · March 22, 2018, 12:28am

cool i am waiting for it

Subodh_Mishra · April 17, 2018, 5:36pm

Hey, This answer helps. Actually, could you please tell me what order of rotations did you use to conver RPY values to rotation matrix… was it Rot(Roll)*Rot(Pitch)*Rot(Yaw) or anything else?

rmackay9 · April 18, 2018, 4:43am

An update on our progress, the PR mentioned above to add support for the vision-position messages is in master but two users (including @vincekurtz) found that it did not work because the system times are normally different between the companion-computer/GCS and the flight controller. This leads to the EKF ignoring the messages.

We’ve got a new PR that syncronises the clocks between the two computers and we hope this will resolve the problem. Hopefully this PR will go into master this week.

Subodh_Mishra · May 3, 2018, 9:52pm

I just wanted to mention here that for me SET_GPS_GLOBAL_ORIGIN, didn’t work, I used SET_HOME_POSITION and this worked.

Subodh_Mishra · May 3, 2018, 10:06pm

imageedit_2_4202878620

In the image attached to this comment, you can find the pose/tf published/broadcasted using visual marker localisation(Quadrotor pose) and the arducopter code(Quadrotor_pose_FCU via /mavros/local_position/pose). Any idea how can I ensure that both will be aligned, will a simple rotational and positional offset removal work?

ppoirier · May 4, 2018, 1:46am

When working with tf we generally use this tool

github.com

ros/geometry/blob/hydro-devel/tf/src/tf/transformations.py

# -*- coding: utf-8 -*-
# transformations.py

# Copyright (c) 2006, Christoph Gohlke
# Copyright (c) 2006-2009, The Regents of the University of California
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
#   notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above copyright
#   notice, this list of conditions and the following disclaimer in the
#   documentation and/or other materials provided with the distribution.
# * Neither the name of the copyright holders nor the names of any
#   contributors may be used to endorse or promote products derived
#   from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

This file has been truncated. show original

Subodh_Mishra · May 4, 2018, 3:09am

Thanks! I really appreciate this help.

Actually, I don’t understand where I am going wrong. Let me jot down what I am actually doing here:

I am capturing images from a small downward looking camera on board my skyviper drone and streaming it over wifi to my ground station(ubuntu 16.04 running mavros node for apm firmware with the appropriate udp address for skyviper) which runs a ROS node to work on these images.
The ROS node detects AruCo markers (on the ground) that the camera sees and calculates the drone’s pose with respect to a fixed self determined frame using opencv’s aruco marker detection and pose estimation algorithm.
I publish this pose as /mavros/vision_position/pose and the mavros node running on my PC subscribes to this topic to relay it to the FCU.(for sure, I white list the vision_position and local_position plugins)
I make some changes in the vision_pose_estimate.cpp file so that it accepts the pose updates mentioned in the step 4 and calculates position and angular deltas out of it. Infact I follow what @jeff_null has done for his project. Now these deltas are sent from the mavros node to the FCU.
The arducopter code does some magic and sends the local position estimates to the mavros node which publishes a topic called /mavros/local_position/pose containing the local pose estimate data . I use this topic to visualize the data generated by FCU. I don’t understand why there is so much offset between the pose estimated by the aruco marker detection algorithm(/mavros/vision_position/pose) and the one estimated by FCU(/mavros/local_position/pose).

Any help will be gratefully acknowledged!

ppoirier · May 4, 2018, 10:46am

@Subodh_Mishra
Just to make sure, do you publish a Transformation between the Camera and the IMU like this:

ppoirier · May 4, 2018, 11:05am

This project is really interesting, I think @rmackay9 and @priseborough should take a deeper look into this because it is a great demonstrator that can be easily replicated without having to get specialised sensors. I am just a little worried about the latency of frame capture over wifi. @Subodh_Mishra can you share your code ?

Subodh_Mishra · May 4, 2018, 5:06pm

No, I am not explicitly publishing it but I am feeding the offset between the skyviper imu and camera as a parameter in Qgroundcontrol/MissionPlanner

ppoirier · May 4, 2018, 5:10pm

What Parameter are you setting for this offset ?
I saw part of your code on the other discussion, may I take a look at your gitter ?

Subodh_Mishra · May 4, 2018, 5:29pm

Yes I can, but where should I share it.

Yes there is some latency and wifi is a tad bit unreliable at times. The data I published in /mavros/local_position/pose is coming at an average frequency of 4.5 Hz.

I also want to mention that I am using EKF3 for visual odometry fusion as EKF2 didnt seem to take in visual odometry inputs.

As far as I can recall, here are the parameters I had changed to make the FCU accept my /mavros/vision_position/pose updates:
EK3_ENABLE (Enabled this)
VISO_ORIENT
VISO_POS_X
VISO_POS_Y
VISO_POS_Z
VISO_TYPE(should be set to MAV for VO to work)

When the VO fusion starts, the following message is displayed in the mavros teminal:

[ INFO] [1525454530.463380626]: FCU: EKF3 IMU0 started relative aiding [ INFO] [1525454530.648255144]: FCU: Fusing Velocity [ INFO] [1525454530.760722136]: FCU: EKF3 IMU0 fusing odometry
I am sure that the following functions are used for fusing VO data:

writeBodyFrameOdom(deltas) defined in ardupilot/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
&
FuseBodyVel() defined in
ardupilot/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

Which part of code do you want to see? there are different places I made changes in and it’s kind of a mess, but I will be happy to share.

ppoirier · May 4, 2018, 5:48pm

@Subodh_Mishra Genarally, you make a branch of the ardupilot code on tour github account and work from there. As for the other code, like OpenCV and Mavros, you can relicate the workspace as well on github.

As for the offsets:
VISO_ORIENT
VISO_POS_X
VISO_POS_Y
VISO_POS_Z
is related to the vehicle frame, there is no transpose to IMU-CAMERA frames within these parameters.

Subodh_Mishra · May 4, 2018, 8:42pm

Okay I will do that. But the main change I have done in my project is just to enable EKF3, change VISO_TYPE to MAV and send vision estimated pose as VISION_POSITION_DELTA instead of VISION_POSITION_ESTIMATE by doing what @jeff_null has done here (except using SET_HOME_POSITION instead of SET_GPS_GLOBAL_ORIGIN).

What I don’t understand is if I send lets say [0 0 4 0 0 0 1] as /mavros/vision_position/pose then I expect something close to [0 0 4 0 0 0 1] as an output in the topic /mavros/local_position/pose but its not so there is considerable difference in the results. What convention does the FCU use?

Adding, no I don’t publish any IMU-CAM tf, is it necessary?

Subodh_Mishra · May 4, 2018, 8:54pm

Where can I see the changes you have made to incoporate vision_position_estimate instead of vision_position_delta.

Besides this, I want to know the convention in which these vision deltas need to be given… I am giving the input in ENU convention

Here is how my vision_pose_estimate() function in mavros/mavros_extras/src/plugins/vision_pose_estimate.cpp looks like:

void vision_position_estimate(uint64_t usec,
		Eigen::Vector3d &position,
		Eigen::Vector3d &rpy)
{
    Eigen::Vector3d delta_pos = position - prev_position;
    delta_pos = prev_rotation_inverse*delta_pos;
    Eigen::Matrix3d curr_rotation = rpy2rotn(rpy);
    Eigen::Matrix3d delta_rot = prev_rotation_inverse*curr_rotation;
    Eigen::Vector3d delta_rpy = ftf::quaternion_to_rpy(Eigen::Quaterniond(delta_rot));

    prev_position = position;
    prev_rotation = curr_rotation;
    prev_rotation_inverse = prev_rotation.inverse();

	mavlink::ardupilotmega::msg::VISION_POSITION_DELTA vp{};
	vp.time_usec = usec;

    vp.time_delta_usec = usec - old_time;
    old_time = usec;

    //std::cout << vp.time_delta_usec  << std::endl;
	vp.position_delta[0] = delta_pos.x();
	vp.position_delta[1] = delta_pos.y();
	vp.position_delta[2] = delta_pos.z();
	vp.angle_delta[0] = delta_rpy.x();
	vp.angle_delta[1] = delta_rpy.y();
	vp.angle_delta[2] = delta_rpy.z();	


    
    if(flag_var)
    {
    	UAS_FCU(m_uas)->send_message_ignore_drop(vp);
    	//ROS_INFO("[vpe:] Sent vision deltas");
    }
    else
    {
	    flag_var = true;
	    ROS_INFO("[vpe:]First Run, hence ignored!");
    }
}