Gate pose estimator #2330

guidoAI · 2018-09-17T15:48:43Z

Next step in bringing the autonomous drone race code into master: figuring out the position of the drone relative to the gate. The PnP solver is in principle generic, but is for now only used in conjunction with the gate detection. Some additional math functions were necessary, these have been added as well.

…ners yet.

…all necessary parameters) and snaake_gate_detection (which only takes the image and coordinates)

… Michael

Mutex [fix] mutex_init gate_detect

gautierhattenberger

except the ABI id issue, it looks fine. Maybe you should also run the code_style script.

gautierhattenberger · 2018-09-18T12:04:39Z

sw/airborne/modules/computer_vision/lib/vision/PnP_AHRS.c

+  struct FloatVect3 pos_drone_E_vec;
+
+  // Make an identity matrix:
+  static struct FloatRMat I_mat;


there are a FLOAT_MAT33_ZERO and a FLOAT_MAT33_DIAG macros

gautierhattenberger · 2018-09-18T12:06:01Z

sw/airborne/modules/computer_vision/lib/vision/PnP_AHRS.c

+
+  // vectors in world coordinates, that will be "attached" to the world coordinates for the PnP:
+  struct FloatVect3 gate_vectors[4], vec_B, vec_E, p_vec, temp_vec;
+  p_vec.x = 0;


FLOAT_VECT3_ZERO() ?

gautierhattenberger · 2018-09-18T12:07:52Z

sw/airborne/modules/computer_vision/snake_gate_detection.c

@@ -147,7 +148,7 @@ int snake_gate_detection(struct image_t *img, int n_samples, int min_px_size, fl

  int x, y;
  best_quality = 0;
-  best_gate.quality = 0;
+  (*best_gate).quality = 0;


I think best_gate->quality is cleaner but it's not that important

For a next time : )

Why do things tomorrow when they can be done today? It would be easiest to revert this file and search replace best_gate. with best_gate-> (plus the greater than or equal to on line 279).

hint:

sed -i 's#(*best_gate).#best_gate->#g' snake_gate_detection.c

gautierhattenberger · 2018-09-18T12:15:20Z

sw/airborne/modules/computer_vision/detect_gate.c

