[PLEASE DO MERGE] ECL reference frame alignment fix #12771

kamilritz · 2019-08-22T06:52:13Z

This includes a update in ECL that fixes the alignment between an external vision reference frame and the EKF reference frame. In addition to the ECL update, this also adds a new odometry topic on which now the aligned visual odometry is published from ekf_main.cpp

This has been tested extensively.

The wrong alignment of the reference frame made fusion of GPS and external vision pose information impossible.

The correct alignment of the visual odometry was tested on several recorded logs:

Note: the jump in the external vision quaternion is due to the handling of non canonical quaternion by the external vision sensor.

jkflying

Small changes, otherwise I think it is OK. It would be great to have a log from Flight Review attached.

src/modules/ekf2/ekf2_main.cpp

jkflying

LGTM, rebase and we'll get this in

mhkabir · 2019-08-30T12:17:25Z

msg/vehicle_odometry.msg

-float32[4] q			# Quaternion rotation from NED earth-fixed frame to XYZ body frame
+float32[4] q			# Quaternion rotation from FRD body frame to fixed frame
+
+float32[4] q_err		# Quaternion rotation from odometry's fixed frame to local_origin_frame


I don't like this name. @jkflying @TSC21 any other naming ideas for this?

What about q_offset?

TSC21

I don't think you need a new topic just to propagate the aligned frame. A simple field stating the alignment is enough. Use the vehicle_visual_odometry.

TSC21 · 2019-08-30T12:49:19Z

src/modules/ekf2/ekf2_main.cpp

@@ -286,6 +290,7 @@ class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams
 	uORB::Publication<wind_estimate_s>			_wind_pub{ORB_ID(wind_estimate)};
 	uORB::PublicationData<vehicle_global_position_s>	_vehicle_global_position_pub{ORB_ID(vehicle_global_position)};
 	uORB::PublicationData<vehicle_local_position_s>		_vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
+	uORB::PublicationData<vehicle_odometry_s>		_vehicle_aligned_visual_odometry_pub{ORB_ID(vehicle_aligned_visual_odometry)};


No need for a new message.

@TSC21 I think this would be cleaner than using instances to publish this, especially because you could potentially have multiple odometry sources. Also, we generally don't change a topic and re-publish it under a new instance, right?

TSC21 · 2019-08-30T12:51:17Z

@kamilritz you have https://github.com/PX4/Firmware/blob/master/msg/vehicle_odometry.msg#L21. Just set the frame to OTHER and add a verification on the ekf2_main to apply the alignment or not.

kamilritz · 2019-09-09T12:01:22Z

@TSC21 I added the vehicle_visual_odometry as a multi instance topic. Instance 0 is the original visual odometry, while instance 1 is the aligned odometry. This way we can log the both instances of the odometry message and plot them for analysis.
! DO NOT MERGE: Making vehicle_visual_odometry as a multi instance topics breaks the EKF replay

TSC21 · 2019-09-09T12:09:06Z

src/modules/uORB/PublicationMulti.hpp

@@ -59,9 +59,10 @@ class PublicationMulti
 	 * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
 	 * @param priority The priority for multi pub/sub, 0 means don't publish as multi
 	 */
-	PublicationMulti(const orb_metadata *meta, uint8_t priority = ORB_PRIO_DEFAULT) :
+	PublicationMulti(const orb_metadata *meta, uint8_t priority = ORB_PRIO_DEFAULT, int instance = 0) :


I am not sure we are supposed to change these. @dagar can you comment?

kamilritz · 2019-09-19T13:31:19Z

@jkflying can we get this is in.

TSC21 · 2019-09-19T13:33:42Z

I think the change on PublicationMulti should be verified first. @dagar

kamilritz · 2019-09-23T12:06:02Z

@TSC21 I reverted some of the commits such that we have now again vehicle_aligned_visual_odometry and no multi instance topic any more.

TSC21 · 2019-09-23T12:21:42Z

@TSC21 I reverted some of the commits such that we have now again vehicle_aligned_visual_odometry and no multi instance topic any more.

For what reason?

jkflying · 2019-09-23T12:52:13Z

For what reason?

It's to be more in line with how we deal with other topics like the obstacle_distance, where we have the _fused version. Using the same topic for both input and output can be confusing.

TSC21 · 2019-09-23T12:55:22Z

For what reason?

It's to be more in line with how we deal with other topics like the obstacle_distance, where we have the _fused version. Using the same topic for both input and output can be confusing.

I have said this to @kamilritz on Slack but I can say it here as well: The only issue I see with this is that RTPS will not work with multi topics. That means that if we want to send mocap data through ROS2, you won't be able to use multi instances. While using with aliases it would work right.

One of the main reasons it was opted to use aliases with this is that you could send mocap or vision data from RTPS/ROS and that would be handled differently on the system because the aliases would be taken as different topics in the RTPS stream.

I would say that's ok because you won't send mocap and vision at the same time. But if for some reason you wanted to record groundtruth in ulog, you won't be able to do it, at least using RTPS.

