Vision v2 (#50)

* Removing arducam

* Implementing new camera

* refactoring yaml parameters

* matrixes + camera_settings struct (for later cups)

* projection from fisheye to pinhole

* Start over condering arucos sizes

* improving project_corners_pinhole_to_fisheye function

* Simulating image capture

* Hello compute_final_projection function

* refactoring estimate_arucos_poses function

* arucos number definition

* Better model (400x2 pics for this mathematical model)

* Add sharpness parameter to yaml

* Considering the center marker as a special marker

* computing assurancetourix -> map tf from the center marker pose estim

* update black to 21.4b0

* sending estimated static tf directly from assurancetourix constructor

* Not using transformix in assurancetourix anymore

* Cleaning dependencies

* Fixing localisation zero division

* black localisation

* Multiple center markers detection to compute an average tf estimation

* pose estimation with 2 cm precision

* Hello geometrix

* Some geometrical functions

* arucos params managed by geometrix

* assurancetourix yaml update

* Assurancetourix now publishes marker_array of raw detection

* Some useful function for geometrix

* Geometrix know how to assign markers to robots

* Top arucos management for geometrix

* Call geometrix to identify which marker is for which robot

* Fix estimate_initial_camera_pose function

* Geometrix can now compute true enemies positions

* WIP on compute_ally_position

* tested geometrix

* assurancetourix small tweak

* top marker management

* accurate mean_angle

* tweak over enemies detection

* yaml fix + ally_or_enemy function

* clean ally and enemy classification

* improved enemies positioning algorithm

* fix size of arucos enemies that isn't a true 7 centimers

* cleaning public/private global variables

* refactoring geometrix compute_enemy_position function

* geometrix is now capable of predicting enemies position

* Much more accurate initial position guessing of camera

* Some visualization addons

* Last missing geometrical part of geaometrix

* publishing the right marker array

* removing a bunch of logs

* Adding some visualization options in yaml

* fix stamp problem + overall cleaning

* reducing image latency

* refining aruco detection at x:0.5 y:1.9

* Obelix arucos size and id

* trying to get pixel from posiitonn

* picture not to big for bebug

* adding an extra delay over image capture

* tweaking the delay, seems better now

* get_pixel_from_position function, seems to be functionnal

* done and functionnal get_color_from_position

* is_goblet_at_position and reef_goblet_callback function

* side selction and compass_orientation function draft

* New assurancetourix yml

* compass orientation + /compass string topic with "north" or "south"

* auto_exposure_function

* Adjusting huge aruco aruco size

* renaming variable + deleting some tests

* assurancetourix launch pharaon and update /pharaon/deploy topic

* cleaning the node

* Removing parameter_event_callback

* Starting to write the documentation

* More documentation

* assurancetourix tf estimate doc

* documentation on robots poses, geometrix, color finding

* All documentation pretty much written

* Forgot to save conflict changes...

* Pharaon node not launched in simulation mode (only with titan)

Co-authored-by: PhileasL <phileas.lambert@gmail.com>

setup.bash

@@ -52,7 +52,7 @@ if [ $robot = "asterix" ] || [ $robot = "obelix" ]; then
 
 elif [ $robot = "assurancetourix" ]; then
     print_info "Setting up $robot"
-    colcon build --symlink-install --cmake-args " -DMIPI_CAMERA=ON" --packages-select transformix_msgs transformix strategix_msgs strategix lcd_msgs pharaon panoramix assurancetourix localisation && print_success "Built packages for $robot" || print_failure "Packages build failed"
+    colcon build --symlink-install --cmake-args " -DCAMERA=ON" --packages-select transformix_msgs transformix strategix_msgs strategix lcd_msgs pharaon panoramix assurancetourix localisation && print_success "Built packages for $robot" || print_failure "Packages build failed"
 
 elif [ $robot = "simulation" ]; then
     print_info "Setting up simulation environment"

src/assurancetourix/assurancetourix/CMakeLists.txt

@@ -16,15 +16,11 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(sensor_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(geometry_msgs REQUIRED)
-find_package(tf2_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
-find_package(transformix_msgs REQUIRED)
-find_package(transformix REQUIRED)
 find_package(std_srvs REQUIRED)
 
 
@@ -38,15 +34,11 @@ include_directories(
 set(dependencies
   "tf2"
   "rclcpp"
-  "sensor_msgs"
   "visualization_msgs"
   "OpenCV"
   "geometry_msgs"
-  "tf2_msgs"
   "tf2_ros"
   "tf2_geometry_msgs"
-  "transformix_msgs"
-  "transformix"
   "std_srvs"
 )
 
@@ -55,11 +47,11 @@ set(EXEC_NAME "assurancetourix")
 add_executable(${EXEC_NAME}
   src/assurancetourix.cpp
   src/assurancetourix_node.cpp
+  src/geometrix.cpp
 )
 
-option(MIPI_CAMERA "Use MIPI camera" OFF)
+option(CAMERA "Use camera" OFF)
 option(SIMULATION "Simulation mode" OFF)
-option(EXTERN_CAMERA "Use computer camera/webcam" OFF)
 
 if(SIMULATION)
   add_definitions(-DSIMULATION)
@@ -72,13 +64,8 @@ if(SIMULATION)
   target_link_libraries(${EXEC_NAME} ${LIBRARIES})
 endif()
 
-if(MIPI_CAMERA)
-  add_definitions(-DMIPI_CAMERA)
-  target_link_libraries(${EXEC_NAME} -larducam_mipicamera)
-endif()
-
-if(EXTERN_CAMERA)
-  add_definitions(-DEXTERN_CAMERA)
+if(CAMERA)
+  add_definitions(-DCAMERA)
 endif()
 
 if(EXTRALOG)

src/assurancetourix/assurancetourix/doc/formulas/color_finding/second_step_md.md

@@ -0,0 +1,21 @@
+$$
+\begin{pmatrix}x_{pinhole\_model}\\
+y_{pinhole\_model}\\
+k\\
+\end{pmatrix} =
+\begin{pmatrix}f_{x_{pinhole\_model}} & 0 & c_{x_{pinhole\_model}}\\
+0 & f_{y_{pinhole\_model}} & c_{y_{pinhole\_model}}\\
+0 & 0 & 1\\
+\end{pmatrix}
+.
+\begin{pmatrix}1 & 0 & 0 & 0\\
+0 & 1 & 0 & 0\\
+0 & 0 & 1 & 0\\
+\end{pmatrix}
+.
+\begin{pmatrix}pt\_assurancetourix.x \\
+pt\_assurancetourix.y\\
+pt\_assurancetourix.z\\
+1.0 \\
+\end{pmatrix}
+$$

src/assurancetourix/assurancetourix/doc/formulas/color_finding/third_step_md.md

@@ -0,0 +1,8 @@
+$$
+\begin{pmatrix}x_{fisheye\_model}\\
+y_{fisheye\_model}\\
+\end{pmatrix} =
+\begin{pmatrix}\frac{\frac {x_{pinhole\_model}}{k} - c_{x_{new\_k}}}{f_{x_{new\_k}}}\\
+\frac{\frac {y_{pinhole\_model}}{k} - c_{y_{new\_k}}}{f_{y_{new\_k}}}\\
+\end{pmatrix}
+$$

src/assurancetourix/assurancetourix/doc/formulas/projection_original_to_fisheye_corrected_balanced/formulas_md.md

@@ -0,0 +1,8 @@
+$$new_k = \begin{pmatrix}f_x & 0 & c_x\\
+0 & f_y & c_y\\
+0 & 0 & 1\\
+\end{pmatrix}$$
+
+$$\begin{pmatrix} new_{pixel_x}\\
+new_{pixel_y}\end{pmatrix} = \begin{pmatrix}pixel\_to\_project_x.f_x+c_x \\
+pixel\_to\_project_y.f_y+c_y\end{pmatrix}$$