+  pthread_mutex_lock(&gate_detect_mutex);
+  if (detect_gate_has_new_data) {
+    detect_gate_has_new_data = false;
+    AbiSendMsgRELATIVE_LOCALIZATION(DETECT_GATE_ABI_ID, cnt++,


When sending an ABI message, it shouldn't be with the broadcast value, but with a unique ID (for this type of message) from subsystems/abi_sender_ids.h.
The ABI_BROADCASTshould only be used when binding to a message without filtering on the sender.

I've added an ABI id to the abi_sender_ids.h file

…tening matrix and vector initializations

guidoAI · 2018-09-18T19:04:17Z

Followed (almost) all advice (see above).

kirkscheper

Looks good. Just remove DETECT_GATE_ABI_ID from the xml. The rest are comments

kirkscheper · 2018-09-18T12:21:29Z

sw/airborne/math/pprz_simple_matrix.h

+     for (l=0; l<_i; l++){                \
+       printf("{");                       \
+       for (c=0; c<_j; c++){              \
+     if(c<(_j-1)){printf("%f,",A[l][c]);} \


spacing here is inconsistent

yes - corrected it now

kirkscheper · 2018-09-19T07:22:28Z

conf/modules/cv_detect_gate.xml

@@ -34,6 +34,7 @@
    <define name="DETECT_GATE_U_MAX" value="121" description="Maximal U of the color filter."/>
    <define name="DETECT_GATE_V_MIN" value="134" description="Minimal V of the color filter."/>
    <define name="DETECT_GATE_V_MAX" value="230" description="Maximal V of the color filter."/>
+    <define name="DETECT_GATE_ABI_ID" value="ABI_BROADCAST" description="ID of the RELATIVE_LOCALIZATION message sent"/>


This should be removed.

kirkscheper · 2018-09-19T07:31:16Z

sw/airborne/modules/computer_vision/lib/vision/PnP_AHRS.c

+    VECT3_SDIV(vec_E, vec_E, vec_norm);
+
+    // to ned
+    vec_E.z = -vec_E.z;


This seems a bit strange... Flipping the sign of only one of the axis would suggest that one of the rotations is not correct.

We will tackle this in a future pull request - for now this is necessary to comply with the control code that we will work on next.

kirkscheper · 2018-09-19T07:34:27Z

sw/airborne/modules/computer_vision/snake_gate_detection.c

@@ -147,7 +148,7 @@ int snake_gate_detection(struct image_t *img, int n_samples, int min_px_size, fl

  int x, y;
  best_quality = 0;
-  best_gate.quality = 0;
+  (*best_gate).quality = 0;


Why do things tomorrow when they can be done today? It would be easiest to revert this file and search replace best_gate. with best_gate-> (plus the greater than or equal to on line 279).

kirkscheper · 2018-09-19T07:34:57Z

sw/airborne/modules/computer_vision/snake_gate_detection.c

+        (*best_gate).quality = temp_check_gate.quality;
+        (*best_gate).n_sides = temp_check_gate.n_sides;
+        memcpy(&((*best_gate).x_corners[0]), &(temp_check_gate.x_corners[0]), sizeof(int) * 4);
+        memcpy(&((*best_gate).y_corners[0]), &(temp_check_gate.y_corners[0]), sizeof(int) * 4);


This was also reverted to an old form, should just be best_gate->x_corners and sizeof(best_gate->x_corners) instead of sizeof(int) * 4 is more robust to possible changes to the type and size of the array being copied. Writing past the end of the array could be pretty bad.

strange that it is back in the old form... I have a habit of putting things off till tomorrow when they do not have to be done today, since there are other pressing deadlines tomorrow that need to be solved today. But I will do it.

guidoAI and others added 23 commits September 7, 2018 22:16

First addition of snake gate functions, no refinement of the gate cor…

feae77b

…ners yet.

Added corner refinement functions

e5b2ee0

Added an initial module, and added check_color to both image.h (with …

ec9e594

…all necessary parameters) and snaake_gate_detection (which only takes the image and coordinates)

Compiling

35e3fcb

Debugging the snake gate detection

c2cbad9

Added settings to the gate detection module

e640b8d

Added a few settings, removed a few spurious variables

eaa9170

start of code for pose estimation - missing mathematical functions by…

34686d4

… Michael

Added the matrix calcaluation functions necessary for PnP

b4bee7c

In principle, the pipeline is in place, not compiling yet though.

73504a4

Resolving some first small errors

8b57acb

Code compiling without warnings.

5a4d968

debugging position estimator

1af0981

Pose estimation working

279cf50

GateDetect pushes ABI message

da6babb

Mutex [fix] mutex_init gate_detect

correct the x,x,x to x,y,z mistake

32a75b9

First part of Kirks comments

51f5191

Processed remaining remarks of Kirk

1e0d280

Use Bound, now correctly

4251b28

Removed listener

4fa4515

merging with the accepted pull request for the snake gate detection

1dadd0e

Merge with latest master

8556e62

Added comments, removed printing, fixed code style

095cead

gautierhattenberger requested changes Sep 18, 2018

View reviewed changes

gautierhattenberger added Airborne computer vision labels Sep 18, 2018

Incorporated all changes from the code review, adding ABI ID and shor…

44b96b0

…tening matrix and vector initializations

kirkscheper requested changes Sep 19, 2018

View reviewed changes

Final edits to align spaces, make sizeofs more robust, etc.

1fd3530

kirkscheper approved these changes Sep 19, 2018

View reviewed changes

gautierhattenberger approved these changes Sep 19, 2018

View reviewed changes

gautierhattenberger merged commit 5fe023a into paparazzi:master Sep 19, 2018

shuoli-robotics pushed a commit to shuoli-robotics/paparazzi that referenced this pull request Oct 15, 2018

Gate pose estimator (paparazzi#2330)

9775cc9

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Gate pose estimator #2330

Gate pose estimator #2330

guidoAI commented Sep 17, 2018

gautierhattenberger left a comment

gautierhattenberger Sep 18, 2018

guidoAI Sep 18, 2018

gautierhattenberger Sep 18, 2018

guidoAI Sep 18, 2018

gautierhattenberger Sep 18, 2018

guidoAI Sep 18, 2018

kirkscheper Sep 19, 2018

gautierhattenberger Sep 19, 2018

guidoAI Sep 19, 2018

gautierhattenberger Sep 18, 2018

guidoAI Sep 18, 2018

guidoAI commented Sep 18, 2018

kirkscheper left a comment

kirkscheper Sep 18, 2018

guidoAI Sep 19, 2018

kirkscheper Sep 19, 2018

guidoAI Sep 19, 2018

kirkscheper Sep 19, 2018

guidoAI Sep 19, 2018

kirkscheper Sep 19, 2018

kirkscheper Sep 19, 2018

guidoAI Sep 19, 2018

kirkscheper Sep 19, 2018

Gate pose estimator #2330

Gate pose estimator #2330

Conversation

guidoAI commented Sep 17, 2018

gautierhattenberger 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

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

Choose a reason for hiding this comment

guidoAI commented Sep 18, 2018

kirkscheper 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

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