mhkabir · 2019-09-23T12:55:35Z

src/modules/ekf2/ekf2_main.cpp

-							       sqrtf(fmaxf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_ROLL_VARIANCE],
-									   fmaxf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_PITCH_VARIANCE],
-											   ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]))));
+							       sqrtf(fmaxf(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_ROLL_VARIANCE],


Since we only fuse yaw, this should be the yaw variance only.

agree. will add this

mhkabir · 2019-09-23T12:58:41Z

msg/vehicle_odometry.msg

@@ -29,7 +29,8 @@ float32 y			# East position
 float32 z			# Down position

 # Orientation quaternion. First value NaN if invalid/unknown
-float32[4] q			# Quaternion rotation from NED earth-fixed frame to XYZ body frame
+float32[4] q			# Quaternion rotation from FRD body frame to fixed frame
+float32[4] q_offset		# Quaternion rotation from odometry's fixed frame to local_origin NED frame


How about "Quaternion rotation from odometry fixed frame to navigation frame."

mhkabir · 2019-09-23T12:59:41Z

src/modules/ekf2/ekf2_main.cpp

+					for (unsigned i = 0; i < 4; i++) {
+						aligned_ev_odom.q[i] = ev_quat_aligned(i);
+						aligned_ev_odom.q_offset[i] = quat_ev2ekf(i);
+					}


Use copyTo here?

mhkabir · 2019-09-23T13:01:13Z

src/modules/ekf2/ekf2_main.cpp

+					aligned_ev_odom.vy = aligned_vel(1);
+					aligned_ev_odom.vz = aligned_vel(2);
+
+					// Compute orientation in local_origin_NED frame


Prefer "navigation frame"

mhkabir · 2019-09-23T13:02:10Z

src/modules/ekf2/ekf2_main.cpp

+
+					vehicle_odometry_s aligned_ev_odom = _ev_odom;
+
+					// Rotate external position and velocity into local_origin_NED frame


Prefer "navigation frame"

mhkabir · 2019-09-23T13:02:36Z

src/modules/ekf2/ekf2_main.cpp

+				// publish external visual odometry after fixed frame alignment if new odometry is received
+				if (new_ev_data_received) {
+					float q_ev2ekf[4];
+					_ekf.get_ev2ekf_quaternion(q_ev2ekf); // rotates from EV to EKF reference frame


Prefer "EKF navigation frame"

mhkabir

LGTM

mhkabir · 2019-09-23T13:44:54Z

@kamilritz Build is broken - can you please fix and post a quick test log with the fixed state?

TSC21 · 2019-09-23T13:51:33Z

msg/vehicle_odometry.msg

@@ -56,4 +56,4 @@ float32 yawspeed		# Angular velocity about Z body axis
 # If angular velocity covariance invalid/unknown, 16th cell is NaN
 float32[21] velocity_covariance

-# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry vehicle_aligned_visual_odometry
+# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry vehicle_visual_odometry_aligned


What if mocap is not aligned?

Are people still using it? At the moment it is used only in LPE and Attitude estimator.

We can just call it vehicle_odometry_aligned. I don't think anyone will be using mocap and VO simultaneously as a source of odometry. Maybe as groundtruth, but that is beyond the scope of this PR.

vehicle_odometry_aligned will be messed with the vehicle_odometry, which is curre tly being used as the feedback of the estimator. Let's keep it this way for now.

TSC21 · 2019-09-23T18:27:32Z

Why was this merged? It's still missing the RTPS ID for the new alias.

Note to self: have to make it mandatory.

kamilritz force-pushed the aligned_vehicle_odometry branch from 72008a3 to e3981b8 Compare August 23, 2019 06:38

kamilritz changed the title ~~[WIP] aligned visual odometry~~ ECL reference frame alignment fix Aug 23, 2019

weekly-digest bot mentioned this pull request Aug 25, 2019

Weekly Digest (18 August, 2019 - 25 August, 2019) #12803

Closed

kamilritz force-pushed the aligned_vehicle_odometry branch from e3981b8 to 8a3b222 Compare August 29, 2019 13:59

jkflying requested changes Aug 29, 2019

View reviewed changes

src/modules/ekf2/ekf2_main.cpp Outdated Show resolved Hide resolved

src/modules/ekf2/ekf2_main.cpp Show resolved Hide resolved

jkflying previously approved these changes Aug 30, 2019

View reviewed changes

mhkabir reviewed Aug 30, 2019

View reviewed changes

TSC21 requested changes Aug 30, 2019

View reviewed changes

TSC21 reviewed Aug 30, 2019

View reviewed changes

TSC21 requested review from jkflying and mhkabir August 30, 2019 12:56

weekly-digest bot mentioned this pull request Sep 1, 2019

Weekly Digest (25 August, 2019 - 1 September, 2019) #12858

Closed

kamilritz dismissed jkflying’s stale review via 4ddc9ef September 9, 2019 08:33

kamilritz force-pushed the aligned_vehicle_odometry branch 2 times, most recently from 4ddc9ef to 8ea7e75 Compare September 9, 2019 11:57

TSC21 reviewed Sep 9, 2019

View reviewed changes

TSC21 requested a review from dagar September 9, 2019 12:19

kamilritz changed the title ~~ECL reference frame alignment fix~~ [DO NOT MERGE] ECL reference frame alignment fix Sep 11, 2019

weekly-digest bot mentioned this pull request Sep 15, 2019

Weekly Digest (8 September, 2019 - 15 September, 2019) #12959

Closed

Fix EKF frame alignemen in ECL

d85ee9f

kamilritz force-pushed the aligned_vehicle_odometry branch from 2227e10 to 2697786 Compare September 19, 2019 12:21

kamilritz changed the title ~~[DO NOT MERGE] ECL reference frame alignment fix~~ [PLEASE DO MERGE] ECL reference frame alignment fix Sep 19, 2019

weekly-digest bot mentioned this pull request Sep 22, 2019

Weekly Digest (15 September, 2019 - 22 September, 2019) #13002

Closed

kamilritz added 2 commits September 23, 2019 12:46

Remove empty lines

ff968f5

Add initalization for ev_odom

d8c00f3

kamilritz force-pushed the aligned_vehicle_odometry branch from 1b9ee35 to d8c00f3 Compare September 23, 2019 10:47

mhkabir requested changes Sep 23, 2019

View reviewed changes

mhkabir reviewed Sep 23, 2019

View reviewed changes

kamilritz added 4 commits September 23, 2019 15:24

Only use yaw covariance for angErr

e62211e

Improve frame naming in comments

dae7b97

Use copyTo

98b96cf

Add aligned as suffix

b3a1127

mhkabir previously approved these changes Sep 23, 2019

View reviewed changes

Add missing vehicle_visual_odometry_aligned

6ebc362

kamilritz dismissed mhkabir’s stale review via 6ebc362 September 23, 2019 13:47

TSC21 reviewed Sep 23, 2019

View reviewed changes

mhkabir merged commit 7427768 into PX4:master Sep 23, 2019

weekly-digest bot mentioned this pull request Sep 29, 2019

Weekly Digest (22 September, 2019 - 29 September, 2019) #13050

Closed

dagar mentioned this pull request Oct 27, 2020

rename vehicle_visual_odometry_aligned -> estimator_visual_odometry_aligned #16060

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[PLEASE DO MERGE] ECL reference frame alignment fix #12771

[PLEASE DO MERGE] ECL reference frame alignment fix #12771

kamilritz commented Aug 22, 2019 •

edited

Loading

jkflying left a comment

jkflying left a comment

mhkabir Aug 30, 2019

TSC21 Aug 30, 2019

kamilritz Sep 9, 2019

TSC21 left a comment

TSC21 Aug 30, 2019

julianoes Sep 20, 2019

TSC21 commented Aug 30, 2019

kamilritz commented Sep 9, 2019 •

edited

Loading

TSC21 Sep 9, 2019

kamilritz commented Sep 19, 2019

TSC21 commented Sep 19, 2019

kamilritz commented Sep 23, 2019

TSC21 commented Sep 23, 2019

jkflying commented Sep 23, 2019

TSC21 commented Sep 23, 2019

mhkabir Sep 23, 2019

kamilritz Sep 23, 2019

mhkabir Sep 23, 2019

mhkabir Sep 23, 2019

mhkabir Sep 23, 2019

mhkabir Sep 23, 2019

mhkabir Sep 23, 2019

mhkabir left a comment

mhkabir commented Sep 23, 2019

TSC21 Sep 23, 2019

kamilritz Sep 23, 2019

mhkabir Sep 23, 2019

TSC21 Sep 23, 2019

TSC21 commented Sep 23, 2019 •

edited

Loading


		vehicle_odometry_s aligned_ev_odom = _ev_odom;

		// Rotate external position and velocity into local_origin_NED frame

[PLEASE DO MERGE] ECL reference frame alignment fix #12771

[PLEASE DO MERGE] ECL reference frame alignment fix #12771

Conversation

kamilritz commented Aug 22, 2019 • edited Loading

jkflying left a comment

Choose a reason for hiding this comment

jkflying left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

TSC21 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

TSC21 commented Aug 30, 2019

kamilritz commented Sep 9, 2019 • edited Loading

Choose a reason for hiding this comment

kamilritz commented Sep 19, 2019

TSC21 commented Sep 19, 2019

kamilritz commented Sep 23, 2019

TSC21 commented Sep 23, 2019

jkflying commented Sep 23, 2019

TSC21 commented Sep 23, 2019

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mhkabir left a comment

Choose a reason for hiding this comment

mhkabir commented Sep 23, 2019

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

TSC21 commented Sep 23, 2019 • edited Loading

kamilritz commented Aug 22, 2019 •

edited

Loading

kamilritz commented Sep 9, 2019 •

edited

Loading

TSC21 commented Sep 23, 2019 •

edited

Loading