From 38c9e8a577ce59a6bff93f45d2a8000e0d39b789 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Fri, 16 Jun 2017 10:44:35 +0200 Subject: [PATCH 01/79] use 3/4 of camera image for lane detection --- src/fu_line_detection/src/laneDetection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 32265635..0f3f579f 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -185,7 +185,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Mat image = cv_ptr->image.clone(); - Mat cut_image = image(cv::Rect(0,cam_h/2,cam_w,cam_h/2)); + Mat cut_image = image(cv::Rect(0,cam_h * 0.25f,cam_w,cam_h * 0.75f)); Mat remapped_image = ipMapper.remap(cut_image); #ifdef PAINT_OUTPUT From ef3611f08e36f2dd7e0f225985b2207d07a1c653 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Sun, 25 Jun 2017 14:47:51 +0200 Subject: [PATCH 02/79] adjusted camera angle and height --- .catkin_tools/README | 8 ++++---- .gitignore | 1 + src/fu_line_detection/launch/line_detection_fu.launch | 4 ++-- src/fu_line_detection/src/laneDetection.cpp | 3 ++- 4 files changed, 9 insertions(+), 7 deletions(-) diff --git a/.catkin_tools/README b/.catkin_tools/README index 79b274b4..4706f475 100644 --- a/.catkin_tools/README +++ b/.catkin_tools/README @@ -3,10 +3,10 @@ This directory was generated by catkin_tools and it contains persistent configuration information used by the `catkin` command and its sub-commands. -Each subdirectory contains a set of persistent configuration options for -separate "profiles." The default profile is called `default`. If another -profile is desired, it can be described in the `profiles.yaml` file in this -directory. +Each subdirectory of the `profiles` directory contains a set of persistent +configuration options for separate profiles. The default profile is called +`default`. If another profile is desired, it can be described in the +`profiles.yaml` file in this directory. Please see the catkin_tools documentation before editing any files in this directory. Most actions can be performed with the `catkin` command-line diff --git a/.gitignore b/.gitignore index 64679613..4aa27e6f 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ odroid-build/ build/ devel/ logs/ +*~ diff --git a/src/fu_line_detection/launch/line_detection_fu.launch b/src/fu_line_detection/launch/line_detection_fu.launch index 160dccf0..bb74b8c5 100644 --- a/src/fu_line_detection/launch/line_detection_fu.launch +++ b/src/fu_line_detection/launch/line_detection_fu.launch @@ -32,8 +32,8 @@ - - + + diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 0f3f579f..d29b3aa8 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -2,7 +2,7 @@ using namespace std; -//#define PAINT_OUTPUT +#define PAINT_OUTPUT #define PUBLISH_DEBUG_OUTPUT static const uint32_t MY_ROS_QUEUE_SIZE = 1; @@ -186,6 +186,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Mat image = cv_ptr->image.clone(); Mat cut_image = image(cv::Rect(0,cam_h * 0.25f,cam_w,cam_h * 0.75f)); + //Mat cut_image = image(cv::Rect(0,cam_h/2,cam_w,cam_h/2)); Mat remapped_image = ipMapper.remap(cut_image); #ifdef PAINT_OUTPUT From 63f63e075de5445461a5b5df61a845544d7c7baf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Sun, 25 Jun 2017 18:30:53 +0200 Subject: [PATCH 03/79] adjusted ROI frame --- .../launch/line_detection_fu.launch | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/fu_line_detection/launch/line_detection_fu.launch b/src/fu_line_detection/launch/line_detection_fu.launch index bb74b8c5..2b9288f1 100644 --- a/src/fu_line_detection/launch/line_detection_fu.launch +++ b/src/fu_line_detection/launch/line_detection_fu.launch @@ -7,14 +7,14 @@ - - + + - - - - - + + + + + From c9e1b30086ffbfdb57d135db332ac9be306bdbd0 Mon Sep 17 00:00:00 2001 From: lars Date: Thu, 29 Jun 2017 15:16:33 +0200 Subject: [PATCH 04/79] resizeable LanePolynom window --- src/fu_line_detection/src/laneDetection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index d29b3aa8..1c27b276 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -392,7 +392,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) //---------------------- DEBUG OUTPUT LANE POLYNOMIAL ---------------------------------// #ifdef PAINT_OUTPUT - + cv::namedWindow("Lane polynomial", WINDOW_NORMAL); cv::imshow("Lane polynomial", transformedImagePaintableLaneModel); cv::waitKey(1); #endif From 6f4824a32df6eafb607efcb60cf1dc49eca96c37 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Thu, 29 Jun 2017 15:32:35 +0200 Subject: [PATCH 05/79] all debug windows are resizable --- src/fu_line_detection/src/laneDetection.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 1c27b276..f2b3fe0f 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -240,6 +240,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) }*/ + cv::namedWindow("ROI, scanlines and edges", WINDOW_NORMAL); cv::imshow("ROI, scanlines and edges", transformedImagePaintable); cv::waitKey(1); #endif @@ -261,6 +262,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::circle(transformedImagePaintable,markingLoc,1,cv::Scalar(0,255,0),-1); } + cv::namedWindow("Lane Markings", WINDOW_NORMAL); cv::imshow("Lane Markings", transformedImagePaintable); cv::waitKey(1); #endif @@ -317,6 +319,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) #ifdef PAINT_OUTPUT + cv::namedWindow("Grouped Lane Markings", WINDOW_NORMAL); cv::imshow("Grouped Lane Markings", transformedImagePaintable); cv::waitKey(1); #endif @@ -342,6 +345,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) pubRGBImageMsg(transformedImagePaintableRansac, image_publisher_ransac); #ifdef PAINT_OUTPUT + cv::namedWindow("RANSAC results", WINDOW_NORMAL); cv::imshow("RANSAC results", transformedImagePaintableRansac); cv::waitKey(1); #endif From 65c9c15cbcc5666c242c21e0b84ee06cabe5b892 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Sun, 25 Jun 2017 22:40:23 +0200 Subject: [PATCH 06/79] reduced image size after imaper transformation --- .../launch/line_detection_fu.launch | 46 +++++++++---------- src/fu_line_detection/src/laneDetection.cpp | 14 ++++-- 2 files changed, 33 insertions(+), 27 deletions(-) diff --git a/src/fu_line_detection/launch/line_detection_fu.launch b/src/fu_line_detection/launch/line_detection_fu.launch index 2b9288f1..1fdfce68 100644 --- a/src/fu_line_detection/launch/line_detection_fu.launch +++ b/src/fu_line_detection/launch/line_detection_fu.launch @@ -3,21 +3,21 @@ - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + @@ -25,19 +25,19 @@ - - - - - + + + + + - - + + - diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index f2b3fe0f..11252774 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -186,9 +186,15 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Mat image = cv_ptr->image.clone(); Mat cut_image = image(cv::Rect(0,cam_h * 0.25f,cam_w,cam_h * 0.75f)); + + #ifdef PAINT_OUTPUT + cv::imshow("Cut image", cut_image); + cv::waitKey(1); + #endif + //Mat cut_image = image(cv::Rect(0,cam_h/2,cam_w,cam_h/2)); Mat remapped_image = ipMapper.remap(cut_image); - + #ifdef PAINT_OUTPUT cv::imshow("IPmapped image", remapped_image); cv::waitKey(1); @@ -196,7 +202,9 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Mat transformedImage = remapped_image(cv::Rect((cam_w/2)-proj_image_w_half+proj_image_horizontal_offset, proj_y_start,proj_image_w,proj_image_h)).clone(); - +// cv::Mat transformedImage = remapped_image(cv::Rect((cam_w/2)-proj_image_w_half+proj_image_horizontal_offset, +// proj_y_start,proj_image_w,proj_image_h)).clone(); + cv::flip(transformedImage, transformedImage, 0); //cv::imshow("Cut IPmapped image", transformedImage); @@ -246,8 +254,6 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) #endif //---------------------- END DEBUG OUTPUT EDGES ------------------------------// - - //edges -> lane markings vector> laneMarkings = cLaneDetectionFu::extractLaneMarkings(edges); From d5b1adefc0d3098d59863fd180d5e9b17e46359e Mon Sep 17 00:00:00 2001 From: lars Date: Sun, 23 Jul 2017 16:44:35 +0200 Subject: [PATCH 07/79] added function call graph --- src/fu_line_detection/callgraph.graphml | 757 ++++++++++++++++++++++++ src/fu_line_detection/callgraph.pdf | Bin 0 -> 107097 bytes 2 files changed, 757 insertions(+) create mode 100644 src/fu_line_detection/callgraph.graphml create mode 100644 src/fu_line_detection/callgraph.pdf diff --git a/src/fu_line_detection/callgraph.graphml b/src/fu_line_detection/callgraph.graphml new file mode 100644 index 00000000..534879f4 --- /dev/null +++ b/src/fu_line_detection/callgraph.graphml @@ -0,0 +1,757 @@ + + + + + + + + + + + + + + + + + + + + + + + ProcessInput() + + + + + + + + + + + + + + + + pubRGBImageMsg() + + + + + + + + + + + + + + + + pubAngle() + + + + + + + + + + + + + + + + pubGradientAngle() + + + + + + + + + + + + + + + + getScanlines() + + + + + + + + + + + + + + + + scanImage() + + + + + + + + + + + + + + + + extractLaneMarkings() + + + + + + + + + + + + + + + + buildLaneMarkingsList() + + + + + + + + + + + + + + + + horizDistanceToDefaultLine() + + + + + + + + + + + + + + + + horizDistanceToPolynomial() + + + + + + + + + + + + + + + + isInDefaultRoi() + + + + + + + + + + + + + + + + isInPolyRoi() + + + + + + + + + + + + + + + + detectLane() + + + + + + + + + + + + + + + + + ransac() + + + + + + + + + + + + + + + + + ransacInternal() + + + + + + + + + + + + + + + + + polyValid() + + + + + + + + + + + + + + + + + horizDistance() + + + + + + + + + + + + + + + + + createLanePoly() + + + + + + + + + + + + + + + + + gradient() + + + + + + + + + + + + + + + + + intersection() + + + + + + + + + + + + + + + + + nextGradient() + + + + + + + + + + + + + + + + + gradientsSimilar() + + + + + + + + + + + + + + + + + maxProportion() + + + + + + + + + + + + + + + + + config_callback() + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/fu_line_detection/callgraph.pdf b/src/fu_line_detection/callgraph.pdf new file mode 100644 index 0000000000000000000000000000000000000000..1e2e013ff94dce56f89bf4ef59f68e87bb4f5cbb GIT binary patch literal 107097 zcma%?+19$uw%*UVioYU?*nxo9qoSZFB4US#qIA+X>e$&=nS`A{{XN*=%4uI0e}Ad84mya^)LTx9o%nV{ei20 z|DOK7?>PIX4f5~hS;qIDwydpx|F<|ZKmW9e$*kx1uhag0@!yg-`~Jb2rab$ zexE_?{QbaoPsjTF!SfULe{VXu*1uWVF0*VavmztkP5kq2F|nel@y&bmMXZEuh-pJk@J23q5eMpuNJaykhk!U+M3GDYSks@ zmJyrzvt)z(o&KL6fA|0M&Bsy*aL*sB8b}evF4+~n^tmMwymmrI0-GFa@%=y{sF&jR zJ@|E$cf`2xLM00ms4hn;1e$cEgDrB=4-MpEL5- zID}!zX_11NxY^YjmF{KNiH=jWP3GW()*P?LDMT_4k z^{&qAXMVcdawK-6<2@4&H&a;(gBbX9`GixxT-o__=XTV;Ma5CJR~DyKnLwkI9Cdwg z<5n>VRnmRK`uL;*v)roK>-OGDA$=uKg>IM`Uk}L6wzI|a7M9wjROa|(b6dXiEAbo1 z3yrZ(*ojl7FR2~b6Im(JH8WX^nV>yg9xAgBF&~!aP{}Cp`mVm}uZ%l|Emhil08E|e zGkoz3vcZ;|_f)f8Gx6P}n3=1FNuD>(9kiYg)EHqYGUxO6pV;H@%2wH^_LHVbHDEA* z{VZL+aAzcRY!_Hrvv9hh(imKl&#xp)hnt7b`D zwcWUuR`xZ^oK{Er8>yiMI7&Fi?du#5Y?(9E&>^R;XKbazK8Q(ir3*}Obda#_%`t1} zDN<5u9T{zKi6GbOy5)k7ORM|y%{0r7cy5&hY6ucL250YLeUyI(g2i;UU!Z zz3ejRPc7c)R@uj{i<>V{N8WrapZ+o*##wQ~^*7$VJQdw$A>r;=4YNOgY{7!H+I~3BS7FLm5E$2eDrDbJI$@V&EhMRbg}62BAX9fE94`ut<@RugOB=M zb3K63j(HC}eJY&i_mvckEGu|ni^ttuDd7T0SEMq2y4HAv)XqaGyR^=rSNsH*ErnFA z!M9spmvrXctEe}Tbq92{ejIMbM5J#K=g0|4Ra3@2ZivK-?{+ykm4JidPxP?7JwVK! zLO;cTm#4uY7JK192$M4u#861OrNZ?wT4?+pJnm%r_aJ-gnF31DcQbnBA6kR*YU4he zZEGZ8p6b!;mvl#8H4Z)wN@uryrvQ9tjbLUz{LHAg3w?^`chDVe8fv@c_dvP-9SS=#Sv!{{ zeqT|Geu;h_i{U%Fyv!8?zbUej9Z?vH0Pk(IwRGUWew4HKs2yR=VF#(P zBWGPP$}Dby?DjI31-PJV1TEY*!`QtKUAysa?9XoO>(&K6GMAbo7-K7X&49tI=9$*( zloXNLZPK*si7u$jbG56iJ)UgUS$}+)xt_LfZovz7ph>%`7voz;Y3$6p{$isl_kjjaYosNAAlJ5@w9%HCaQ{9Vy{_ekY-U_yyiFc-l3PH&tb~^ zHoI2RS!|W4j+_r?Qem~92x2Elf^q|%$!QXfxSX!W>wTp4=QPsQH{_h6;4i(m zegurZiRYV-Nqxe8iWRSKEK6T-a%+GnV3t3}#|w$eJh1|oo%ZaQity;H;ZOdp8R zRYgg%bz6DUbPj4+0Jomo!Vw#TgUOtdSMOIO0GCWD8qojt0z(%*Nu z9Pt`8(AKEie%=w{J+bQ3Z=11^2urGb+?#8<`oP?dnMV0-eawT@!-)jyZGA^Elc}+i~kbB-0Wi;sCJFYWE4d}<(O{=+O z>Yb5!U*-|}yxG|6KO0_*K1Jwt#pd0rh7#MXE*}!S=DzPywtJiR6@PWuZkyZ|_2{JmL{}KY z_O7gft_|b03nX%l(qUB&7Na9Y^M!Nc!$4q z9CY4~yDN2xivYMZj7p_cHol`msT3lm^$_9HBxEee%=mG3b4Z*nPc z7zs6(n_t-cw7~r{`BllN3+=+%?KbUL0NQc|pPbcJGY}0EL@TJQufsK_ zRJMznD-XV2?a>eV&c{ENxzaz;S3Ni&qob%FNYfhmJek6 zIH*a@F~&IewEJ*$gViSO)p$;n`>p!ZUb8>;X!gjC*70sT*csK1nz7!hm7yf|&S7UB z0m5K#F3!fOi7z-j9Jq+7%CpoS(ercgdCY5md)F7(<~A96wB3gI&eqV4z`)_6=0|si z!_6{)_M4W-FiBST8285n#cM#><$6CT8{c6m z4S&OYSY0sdZua;Fbw2-Ig~1_S7etoZbu1aW+#YILIio4bOUDbR3>seoIj?51Rco?TiIh*q}_tU6ELCx&; zPZ3$UGJ|>Y!ilV3j8xr#oHMTsC+k0z2KoR=y1^ zc>ac#ho@C&00zk?j#Deu3~aPLVf{wby4k5j@AoMzNi+k|(R;!lS|eOKAH+ubQu^Bz zdl-w=OO@W$6BC#8#(Cd>t)yj@$375cto>L>5HnHQ!qUI$?b`tXzm9n5gS~C$5M+6L zl_TM#O%UWzMf&<|5=*?|C}-(OEtU?WTf;-^MPqr)a4$Sy)f+2bm_79_M5r@4Ihaoh z@6{asencrp8@2Pg_?=CS?TXXS))<^L57nw#?hihLUGsXPq@^RH18E0ZtkCS;r$8%K zUPQWX-$0P%iYtgL8kf4y{{XlC<`$O zYvwVn;?K=oKk4kHH0yE)1(R(~_mT3T29TKf4x7yvr4duqzS&98?~7BhNn9~jeoPj{ z`z{gEs1(r2`-|_#D?9tX=k@m~F$*dBv9gmAFr()9(oaU~LBIg|0F+mUIn}zgvc;>F z?eMs$BG}89Av=sGzhYsi>LOnEgWGr582a57x;?+{$f=Ir`RbA!Wa_D%Wn1*d z2VlPKN!rFf=Jk{(OD}|I?c%y8L3Qd^X=!r;`<>ps*m^@ST)+4&@^RPqVNGwB-5)eY z6;X^Hm16z%VOeE>{kC6V>P{Bfi(OjpD0T{wX_!drrFdmZeZQudU670BSNh zcorLzW7VqMtQmZq49+LSnWDpO|7sFPF7;ZE(k7wt+s{cLJwq3j;R=AZYPdQ!c(LCd z4I3I7l;`MVg{O_{LUob>F>dp#{qJz(ifmCm>-EBHIUm+%t1D5xMy@?nTw;c+oN@Hk zGh$#HfNG6FJM5SUc_pSAT-gab$ZKh9CRcFE)PXpXjVU_>ufsH`>Gy629KY1I<{E^c z7bXE2O&9pPXFq0>6?-fcN*J)JI{t>YU4_n5QWy<`uTzYUv#lhAD+Jl|U~Dbrh-%lF>PP0SL_Q^hCH}lv$lP#RDvLpp^ znaPj)zIJtlH5BcZGFlaBTc_*E?p?Wc?j+NoRQE74+np|WoLBcy|HWar0bDJWg;-(H zl(pOCM!HH6;M>?O7>-gcf+crVYE~ZFaZ@%rX&tsmst3Qb@Ww9u7TDCe)un@dIFdd? ze*HbGY#e|@YUQq7ju23994f=*y1R7uCVsR-QeGf0HdeWZw31^O8ued8b2!nC)|s=}-cc2Mz9uYo-_sKW4N9t?Qrz zl9NdyXwjxSE14DQ_;8o-|~d>VE4fKg`&<%@}b4qv!_*V^gtjOF?83P;koQbGt?o~-LLGt&)m(E zItuyY()q*s)TLvyW0!2&YcI!wy)Ha3;1JZxuuxj~6{CaYx{s)Aw@tQ(w7gcczhB&b z%v?>)R+((6$bw%s-dcGFo0qM`ut{{hJ&~tskgI)GAq!ihpb5aW+Z`1+AIDqhxzK_( z82h7y#ZYJu~f z0DZ;rir7>nd0pzf1rm>u)+ z)Q=DKJ|5d@lfxg$C3UogK3)O-qFZ>fD{s8XN7?*>w)W1(MIv*UtfPh4lj?Pbh8l5* z_3)%AT)^5^7rE(K@2_4r8(&tor`Jo`Tzo9)Rl4BN0j^SI8cz`|&lfrUwVoe)hXM>q z^Gb2sI*XhzzsRj=^UC#COF=aJuqipv=EF5l^E^38wgh*DPXmFM)hD&flP47*K<0iO zqm!tb+*`w>xq?b9uEDL1d58I4HrRz8_n!7ea5~Hn%3rB5USLM4m(=ghqS0B8*Q$a; zyG_YDoP(0zUjVmy!RC$f>D0ky=5dEQ-eOp7iNS{ygKEK&w`W1uE@brp6`~ulKY z_PF@dD^Ym(EAb;1WKBo>(jIO2sdV1~b)v#WU}1MoHCA~zK726`>p91*!d-`~T-v8) z52ugivnU2(*-T2`w^c1r>=LhCUnGYRlzMC68OR!&`U7f0MRG*e={jQxvfvO&AR<6hQ6B(V%2eG}^M}=q2 z2kzvAp^`WDL!(`G%N0YYdU_QL;2Cel2L}?qSsE1L>S}Kq<6owBy_645g}68FM6<*v zx>oxq4SH$O*!d1(9Nz%AMiM-nu|B@~Q!M5$kn@YjmV3~N04*z6@REW-* zD{jmF`HL6p?G9fP<|>wIQ}B2gl_9HZgT@O7R*!iyKUCz=ZRA-9Mz7_?V^g+wh`(pr za+kMS z5j;q0z<4J$t%bk}XM5@0n`|fH@A2^0JiU|T&4ujsvbp)Yt-Jp(4%voqkM$@Tvz39((|i1d?jq(+4f6xYQvI9kf-sMI;`(NV=|z2 z7g-`YF7pyPqpirs?8R=q47l+ma}DrSpELroxf`3cW`F72s>w6A7N;?aH6tD@)nC0=TshHF5iZp))Axgq>-Q4hZkS-xI*;qexVCd! z^Y1exS4Pz@3;63)x-a45H=nVk)tT9;Bj8Me2f^&yRD0n4lKx@Sf%{E!R48^ZZ-(Ey zjY9hh>%D&XYcy?i;Z*wNi~p13An8UPv>HsJ`IRm;uKg&1OS2sgaChdTDkyfJ^KE=o zT#XObI2?Au1#2d;g^+ulem@oo|8{O=&EQMF8;ACsmgx|8_5ywH+_m{;^FYo+JH;*P z7%&32HC=?H&fpewN;PTU=)|i2a9*8K<#J$8&C!ZheKhE=tooicd9XN&#cM-Z%=+T> zQmKN^ImVIQv^2_$?rk~;D#ma{OTF@Aht}4_TD^>8<;|B05pj7qc4axa6nED?t?tVB ze%_j#G?yl@19}#(Eqa#mhC2z+bD6z0(3|jqbq?6J<>DZBKE@D_O^7rjxbahY9a_kt zb<6s*Zk;9N@W9rJZ>S)p!DR zG%-Ebb}CR(f~NWr~Ufw04CK)|8jtin2@AIp9Iuv5FknKvxHFJ))%ei{_RI9bd7`ncJd7Y zA=j9SS8~=W7n}R?uy+x<=QwDWZ0<4tEpKzJPgrrop1fz(eJoU}kM(dYT*3QBqR-*m zZAAxUq=MB>E4~A8mwYb^N$b`hhg8#W?>{N`2o!3QiXgV0z6uaAEe*pC^(++Dvx||; zAFzrw==x1q_1`{DB7@B-PxrbD1|6UA2k>lLPS0<0ZHjvqUJB(E&Lu(X(C3Gc`!rdC zIB}y{Z^nHu^y=Do_zl!X38!HzrSH*oY>vwPy4IEZs@FU~H$4KDuvC=E{*07e=7FN- zj|#}!sa*V;)^GnANYhVY#6SC!E|mtOUBW#rw9sp3P_ThNo2U2d1*DnOwK6tD*KawU z0L&0;_jISOYAbhrE0s&Hz5i)75trQHVzTWeqiyltXWQl|I^!(JCwymu#?PvLjP+?g z(K2gE>zVI^(G;8CHW&hT20nEu`E&VQIgR>kg_kS@kK$tI*9*TXR>4q_z0;J*TDiSd zj^)u3O7O(5$w}qQwyPu1_>^-HqGN9nZk|1#03A+W-c>i4JumhK{J6C+Dvcn-7f`L= ziODt(Qs*5bzc&44Z!?ltP`7Yfy-$T!6Q8EKD()Eh5I4WvKvr|?K|WaB+E zkp|XT?|aoti=6MG-m+ONs4Jl4*lOohK#r^Dl=+J14Tvqby|3NF2X!?<7sqp2Fe+R{ z?UY-62W!0;ch;DB$BLcP>m7B;n!lIt{#5o;O?W+`>iLJ9oj_-+9t}2!eyz7DT{r}Zq_Q5rg@}a-tST%=Ap2q=1qP6 zn!Wrzh_y#lxntHleCZaM=<{(w=F}DF%fn(TkiKm4~tU{ z<1KbMo#v)#Wk4SkHtc}>VB}|qrLIyRvQ|FHGby8cpJuuR0lhm3{qd?;^uiyr71}>4 zw~S6uK+K+#s8kisV&(aUu04>y(3ZD7zMxjFSDPgDAgjggLR)u{B@Oz0efpdi9Tx-i z85ld){^Z(DCV|&7W3PdfN1swR#;Z`_eIDPfVreS%E#-Y^4@G4-C_;m{e#F?bC+N}d8kVO{aOW`FN=>%sM*}Esy?>c8u%e$ReY>l zq~GzZL@8ZpOgDD8d@5VR0K(0Z5;RL!&I*QagFu!l9DuW~D7oPgsMgH$zP8(Q~5Wk*gB?x+yO^MJ%=4qVG*cUN?SZxsDxtmuO0Op>2d z^@GPkx0Q@Z%!0}1jhtK=9;fmgiyoFY3u(bp#)(SJ`Oupg^}!BQ9sRVKU9|49D{8D? zbZJ>0kw6^2x?Z;^B{K&E5B29FkeSWbXr@goPs104vUc>1QBtqvZnZ>wvyPExZTH_vLhdc|UGDTy8O1hKfJjk_q{m~k_p8_4P zorPO(JFkH&TesQfR}p;QOKCRR*>wUHJ9}!e6IF{m0RBE~K&77Xo{BmF%_0u19jYv| zDeMk&xFh;iU+Gk!S>N);i-B6e?%d%$n|1Wl0SnEozBXc+A=-rXEXYFIcrd_XoljQy z{P%gAI8`m3+x1(uG-ymlRQTbdXl{MNmbW}E+hkOm=Vj_x%gK`T;5@NoyFHnYw#|A$ z#%4_kJ(x{+z5e)EEjVJAqmB|V z&E5s`UA+{4X1#<+%8v42=4F}7Tdq8vn0JKHB`ZEv=B%+jnA{F=nk;BkM&;j$7>plB z5}Y;WkJrf-Oey)eZ6I))Q&iV)k=&PVtm*d*ZE41Rtp|>*tP8XtWM4cOJ zZO!{(H?M5$i`JIvOkS!|@Y<)% zHv@LEGSJ!hs)#=B#w7m|c8$u>m`e8dz17_Di?G_dZu?WPcKE$DTz(XR$UEkyVPh;!a!|?o?%8^DT4~S|o;Qo$U9?OGtv){C zCA!n^NtaeelsJnH^ZKjQ#?F(bNN2idR9`aXOxY$~T1YQkcRq(iZ-=n5 zvl_fH$Q&^FWV+gi?UBqji>KP+C9#Ho?IYzp z+-LRuvW?%+5F9USX|Te%jgmmna|~pL=uGyl>zv+mZ@A#6)>SG(NzzZq1|~qe`)QwZ zT#7f?Fttq|4QI7`!t=e^_sD9mn^%|zSt_?R)9ghz+A1soDjxKH`Pd58<%gx&T0|BW zJ>jD0HqEs@wwz-VdA?wB4^-+NPWt&|@m0d=IiKWQr3=?i*`Y57d68FhWm&NKHBi)J zr0t+Fv}cmTTNpLp*k(ZzBd3PN;T5Z!M_^PSgqETF8mf{9_=Zm=91?{Rioicqp zpF?x34Q|GE*g*#*xWx*Pt=wX9FzdePT*hvWHCG>NMCVY(Sgc*pA~ zAae0Et!x`e4po{Q7Ss4dG4+?N%$QN{&;;ZhU=^pO(fRRA4OpxzPa+;kQ|T)U{!kfg z$F;gv7`;m|{Yntyv5MPJVLte^=J($)TqsPuZ&SoIU1B+BW~WaI;FWD2)|XNWU1!y| zKQF#N<@+_keMEcg+Ch6?<2-*MbE|^J*Q?Td1?OphEI|&vV9EFSC2a}wSS+=x-)F1<~E>)YWCRxncirVZ8 z)v)clVI6wzmG92-qQFC8U^iiH=VEOl=kgFDl{)S_rhr{DIk0NPH}V^;((1Y?YTMoM z_5llnda(qZhv`Ne3Qt~rXuU-vn}5*owt3uX4m5$*@CyBKnBMNa1quyFML#Kp!uf<# zAIHOf;cX01Qr+bHp)q?)wpfwfk zWE4DALX8BVVUwco430u?VmC7j^E6Hpiprru2LV6VA9g>;Iy90 z`z5A!stp^8QYZ`g9H?E7(nwgcB;egH!}PeQt*Y|WTsiegG->EpBak@%Ty}LV)r-^f z_h!gRdqXZ-hfVJ>dfaye{*tP4dSWVk_0pzD5qVyy`EsoT1ZrND80>Fp*SH19{_IQ^ z{B!6FO(x<)b}jTn(fhiXeX5j7!MT#k$5yi3Qdic}?;^g!4im|L>v^KwvV#Vn*Ls;y zuU0_c?bAtjHL+VxH003LSQ&@!`PLrE1X%YRmYaT`h4Xv9DX+;Bd0=5h4Z419p&!{_ zqbH=scrTvK%Ki)#!bkbgEr9X%WTX0$>USs0%e6$v|V%Z~e)Tz34VNYG2a=_-XcQ<_Jp_g(PltHgLZOW8nqqNsT3L_e}EAi zi|asA`mAY_$juc|wTHT<<-9*Uyy)%H_t45stLAYE5yJ0Of)`m+9Z#;69Dn9-&03vO zKXpPEiQAI7Q;e<3=G7%N;N-*NN~!f%{i)GnmKW|;V2d^LaoRo?dwW1t-?-Gi7>+G7 zhu6ViK_osVbS{!HZ!a8?Q zTgGdze2>6wpg>sIwDe25xbVcbyr*IxOnT+od9|wLf*%PhcE|=OJt7ym+v8KES&O(1 zf?8wG5hLR=8-552EN2X3#!1ZtkTQYD-bp`{gQF>#)m8<`faxqemkygX^O%<UB`h=!3#T`OMnq0k$A!sqQ0+0DOPsXP?|I_3_eAs|?)+hL>fd%1 zy~a3(*;bq7c<+|>eO$CI14}tE%`}w+Z4OMOM762aaI)W2i>xLVE9UsHq@Fo48GFCw zU%u)f9ez&vN`N~rji+z*jG{SDMI?4Qrs)r8f;PQnAgSCMovh*$=w*lEQmUe6&f z1TTblcci8r!q;mh5MJDKCLL^PId#B1?1vjgJ-I;0S?M-C7Dv#!un3~Mz_d=>mmgKy z&o6L;Pf>B^jgz=QqvKT9VfI)he#h36HPG_)m@dK-OkWXBedL*se%U_s#* zz%WHE#gYf+EieYmdR@SQhhJ0=-V9yZwS}F%S &Re?!uez2Cp##|!cC{dwZ5!d@ z$nX&FD91M`fMuI|Jq9nt_|(2wt&kg4!@|WKlmLAiLl~yc;C%9b;dg`W=*O}eP_TL0 z$6o1?IE2vu%(-bFvij%JooZBW;>Xo|^K|chCGsrRo5K{Gi=FXZSh+{w8LOAZV74#)Rd$u89k~4|`_Q^y+i_D(`c#KC4Uz zIljkyXyEysKof;fLoeq$N{R!84wH3!a7#SlTh3RG-~6g?ZB=1lLy*(bm>V(pAO>$TTwE2VPP`4$6>5X@)pN#(&?F|+8N3aw_Q zqT|wj31;V6e>DJ=VdWm6wq9>nnw>meOD_J>qtW*ckh6;G{4eT?Q2<6Cg(bCx0mcnvwg1HeQmuF*z`xe z3o~tOcN;;ofq|bMxE|OjMZUSWhxoCjYfya#FBNtC+pfi<)KXX>D4i~%Dg{%xA~{jz zR=BqL>2WYX!nHWM{4^-btvoF1nK?^LC>M_y(+er*92Rxsn2~GurB-?D(*d;?2ixOm zPFp%-5*kQDQ5v{Bg4%$~<`bmcmu-8X9IITYh9RoV}IYl;E z(XYm3mpT0=JgpnQl%>JvW2$zMb2s68($$_JiK)SRtxbPi%DD(ewS&CxOQF?kC)XSo zY{7}l^LleotrN&B47&DmkeZ zu~r1>S9Kl{F&5%&p?jbQjFXj?+Gs8yi!d5sH z15-d3l51S7TDQvBh=#Wdei!6LMS*9Hk+~S8M0bCzZ^KR&*5Fl14fB;_#1*!^YR8x2 zhV$I+=%gS+$&kH$>degvnzt|-8MH0dcXWx;sk8KuLa5|TQ}wwwS-?`viKGKIAqO-@05Hga)T*X#Yf$eX|~kGpn2Kvh3;M3te! zqJQYOl|`^_%^YSqko(u$oyItEJI7QnRR+lmB`b*3Es~9WYO?!L4JmPwW=Mzoq{gjZ zw`TL30|0=B{sw*G30B8BLb%+?zc_g|-u+p|RpXJeI#;$V@sxB!F>a+{?(8k?eq*W@ z*pKDMX`#uH)PVLVxcp=W|Hw{(F0@U~bg3L4I@N`Ja=YZ=Z50^fO`x}a_QB52&0>n< za1JGn@9zBWb1iNp)~7$T!|Kn`^{V8w2`D@#68v!e7Y2Z7=V5K}v5F7rL#LJ-9B$Hj z)kWhN|BeGEfb^f$?SC_2w2C*?bQd)mNTgIvJujvC(Pgv9DK}1vE)Z7gH}={D5b^~M ztY7$bPv&kWH?Za7$xPO{RWtM%oYU^+?GBr(nA{dh^o(h2F87h-JS@)iHdQ(>GuMR+ z;^SGfE>jx%1S^2o1l34d_rSbOs9I)e_`SdsZnpzNBMs+O!=;O{mv5Te820$K`{;1> zn3`+n<9;<7RsFfU9DKH9Wu8{I`d$AOftY1jc+5;dnqaz(|T^X1sikJJbc;JUFRKm-=&y)LCg2mi<}xC4NqVH$$@(U zm<7k~`aJbVq;Vo9aeE8mGn_###P&bfcDnC$)=kM{JnGy5 zr^T==?VTc_JFDS>Nc_c-AfnxEc}%XW{Rhi}hn~fspAQRB5pEAq9a!R9cXzEns2eFD zU&z=`w>Jad=T79y z!0VsdvD=Z_X5sQcb&JPu-J%V!%#)NhoXaH{Nua}w2C4v#- zc^>z2jlCpKIM^sB_fk5Tw!FWDn{NcOYwUv3>pQKKoo6~+F_9|_nQi5RSLPIuCS=*$ zrtkg&T)Me#(&j$DNqR`~rGQG3TxylVw7q%J)2M5tC{rKj``tr zn>sP>)fsLRznh6Nhi3cjyZdWThJFdffl0O|U){o_Iiro z2HuatRalzJ45eZ5yXhdzJ{q=9Hw3z59S=>)&DNxrR}QZ>zNoCcOU0|7a(>>?qT$vQ zpGRO<+MyTp^PFFbGifYg*WQs|+pc|ENvo1yt(BDC^fIsOvMpZMZ!>5#F>OtE&&WhI z=*H@S{%@krqfJpLjlz5Wg;@*;3W@|3#ee}pL~>FT5W!Ub|G7Q0m`y*u=&tT6x!f5b;+|F}iy36>uFttHF}tfTRIkmR8wO3Qm-r04zy z8@lH1Xe2cl{yk=PXOt^BNGpO9rEHR`sEkEWulZeTFg20x2~j`iPf!rjZs7--v-7;d zc|GrbR!{n3f8{qgvoe-UeHS5#x*^UGI~T&yEr3fk|4_E}gV z;jYnYW`~ZvqXwa7mQg1?=r>43!V`LVi3n5RW^?u1J$8O(^?3X!@`86UhMIR)zY=ot zo3A%A8IN%b_xeZhnUK>v(m%}BXb!iM4JgD4PKz;Y$RMv7IcF({`NiMFishu93qv zyvCK+de~Kgmu>RjKhjPwDPmlZ_FZBNTkx3;sw_VzQ$9Xj8rNe}5x?FsxQE7Zi)ojZ zdQvX}QUg}m5)gur{bbV#^-cM{G-gYoAN79nz4BqpUv1JYJiC}rzOu4vKdY15q)_XG zsEo1W-ZSrMN_%~pU>&pH{sQ93HIN~F!T-I5ncG$xi-XZ0edSm63{(>ez6@@duoX5g zqIVsq>Uf{pyVEZ3R)OjMJzAQc-P}i>U&L$mJ*v+uBW`1&9F&V^?)RK5+m%x+!1B8h zcL%k>6HoJx0;b!M$TE(yW-ffC7W<^j__2d0vuO8Fy=>ImHTX)U6$GAN47)b-hHa=n z5VkKMZT=vSJMe^7E!-^CsQ%rs_Qz_68Ef1;#-piU&N^llU9MG7eXY&?s`j#+j3~d~ z`fg|((irTa!MuEThS2VnOoM}C^K@T}+-_&2bd&;+YO2U;qR!rf5i+c~+tw^nN5rVG zc7B69wYhl}b7l07$LXWg^8dWHvTa&N*GV&|KrHd!J?|6>nNi^=qn6+kCvJ$V7ZG#} zCIarCE+>WTln#*DrP~nN)TZ&MGrxqrbuyNd9yRn{dXOt|eD^M11R2L<1?;?9a*Jss z($Uh(AK#~hTPQv(!7)c9hF-OuqY19m;d90@$TPbnR)-oTrynfJ$NZ@*JMY1w57!2| zK7i&qb?Mc1gWv=#ZPkB+)ZDq4cJ-1NBdzFJH-p4##jlEG+uaVUDErI(o1EpDaQ^KR zRu!s^9tuy|DNYx2){0Qb_m*K*x9YfTAOGI+u!MW!xA_!vxN-aynaFZjQX?%ZovyrE z$ND3t(#@hv9U3c4D^Be@bfb1>YuQydZTgRe2pR3=&D`D#Vi4)N)m;{mmT(H&bEM)D zM|p)e!0T&ykb*kR&S`O-wLKE6@Cua-c6q$yiII>gOP{Q_s#mz``8{YjRP#SfZ!^F9 z_uIY~k+=iK|9y#U2^YbRuWIye!{)6rsy^R5mE`=r9sJqUo3DQ#?AWOwg>WqKVvAFzH~Dp?VI|eO z1lW#x1CM+(#k|(`rrx}4fbNBbx}M`fc^)GWDt#Z5MNiv$A?@AI!6tlrIg z_14Sna~v23mq7<}L?7tMwACur#BSrcu|s%?ATNq<9=JneK&Tvx_kzaKUHrA=z=w$@ z*YsCr)4DF1uKOpZ0MQX)WJ3aPy>i5nr^t&rX=_CcJX)eZndoJNO^@n0{~1bp4VPI_x&x`FC}sE zZAA|DVbO@5xx-GWeTp7n^H^}h1l)rPRTzausyd8|(*Q>1y-s`kR}i@e7Cnh`OvpT^ z8HUs6E6GX}tM>77Ko5KcW|Pk8d9jMUl32N~PCMxElt1@$Do$7VYG93aVYvuqp4zdc z`*vzBG28;L_;z7)YXW4C;&g@>eE(a+`e7ZOt!86DmgKPSphT~$DK|jdP=sP_u56j>S9s3Y++H#= zsa#gwzwasjsFqh~dTmx>wdFkDSZnA_R>?|72K$a;j7BlAq2?VRaXz z(=y5RcJ0{#{H|GivOtHqO3QbbmVnV!>;_|1rh=of1^EE3@?g189Zm7s_plm zeenh=Anvmbc(@9)FYg_!YB~6&u0Hc`3bqvF#9iBWpPnAE5K5bZq=Rq=kzK!H!$CCZb5HD6l%;8~4<|FVMf=93P+jr>1|VP8}Iiqbo(b zEophYJoC>Zcu6V@=rJ$s6O+vfipXr?1mwySUfZHsew7Ob4!VXq+#5$dc>&y$#%NU| z9Oh%&cOw~&^G``CIK8yTf7>0tRa?v{3V?9BGR&`v14xa%m6-n;S6h^b z*3H4@N8;57`X;z_NDqemhZbAHJY2;KM;!ULkHZ0pwsv$M%x*pVGsW48Fx3lV3LIQaV?ci}te9Y<||AnLH;(go))V=b-k~PoXr# zkk}M?uwy}-6Gacyiybcx+7fEnq^upS{-xF>U6-sEGFa^_+7sy|L)Oq5tlH&IpY>Pbwbds(!&@QF zXfOIZv=m__&xjK^b?9)IPOdFaGKSTP%kNIe_OJZxwiP|9$2pnr@d}I7q>b^aTS0|*6{33Rl6KTQF+4dvZb@`UupKUF?jWiZ6gLcnSZKVRZ)a4j5{@_<^ zd2FE>PEH>`f$k~WurQzyJGK52i#eC+lNM2F?G4xO3pr&`aMS$xM-_b~FJavLwi71b zWW>|IkinffKkj>^fF8HE_NwD8jzs}FwWgOV;oYYBZuPCb7d8B0PbIx8y~L%MUi1o| zIZY8yxxgE27Ew&S&{FfYegf0JPvOt*;CQmWLvB`UcN5}WJwXZ+xHopt7f{0j|4}f6o_&UZX0ZzeSV#0h_^!;r7c73<41qg?Mk^5q%3rvu zVr4FE@+|WvhU@)Vi!6g@6_VPMm0JS<5bQa0VBF&KWp{Np!KGz+y2xiftz5R>rnU<` z7O@G|jS83rw_)H1_)K1rvvzu|GIQzjdg06w)y-dMdwMe2v1fgrLl;Mohz^drS5r;v zVDJC_NNcyz`Dx$+mJOh11y}zsP+yBz_m&+=uZ25PS~2)et=joJeZhF=2-tDnj6UPd zId+7I+cga2e0+Kf&W>!$ql#;ee-C$_MD$M?w6}-O=#jrqdDno2i7@Hm*0=iCG0LM? zc{I--ujaT+EI3j4k#Ka_w;<`Ae)qWnyv?1GJNr9z+W})TF8_#D5-^m1!mmE~voSudWC@h>k4)q+zqp>hHK34(i2io)RjhhN&KOP+38 zce~TrJb;*)>r|>Rat30aYKLF)78nX7RlbnU8cI-W-yaOM86teAB`oGHhVfUZv>cC6 z=4W?nE`9y>jti2Jl$jKlMqdOB*JlG`TAE>8w|OIAHa9e(V8E)$u;KI^Ed0`AZhh<` zr%spCOS}6$*oV>Z3ClAmuQaSdfKN)rJMTTW$C4VRb1=4$ST|4Kbue93_Pi12J z@Ua#^{!#8f>YeSJ74ehn?mbFt%ikYlIiX%Hsrb@bU!@Y|lk!YQslXQu_wXsdH@ngr zo}hZ8QsL|$F61k9t$mEY0}R{1mt=L2Wyg*rk+W~Yf%vpW&~1n%mf1|yMWm#eUq4qC zd9Pd|!X^_i)r1l>a2!xBcp9lZ)Ewo(ayb$?pP-fiZ;>xmgBAGxn;Nlv?v~`-f{^E- z2S(saZ7sEKNg?Ws3+FQ5XM3({4dBb)-YT){Pjt;DuK^fschoUpwghE`1;e&_4)d8@ zZ&Yf*ckFI6H#}9Lpd}Bx_Xt$kb%0p3tUz_m15(h^Vm^7OtIe@@#n*7xD+Ng%mDOJ| zYL}WL26pH1SlnLy39^Z9gF!wZ8^#_PG@!XYuC#7vo%lP6jY~G1B%Po>B+5wclyIUGg*E5JmT?bRqZ9+ET$M z*MFxv22F5+C1LB-Z2!_Tm3LY(<=;o}4q4Gu*1)9u9}Bdb<+CUzt*m_ zA43Yq_qfZbJJk+zxZX1dSybAg2cm@jn3qgy(+!C(GIK^4+tls~{+ST!CuD4mF2ZK; z+3P*MnxWH;wfg|H(eIN(y*j36%Wb}&UyFb+m^9N^FLDSHj#M|MXX0E3F525Ak1pG| z>wSNNA!7zNB|LZOh_3_PK;3KmQ#?MBU3$b4xaK*np&a0bQ)coa{XwnD@9d|y{lolmb2^13bF!jvYu zpd&s%fU+dLmugLwA0kcSu;^5B$N<77BEYuXhS90sGWUn(W^xh@w6!U@s|no!yIorNCelaZ8tccR@_LUJ$*D3ZXEiNo(9-MK zb`B9%?2=1k^`v8{iIog0-|g?SXYJd1tPDS)l^&Y`p$w=u_9H_=g32MTufBa?=&X-t zH38PPqHoO{fcNMZfrds>doQA1ypFN?#Ds)XMt8$+OM96xfpiWv32)#0tY2Mir@3^0 z+i?rnUd}?dKOt51@7=bwgvBpx`LzK8%f{~%eq8{HO0T@a)=(`6Cq;hW*Xr~_n(H6; zwHxwr8SiT+P-cbz$?*0g&y!zU-_u68DavR#?1*LuPWD9an+^!@D3o?SpM}Wo)3y3B zpvTw~7p_PnN&q(ER9=p(_tZ@)2^+4C{NnoFchWJDx3T+kG%u z!qKE#R};InKV5+Xd&7Src5b1@!sfzcbBYrtQ>s8D4YZZL(_$?`H0lP_dUPS3x4DO&AIW3Wf5iq+=Ayj_a3 zfBzEKNNW+5g{3PGhCoMWNFHv;?z<_Ju4R2vS%MktpHK!?d)(;QB!8mH1IT`@S5EYD z!6h*HZ81O#7%CmnRdzyF)Fz)lq$sVr%K^LUQXx+j6=xuffnHW7-hVTsBNhkKsxaU5 z+`&^i!~K^6P4dMwyQ{t)$=|dzymi-up>Hl*4BS&X6C2l~xKlG0T>ze9bEl>zz=o1# z#ZZxcO@wZ>>V5mnqvU8r5U_AvWRS1}(^8kkXPf;1{y0hA8!?3KYt%=UCKzF zkzd{TdMUt_-dtMsp%cnPLSNyk<`-=|VxIkWIWm9$P5j}ta;%#PR;p{SLLP%odM+dR zY83*l=$TYnHHE^z=D^ai4*KazP2{#Ry%vRbZQfS1A(QfrSO4b5*HQI81#79Ixy7FJ zr9nYY-kyCOqnDj~{&%+{T3<7~qHi|@V`l4QrB<)9j_dnI|0S2^rc{TEMA|#@x`iRd+Y(->wC{d$aE5o`zrWSn{K8zcQKBjVIA& zsAKg52%jN)4MwE-T@0A+qg9nF2v*d~_d!44Xb7uN50 zF!iDX)0JJE`b=R8@Mg2atn#R>Sq$6s7~s@I{@7mT_ovTBV^h^(iF^t8=6KzWSk;h` zWrbk`dJukm=xEioakU&MC-{9%hi~U_xt0$AQnU94;^=*n3j#8*vLzp|P1@P{BB@t_y3cVY?Pwi=J?$0iC8Fu}G{CGD0&m?f8GdOIIn8-LolzI%I!P03Z` zj0Wta-(OKRY~5I}8{i%|0194+f94T@eTcuYX{^dAM;L{5dk#fuqG^}y{q=cr>EXS6 zzdwKraa4_e+H`!uA{AW@yLIaS1gzrSng4y;TL(Lzt7%&6Hxu@iOHzS)mRDcsQE4m5 zzOapJ>&s%*-$lUhC8R15wEpB~sy@zvRK!2~${6bv$Bmy7Yr$@L^!KXv2&c|2d>pS! zpdCMqqkTAAibJUB5k8z-z#CLYUWq=rtMIMetg_XHAPr}E|22k1l~I<9y;)<{9uu*{ zwo5b$HJ3;_n;L@p7vOpV{a1jjkyQ#%d;BCH(l-%J{GO1pL*~|tBsK4~Q8nSs^U~>E zt|@%U=p|mrkH-zWZZW$pA1Fg+P2R@IMt&ia4)V}`tA0Ojn^1#o`*o%m41Qz&pMpb9 z!r5Y^QYD{14$IS&P+kRY`rOZTVg}SJwVYDZ_2~Mx2x5qCvs=U!J5pJ}oZb}ZCEynX zI`U6~O+*fcyYGKgzma|bz7*atG{B+QG#}43mfV*6wHfi}_4VN?$&l=rZ{Z#Z`y9;` zAV~5}FcLa(Q7ly`=vZp@DtY@m&sz@IC)6YVxdUHgZRZGL)H^L21ICcYYoK=Xb^Q1< zfrzg%LlxkX2k;u>8Vi>w^*8AIdlD=CaWaSY`@Ss;uh&uvkR9E*G4)s|F*QMQ?&iQD24YbuzKlghWxd7-rImoj5BR z%(v9$@&#D_jTi8+L*m>`){s>XTpto}b8QC0dud0ELOY!qvhju&e!W4`S7EL7XToxJ z)m4bF-Re^JUobct!h_#eg41P|%Q;y0Pz%cbw-_~~!J;~`Z<5%( zf7oDyO=m^7@>hyW*8~)FQ2yQ&H)jq|`0i-?;#}Q!GIGoN4FM3p-Mq!Uj$Llkn{|3y z{=76s^64RP#aml)wictg&2%8ns}1&|mdh-_TfHjaIrM;`VrzhdGC%!2)K_~Sa=nU0 zFaD61JNRsHtyDOSOZ)Fdrq2F>UR2>xEHeJJ?_e!M**=vP_fG83@kv? zNNU@E-w8TocQfO5fC=XWNU77oM_zy9IypW!i(P&^=>Wuj`qeDTC{y00SSCA8+1tO# z=z_eqJ(&?0h#l@Ve$Um|CTB$N;fL3oPB#l5jsz%;jYAbI3^02^Z7zd1U4CRQoGrDO za&bJMw$s7L1e2iKZgn)P_mVzW7aIo>(~PfOhi`GbI4jRD^7rMx4PQs3%xt%%OT9|j zbbysTejlpZXekU|SI*G@OG(c(|I@AtfN96%bv>#8{PI#OKyfOTZu`;G6Yhs{zLCK) zrG!3Ln|=C1B=a_$F~f3ON{Z1??%tZU{WV*lO{zo<2Lp?KCDa4e6Cw|C|s-HiBoxQ0ACv2VdEI zcn>K#ngn`U{Ki=s!Un5YH7|{u6NR(;d**E%YuPV6ar7}{2b9H37ha;+KMcHT{uJ2i zesV_N(c2#){Z*D%@M+#3H%I57yXz$R09V5&&vu*7MQd zZY4tR?X(9B1IOXu#U85Qg;G`al@p%R6A3&e5{gixv%9LEAw-e^I~hRBDC*JX3eIyIkzgo`>9yXDNlpd>lb>GKy1@ETrP%koE&#ru_72ecT3hzga)g?Dg*^E zz`!T6&=oJ8ElL<-OIfDZ6=O7!8#Zgpd;J5US;!mwJO?#?ceBy?d}-!YBxS>j`@y( zfjxSks(s^xzLf!b3v%s{IkS8fd$@vgF!$!6Yv0ED^Vj=cdTEQpn;uDlbQa6N1j#QZ z=oyJB)#b1CZC!Wmk}MuqcD1L`E8w_X*VCltCgl6?`ddW#>mdhc84mk+Q-5 zS)Fll4b30f6&NoAj-2nqCAov2rQUAPys7M!OqTGGXlfy+iQXt%9=v9tub8pUsjsp* z^b)P#6u(_pu!g}2$VQAd`bR=*e`kUO6>IZk15*x{?MN>P>Y-)nyJ@y#Ylx-Y6?w5&k`jFQC%SfyAia<(K#H)$u}|RAhMf+Pb%< zRv=FaV2IX^V2nQi$N}jg>`$bG|K!brtxm-FB@Bn`i@=reC!$D-#A*7BR)-tZBo9>~ zluw((VRe+{Xg4v;!#K${_V{22L{C}~t{;-ad)h6@)%Mu@3%~Sfe;IE2TKE(Mtw$nk z7a-uY)*Za~4XBRhKR|#X;^|mhNSSnki;ZOPtE@q!iPy;5r@qn>tQgHs>l7JnhkcFU zfatMDF1hYg-!Y7Y;*DS(!`jA()N>6YCzon}6?sRWs1GLRc01o)z3yUzD3Z7aYq9ZA zS$LP*GC9PLUPm?M`dq47G~$^*e>5#3k6jkap>WMk>9n0>1ql4qOhgXj-Kjn}mnvtn zt%09Wz%={}`@Y*@K7MpmB%Sx$<)XYIP~I?z|<5gUFMuJYH`bqqsE$ zT~j+6-o-p!PonkY`i9aave*=jY9H%JwQqxO*a;0n3znOcO65)?GOi@Q`~d>Heg8KP zJF)UCoYyAwpVF1~AmgBmodAj-ew)5{M?YyFg8e=p_kDf1qlT;TeEvuDs>;d^(_lJZ z{i!`5^fkY$YfYrcX@{zOuh9t@Ms7Go2Zp&%Q8!Px$L-j0b%Fod_zvO+p(?3t-V@3l?uYf>TzF{Sr^AEs zsj-s~dd`nP*ti?dF8XO2`tE1Ewx3^ptI${M>(?e!nE>lj;0Ni4%x}rQ&+Bk~RdIUw z?8FlQlmf;hdq`pCxKgKV=jcFX|I3}5^>=Q_9xbGpFkNd)qflvl_$KdU#q65L!9ZR& zv1_q#3aX`j|F|FS&Tw4BPMk-<0g@FY1<#CBUgf7@+?9^JlBykX^b z?VhUh%jvq}Z{8vVL%E>Rc^s~vr&P1mm8t@*_!+NA0RRTO-N&TZRO!vRQ;|2sV=^|9 zk1paS>Vor7y+Jl&Rqtw>nGWV>0k)V|0bVOd;h_h;@lT%idIOxpvPviOGtLLBb zhUg|t8-ASkN10H`H?DaLz zt^w=U`cQzGi$XDkufvcy$u|)|rTpFIeQklDCChASPl@Jrna$)mSS{LnuG@Yf>++%X z9{X5NdfD>2#eTNq?sbq-i;C-tVQ5i7{}&yJXrt)ffOcXxH?y?(c+ zf@&D($y(Rs0Fl>{0>9|b?#0wz#puxZYd zi>)_{yK}8xUnAwv0+s$fqEG}kq-!wyl#0*n{wtl$O=*5)a~$gZnzI`SwAgnB-NLDm z9t-55()bP(`U5~7K;>oB10T3i-^YIc+HAg<&g9Q?Xm{Pen@S_7{#4lg5R2*alNGjS zON92nmbkpQ(dGF*kLbVr{;;ZxgxpvFy2*ElZgRafg8neOs=I3H@|_Qm5gjn&?r=r$ zT~}m`vi};@uA7lH*p5{n+R(_R()+7E7gO}EF!N=36}Z5XI9(SNf0y0z&3WIr*lXuL z^A&_!7T^PhUK)*w>VZM$)hmF(*nl_G-A`EZ<5mbchi2=!pF9eS0_8hccu?~buL4iU z#|3~@udG^Kb^6M>7C@iB`4#?Lt;;z%yp#_!;P9|S@2zqG6;U-e1F2Rd%8*o)+ymX3 z&MRT?@!u=Gb(&&teXXk8`pr*E2tx$)RZ=ZSuRKP#O3TGRba*`#&!Abp{DGGFDa`G- z-!ds?4Z^da-J@8Pjc0lHN67mrmpr^>4Wv1N;TBTuD<}4=T%ta}IRG_h@5i+0HtCzh z`S4#}64A=QQKdi?B^U20(AA`81sck|*~fZd>j)_fG61DagY0zQ!;;WZB<7tew?#vQ zfd{H2k@GEF>V0qWEVjKw+-atZJ}-vUVCCi@ih;&~4s zc%LQtBk|hnHKf4a38Pzn?fmdu7=z_O7+ZH$3>v6Wap3ZAqfXY%6G+zU7>QaY9*_WV zU!T60fQ5Q(Umx|+iu_7$CqRhg@Fljdbj?F|+S&V)enhwCM`gOLF-XZ*5BPIa+-Y5s zH$V5bOZ7#(B-RlU^4r5s`P(YKA99@5=i_KdD6W3oU&u=ml_uxr8^OL`*@&s(m}ziC$l|^VJy5f{dw$~KZZ++F z%G1KMT-AiWik@LW+y4A~_F63c4)Z2*_}~pSQEDzD;%BvxOswPUJKsLWTI{l)A79H? z^B{-t!=M)HZ$uv=RQW0#9@7up>d0l!J{EgPB)u?@Z9<%@t>x@;9DgL3Yh(4ZbYn`S zhG?a|Qw`k82cYi=66R;!8T8rQ*+0IQgB3_0OaRpdCaAfj``bsuO$HOQL+YhiK2Fgq zfL;P!V<6-u8TN$1EE@;g-;*bP<$3Izy>0xW?X3irU&%I6rh0u$G225hCrM+w+sh}d zmO5wj1(fcTRzel2_4WCL^1!lZe!bobfjPtP;JK|0j0PUet#)-NP>CAR(91LU*<}T; zDjb|-XSV9iw}hDPbY?;#)VV5gAMLsz4gh%lDr3Df_jHwo5DVu*cp*!+DR=&|veG~Z z$0Wq%6NbFb6&4#E7q543Ajpfkugo|q*2Pym?5|J!eg}U9ZT})Tqu`hM!g94~z^^mw zULH!jqSRNFX?YYaqKXp1b5tNMkZygvZ{)VD@6o|{)G2*h0}w4m*YCHll~)u^>j4+$ zY#IaNz#yhp)s=E6m~X{UpUIr$Jiu~Ks(!YLzko0dvM_)wp+THiUKe~UWDHE&qoxc~ zr{c{ojne1{5G-%_k%KY&Fk9gGK(S$;AjaifYffEiU!H^q-~uea-m$eI8GHf4H_dEX zE=<4;-UpOes%IO)5ZY)(Y)odTB~!qKWS50sHUDkz!%3~2 z+oek>y9+r{=`^n0hq;^&YytS>0P66aff{w#r`fGVGL5=Nfs0O{e=7_yL<-YKp#PZ| zxep-l_c9~Zp5=vfq&AcCm7z2GBdHN+hF@ExA#DoFZ|e{2PS-Txxx0_%jB!m4A0sg4 z?DTPW*`1+o#fAMD7Ay}lDDIuMslF#`w@*JA&PQbhEsnW(Xd_;8MU8Oge5B4~kVajE zCNZ-s8|gXeDP6MqrB3LMrY_DR77jf1__ATKl0clrhgurNRj$5%rBe|5INXC0l}_X{ z(!7*|ehK9%b94)nf)T^DUOqd))KVqcNw0#nkCw2EK=(j-ljAGKM$I;Kzb?xt-vZE@ zYRlJ2vQ#nutV^+xQn8A!dY#J=X@E8Ons4|!*|fLK2RAgq#7>@OfD)*vK%~3499+1u zc=kG4m4dWbX^`xKSlpkJ>-ET>rRh|&$NkQyDNBXyvguwW6fA;{&-?!K1k{#(`J9&J z(LO$YmK)0Rfw1Vc7Fy2g;e4G63wb+KuSC?geeV`QUB69hP7_-+^HL55w_UwCE7Fm- zef-@DjofaHE9$-8D2=h#oux+WkG==BNa=j=dX3+E-VmrIgzBxE+XQUV;Uu35rJNYK z{!y+A0WlwYNvYnOMUPTHob?j_KFVDE6+ZxDkm%!O6t9k9Ns{W z9XxMfZZNqEHaF40n&fp9Xc>f&iqHb%<7!p17kiPH|X`N!r&AGqn+hkm1*hd4p4e^-((U0 z1zzU%F0f#os4jqYx8@%6;MVI4`yoU~jWCnud^+qY9I*}~sfgHP_C>~y*uX2sHtt~~)fY*9OYLEulv z{FO>@`V0{dq}{ZkG(Zj@Bm>a$Ydk5T2V?kQXeXiuI`(uoA+ysT+!Cx-?L}$1|M>9j zfd`Tdtj)V^v(s5VCW+zo{7ZYYXHXD~r$eW|YcQ;pwvo>j6;dS&RS?UJA;T@1(dXAG z2pa{Aegc|h7}4w9b9a|nTI?F1=26y<7)cDRsW#1t_AAhuy|ltZm*ZeTnTIoMB@jjMJ`j_255tSfx)awwu?wutN;xT+ zV3@XsPg?(8!(8=|5W6V3?}@f{%Go9bk~oFv(_Ka9QoFH*XZP#J>K8D$+K<`?8fQa zWske#?K=xhl9%Fsnw#I^dU^+&g8$Rbz)Zw@XPX$?QTsyla7)xmD zdyxq;+J@;XB-?tobowr@#rhOAQ_or~qYE`0iTA|f1Sw2`BMR)RI&QfoibFsFSY4R< zX9Lz`q9Agmq9z-UN_A_+6o>~@dK}aW(Pc(~GHZ(NRAL4W^^?vDR>1Zr_0<*F{wg30 z`9yBK4=Y&L^iTDwC3YfC0WpTPxpk{JOo}5O*a_6-2?aJ1=ngDB{gIXwrtMCPV%LC0 z^6@HKN!dT@QBQZnt3N1?O6U;8UgakT2Z7Q<>e!tLT%+pAN__*+)RKR@idnN^wNV=S zU+b^sCcQGiSaZE1`y>2FomI&JzqAiuYhOf!ut!D1T&1|+`Scs~ssTCGN)D`0jtX)u!g z-@q;&gkaWj1`BiUEYb0NV5%Ci{YzwXA7MiRh8r8SQ*-abMtd7t48{^e?F%ZuZJ%r$ z#R8%PyvjCWUhBlY=RBvuJaois_#BvWWEw6oC!~Sj>yak^P zkfmALH5yNPT=t93tt1zht@lY|zn|-I#H0H7z`JERBJbW;!$UNF2Ysr4Yu)y8y}0HU zHk_U777~mF=phHg%X|MWd`INIWvf3z+w_*N{76P0p>yr&t3$~VzH4`s^%c{r5fk}R z{=PlC*n26+Vinv@Yo8F~O{V1dY~v5xSEW)tz@C-i z*H5=hpkGAt=$$GT-6a*}Iz=xdKQ1M~GW(E^!;II*owC$IYtR-s8*n#u0A~Ed&6=(= z{ukJ(8N+sAJYC+{GFsSczx(s=XzKOqV6PaUIRxX#Qr%bNRo3qc~ zhl4=j6VInI^AVL2>EeQfyOikd`MYlOkS-Xtd=Bz;1H($*6RIU|8G6&xm z>g|N@dQov=sLorQ|Joo&v_S^#R{F)ii1%%fYmme4Kf0$OX%Ab+_@qOl4uj&K;!?u; zh0Ydm*gQmixzYKQ8g(IY%o}AB^(T3^ooeJIj`+5lUG4P<0`7N}oK;U`%acU(cIU42 z4{-2-xa`u+)(^Pozy!1pc+NI~S|R~_^K^wR-BBF!^e4w6mYbbzO1-Mnhm zqp?{B5l9YyO$VC;K~B5Ei^QO2R~hV2GI?1n1I24DmyHzz(${y*_)|bSvnyTOUQW*O zs8dZJ9^L9d9l2(kn0>EiTz8(e;?DcbNMtuR=A{37X%I#+jBAUzL84e?T3S`p=&ybp z-)~bZ)!D_?-MpV5M|3vm_``1Y$JZ0PPfuG-mVI<<;PZaxd>1ot6QrC0bzyS}(TCUl zt@u6X+eXyd)^Ro^YJRFq@0I`Y{m~MmN4$LhFq^+`)_k2Pw`sO_4d(#F4&y;}i#O07 z{kTds`f$!n&P5>d+W3)yf;|_&NfZz7ME1Z(Dd@F(<(TWDeGJcZkWl5q(~352I{r)q znv}pkO-lq4|J66+%W#-9#@hF{dsy;f8&?^jDzIds$pqG64uL zm(o?~>JM+>*?5(ZsrV17bY}xhcvlGL0V3q%eYP8{KObW`Zq8vIpbF`M$m45(31{o^ z)#$9$CIObmA625-r@_WaYT%$qY~_CJ7bgOjSrE)2c%iYl81er2%E<1r#U}pyv41+H zm(uPOi*-E6V{d0IMUbKmy14yfQQ1GptFk7(P;5- z#BgTIGX|RL&vO<34hLpB{S>jHXLqh`xI1)#$k3Vro-T(Mg?YMbz8%25Euux}P$d#l z2MEkq@dkhCEvPx)=(eqe7cJP*^=V)AMm5fN3Lx=XmN2yKlj5U6{FRNw+h?9UeGa9Z z16#-(CP!0atq}&D9C;gAeYevYyN|BI{51J=MtgSE+}5v^`m*-wey5+bxZIvD0+Eg^ zqJniV*_bL7+&`9Y&I;`MYV>r|82ps*eQGj`kKBjy8d>QY3bk%>g~{PJOX%Y`xL^iI zNu$&E-lqHqUUh6q#Hab)_|DAO_FHqtjl&$*nM|uP4-STYc|lqr(>A)V4OAi6fn*Hj z?WUA%bJ>Q;JH=`!(fjcwSuLvBRp>jDPPjiB8?YyC>tMJfj(rHqzM_RQVe)!Qzevo& z=&JP($CD_MRZEp2-5mAaDf{;PC2?K}RQMXhK5A3HUy4CTzJTE26YL9Wc8BXZOq$); zzMl7vQ~se~*{0MMHcV-k$ru7k=aJEh#=GTgu#&sKrB)A)MM_qaEce%>*RP_X4D`*q zF=`ZG-dQ8|v*X)n`Q+tHi2FQR3d7a4U(<~BvDE)wHSB%3A^vJSe|u2A$bs*#QGE;+ zDEfx8_9V)Lzw?ys=5T(zm6T-m7X*jW6OJAQ8n1nnZz0B{XrSY#c%o`|VlX-~15ZLu zLwB>8vYD1%x;SR;wLcGo?S=<@^FrvEM@x?h=fEb}x}@!sO$m2A$$uWAg+7<8Rn6lU z!sz?#-ze5vKPnth&_B9~#{QqU#HK?n81A{Z5j%vKqbzTIrnTADMTiYxrM>^(jq(y zdzsRtIV|fzb6;Mz8E6jsr+X7UOWn=#_ENcAj=Wu90X3VnLAitRS4T-(8dLm^1cnuI z>uEee`ci9^l<4vLecqtW`?55(FO)Sp*ZHf-g{MJU@xu1maopKc>?GZx7vIcjI-vJb z=oSCu{)av-F%=50iRf{<;v;z-jneVich8$e8NMjx-4?&o^L(coNqMXGsXa|^NpR2k z;~jIOfg@9EA%74Y@uv_Tp8EucCfcB`ujgOQ&A~GlIZK|teRSq()Uh?1=-Xvua;&bM zvbkQ2(|R*|;+$Eh!PgJl;;HmBRljjPFvE&Fo~pw){FOksd1#E9jXZmA0Wjx<4WCu6 zrHEzUkN`gDSxpLHrz256bXMZ%Gn||YoL1B2T7T7wnV(YH-=Y*k#?jw<0U3aD?r&7M z)?0i|oB!A>7yz%IG6-m(cwb_qmVwE0cwQ0X3q;+1QXb-WbI2S^`RGG!pL~|NLE>hkW~K2nf9js?tl)R!1{$!U-+| zSUFls)#GvEtHK_P=pwuV!Z-IVrS5SzUi=216rQhuB^Y)Eet11jW+q`&$CG^xG!A&PEqR+gA>Ti1n}#g!uP=%>^pGq&el#_#9MiAda9UPboCcf(zinHRpYQNyBD)A zs#KJ`#daVqo2}0e&jpwDVsJ4CmkCVT=SHP5!Jmcxl$j@Xelq|A?ht_U}JCjrAvP_?ma8}7x9|Q#p=t= z6!Q|m0E=8HN{$jS?|lBsvd$nhiW z`5JU4gKE1zpFLwc=|9@WC3x?2+M?p(@eVJ@o)UiZNA;L8FC@Jy^(jzHz7Q8dt5lkH z+t<6Z^-6{$rA2s_y=8piDWm+vno=Y=$1nJEbSO1R_B*%WC$2MD@2K8M_*Klr_esco zrL-$I%%~C*6kcK5>nbkMFu|L>NB%iOlXf@+6^xUGsTA#Afta#80AT%4bAKbA|3zRz z>C9jET}12(HP=v|NbjhX_*1<#bp7w>=BqrEK?$|oBO_G`VbRrmJu@kc$}=fSR|=nI zU-5#s1Ot|O{dfO%BDHyi)8QLLD!H6ahgF2{${p4a>PwM|Z%05iJQ6s)Xv-ZQlNCK3 zt#&W(mP=2iWS4L)Ie+~-s9Pv^eO4a+mJ3+_^QV>#oWMi%b|klF#GkD1*@nyS*6*dn z>kiDWGZ%pb=6(JWR|)0&^&0SF-DS{0))TF+e|G%PoS%r-~slInO z3b=F6Z|wpx*=F6#6hY{!b&ZkcD2C8fDXKcY=%YgWlXjXs(89K>T=@A>^~2$ssS2as zA$Gd8?-AIf*dfWy{unV1Vq}t^nEmI*1%l3Ru~O6SwS0`73!v*OW$1(EiTe^Y$Wgi; zys35m3H*Js-;!^^ywJ?uEt-4}rIDFWt7IuHV~H6fJGmHloH*>1G~Wy_?W4!{{kt=r zb&gz5U#}-D<|fWA-mzbp_9w2gObXG@O^XaxV)TnDcgss@KkmY=ptkU-GvXHgpWee< zw29uTNvFiWCvROFKuviZ)?T&ICyAaQWptYGJ-->c$oA`A}EKY+Q>emw;wKR=CYjJ$7rSmt#ar^i(0f9oD{j7XeH7V*F z`CZT0OLl0#7`1nn;RmeGT@z{`BC$$?eq_dQ%ca0??1KH5R#Dt_YW=o4K@D_0sK`n& zpz=Hx{-X10qF_UA(i{)}7Qfn#?#Cy&s4v_qR<%=5nV<;F{}aalz6ZVZo0%7LTWyH$ z{kScB>?vuK`-Jbn;`PaPvN@p6VsVBwaElIwQ9&8SyRdb;em-x4^XFCUOG-uHA6n>) zeQAH(iZ0CCmvSjr5UQ1f+&Im`9a%Af@>zGLe{?#j{3Dquo zUC44of7x^3YW`jY1tLJ>>b))GYlpt-9+ajj$INVTtTKc<-67j~U%ov}sKtJY6A>QA za_05EdG+#k$x#=!&nJln4%Ac|2Gs3qmBkFP&dJh35JGOxGi6YK zB(^uNT^l+Cr6SqN73oSd!-ILKIjSuaTzHptvuid7AbdVwHt9B&*XWis#m@-(c zxYsrl_xYYzzCRk$^$V=~4x0`?8vhts1wNNq45td#_YHNg?mL+1J*)~dLAUEDz>B+V zKB+yJ=Ck&>nAo%9%yPB6F+I4{)NCc1+<8s>99_*!y0HG=|3>56u>eZDF`P2Ka6;(k{#t<$73u zOj7>ci$vt$pqJ?Qcc`Gf(!+g>>l1YI5Xfn z!|y?rUAX9y&S5 z0ltUmMbI|=8|Q4*mJ3qIgJ@I5zN&bI>NekI@cZWb=-e`Q#LQNSfwI&mluA|({ynQi zQM3TzsXBSq2KhqSUeD@Ie~@n1Lhin=m28KzsiePEP0pVZC;Zk+0|4)Ub5YG`*#3n* zx8G>mSp5w%NxuL)s8*09`JDF5S2EH=92}-dI(pyC)`|Uo(muv@(jSphJ`bfie7d=% zG32vJt#F)8dE#9CEGVIhN9TsBhTNU{$1=^l}o%F z-8ip&+6Ai+p#RUA32dS7;a9!hP@NQu5SM|0(XtTvv_Q$fHCj+l`yv{N!hJ@#y!4)x%r8AAd>?O^Z=TB9a4=UQ5oc+-7>>fNHM4;{pY+T&N3H|9C@ z*V%FAreNJ=yI+CYP27P^XzuZqf3t40(0>FsdIA=KT(r#w*U@4h#y$U_J9%X7w2pQ^ zv#sq=vzEEg!KFRi7p>u7yW;%=ZjZ;NByrjH)y(I8G}+Zx`OQkJ3|@Qclyovy5iNWd z)Y2ao`fkQ3w2`-7g;YSv%+}q^0>$?vTCnE?$W&(1Pd~vOq7Rg}F}+^*c6facwVtk) zT`{T~mu-3wG1y*IL;90`)w_ISvm_A7R%Uu+55mD52)TAUEDXxu&TN*Q*D<~*Ot0Ot z7rhy2Au{FQZ51rFx&=tc7WHypgRSKcdMJeXeEXe}vekwy76KcuVuaU2I2=;hzdStr zoiZ@O28jI!MAgTtf|<L%uI&cWwQ~Ao5rpR5@A>KN`xUC?C4J0FowW5g8!9utLBDF9q4A~%l-1i^f#ZOX=L304pL<-V zIsM&Z@Fa-grH9w;zvq3_L5bczc;|33V&c89SrvT7n-Ua6)cl}3eYU&qH#7ay7Tf52 zltF3thd_cdfcECy=#S5Uc#@g+->#Hi^{N@@C%wPkoVq777r~lva>X{4aqKUMHmeAV zka@mm`tjU@p-!fM+^G5ck?Kv5Rr}(b4=`WoW*x26pNg9s+qGR>J#&_apLshqp4A$D z&bUV|Du1l}W=j!6x|P|D3V3$>&D551Ry(%r_1L68u7%vSQLOLG^ib|LAC9k9wy-nO zRoDWRhKi`-8Mx2m{p$Y3P7k}n)EtvXScXBr5)wt~nfu6Bzf=^L3P8}6>ejd;`oJUF zNv2R{2Q?@(r~`&@h6-p&x|z{<52*o>eI9QE7tSM#d$h_bSi9siwds3U*uH6jojo7d zdC@B#@yBsnH+l%|d3tm7qvwx{;e}RKtwTA*68BMGnSc0tw@s~wlliYL7m`bdB3Jzs z4tWo5A*Zm$71tH!$k!K@-cxTbUAbh@P`J)i5o8x}3l&Q&&NWxh#SkMJ~#oXUa9;M0NP^ua?B<>$L43^X}pJNe?$* zs+_huPafv!x6C)9ldX-+>K=nx9i9fMguw-IR zkmHv6TIlTcDb(|}&Gw6BxiYZ#FFed|iraVVw{a z8T{3Ezh(5jbPqL$755u~o%c%k)~M!MFYKJ~jkl@XbV|A;8SJbNOnO+WrmdO)%X#_L zjVo^`o2w1Qdp0;Jqh$~UTe15>ZUCw}AwtWu5R}yOUFGJ~mif)!{TQ546#90L)(g%K z$IdUF1P+JFr-Iz!2PY>#&lk+v$|P#Tcdz8!ehAI;W=`xqp)Hr~zox5#UeEvnCDq~m zlc@%#lEXOA5M(cDwMKLJbkyDXyH`=lN$Tm<-#Gnl;T;s+y0g%aeJ*=PtbvSinjfi! zif{S6FkbgU`z`MYeE%uO$Cn-b4LxQJ|M_Oif2`)(bd{MsZ%;pg_j+2EVrb z*_zr;a|1-FZYG7t5yDS#@fE&gTLYPNd`}2r&uKSU%QhC7@|R0`b4Io~7ETmUfu!ZD zygca(rJ$ioPq^<7jZQl7SL8TF?!_MkPCze)9q!sCeepm<7M)dQXYRbF?v0#(c$7~z zTU(WcXLL~D;a~it<7^X?!%KfimRDV3GfsqRu4E_6n}!Ut;?=O|;$U!m{>Z`6X-|M2 zfF4Duc&l~t!O74$ag{Mn#-CU0+v({^Gb|cG$Bn5Dx^}j88lrH$?GJWclc;_g$aqWd ztI=U%&sK1`AFA@}hIjk@KBSiBondKcsy8vm>>Nn%$8+$&L9SpdvQ6v3{-y)x_4dI* z=|1_tODeyY5+|~J>xrQD%0o-h71`P5u6g8b}qm#wH~03 z9UQ6I0+c$xL&yIlEXd&6bz2fj$89aHG={mBvi9nJEI03dzCEfp1pantxP(0(Znh`5 z41Xq&=8D-WIX0}>_d_pv*D)aq-(e`UGh;aT8>LPA%EG?l+r`&s&Kdfo zaABd&{i{S{dknb1SH68iX2EED38eF{daAVY+WzmvaN6WosKj%y1&r3&IgGSY+mecm zzME3$=WaF4Jl39WDfvOXU9a&->*q4Y;k%8BPc%eoXYR3eh&I3gqkmP!M~_J&+$PTB zP`Ulqge7+t*YEuR3j4HS2_McTiR4s?-n6aGji)ehlxHjgXZS`wwabV@;y+_@xCQqh zwhA-$xXMsfgKIx)W%kSnYA<`EgL|1p|6-77-esz|9m_q!=CtlDaxA_V|TIb;oY$wNWQ3xl@RHoA5}hHU-(gE*p4y`x`0q z9=Ba_Fc`1bGi-_Ld|v}&8TbBn2q$V$bkerEu?tiig`)932v*g5yA$urhgbC&j0Ir- zNj+dg^WIj*G!IP2Yi>X_-bZuSF@gH;Cc$?aRV!_vMIv&`SItUkoIY=N4L9cbG;S$> zlTRCC?^*uN_^DSC;S7=kIr=x@IKQL*)rB&on}B^zOsucZeiq;@zX>Q&d7u&Ut!LMp z7ep_HaHwt)?SUKxE{H^ZH1C#`f=R)Xm*`y}xs2|x_h;V>$FDT(EZdz|FrsIeQ!Djy z>%O~PgM|!ypUA_YZ(By|nw}68i|c_Z?W{X@v8q5&=f1i$bY`CkP;=NP84+^vcRU<~ z?zKO+fKdl6WNgF6-2r>0rpY#Sjr9%5_1?J&**ON(tO@8qqG#52R<^+QN9r&%M#g*P z2Giz@8g7ccS$#6C`qyjHdDP1He$9K<%Wn_bY{JzcteM;z#tgE>1ReNnSg-*FZ4s_z zQ}c9k!1QYr>tIWrvAi+KXV!w~bxT;`4xsYoZS@=^>a>$BS0rz{^hnS;47D?sJ3n42 z!W2~09tOsG3qvyJ_zEkgwQa4RhADjF(Qf)(!Sw~$a#iV1s6MkT9`5@k^qY$8O=Rz@ z?e_SaJRYxiwmPlHJdr^P1(oX@%~Z%2cBSG?Dxdt;z{l&Aap7-Ruk`MC=KkAWxRU|< z>%;54YPgicnslSyI*^{0ZDpBg&zzcCtB{?>D;5FRAtm2_>$?w_EQ zIPt2L`eSRhxK;w+`r}DKhrvE+cZF}KIlp}@+95t~|C1{mdi&NB`wpPY>)w9MjnW+N zIBP^^`O!o7KoipnSfTFha${%At_Od! z3Zn9^7AT}SjfKH=o~@IVM|FSrfAQgOEjlqt?=aj(> z|LZZ0#AlCS2Zwz41#CU*9JohhI2{W{86&7*JF%xYmBER2qhNl2xlYZgHM$kRl!{0o zFT51+!_W`92IMkZCwG-QK4cz%*8UmwvHo}GSL~p%ZF{SK)YZRv?U9#>khz<|JRieU zb4X7LZBqr$>dz)d$VuIByxL05^6qX`YRPd8HtDB*F9P@a92KG64-Fsk4N$SlurxDm zJX)OctQ8#xOKV@Lm2%2q_6jMFZW~nj(r+S}uf1J%L7`LFtZ5X_L2Ys48FAs6Xv)|= zOGEkW=uePJ<^?@%W3e&@de!s&R_Ia1CsI6S?9D#Q*6+WiawXf@KR}@6cgI(6 zSYeO_x6AaDY2mDQU_8O5XP{}Xnm+@*|K{ZnJe=;8*KYn9tba^i7{2U$f26deL%MSo z9kbm%*V@NH&AcAA|P0sZoEXE=k_XIxkuMCEAc0 z>Yes@EY>#b=B73u@A!Rw8`B9KIrJ6APA(-{I{UlQ*U)ijZ7f4&KI9gjHbm@oQ~+@6 z0$LKYRwb_07l5$nW^blN)lam2c~}XETc%NOH##f;H=Up1oV0^CYq~IAomc+7Y`j*h z6EylO_AqI5kr&m=i9j5`26ic@q7|Uq=WWxOS5A~NxmQn=`&ow5exo$G?Mp9ehPTJc zSky+3N!FP+qxrbTT&^RO0?#3l?FThr-3$E}>F=2JIRV4pV3ol)-u&ihV+#1SjiJ`l zkCRfg61(9oD}LzO-V0%D&fEBL{k9dzN?acTA`fUr47(is$X=&l!oIY8`#g zJ~CvzHp;8r9UWYMVwH=%&3fma|9%*l6kluL--3|rz27wtA6!U`!8$15me$1Lw+{d_ zBfH}sdp;}kuK`^I&L7`onr^R;cLnArx{F1cxw0Znkoww9lj`8Xy&Cs_E76AO| zJR2Gt!{unCBgLC~p+w8$gP8(`>)L`W^|cMwq%ivEqj2uq3zviV_M9hZiU}LjFAF&d zOMF(V5mZndV3?og8N;Z|yT;(TLKH7bz4Wh$-l$XN+_(aC?a0s1D}E4+{!+;ayK|pv*Pccr92v?ctrUufFp4nmIA#w)qtRV_P62g2x&`%z z;*I-*_=xs!f9>cm1diIk?6rc^_kG&TPa1$0I`y0`ReMwJ?&Wso_tT#jE5~_G7w~+c z%imuWXS^JtwYQdLH~v&e0DE=B@+Uls=A#T!()~SANU0@g+(woL(>uU#h4mlX>eiQ^ zcXa`zdf#g2fZb}Nr}sMrh$5%A#oKu`g%!;(@4^xU zyTLGNRf{K%9sT9p?Xo;mif^8-B%>zGIL+T|R1ebqDSWX`^2}|QDLngoLp2nf-|z>d zx5G6;>{wh@hlgU!ifd0XBiBYh&r`*U?n|8E)%(ka1J9!L?d8Vwc;0>t(8hE5^vJh} z>W|Fxa1z?LOk1wb;y8E8UPPpS==Qgy`!amu*dmp4A;pVzz_xK;Fa??YS+)3g$_|~w z`#T*j-}XWd=(s>UQ95T$-}TNEgkVzj#BMg2dfgT$WyEvbf#TJE3RRGYuoaGtPm)(k zBH(J*S~ID1dbD+2ZqWB)AEbWoK07Aubv~P%0mKy4pVAd59&Ca&{&qR>%Vj(y(`hi( z>ZhcKoiLEx68_$};8;-5IXd|~rzEzvt?Ra`JQn5Qva@?bEi7%)z}A3`(hbc(kdwr= zo7%~3Waq3tXd$dQ`_{SQsTmDF_Ur46do=#C8g##kE-UXD{?b?DMV?K)A1gao2gCR0 zg5G?_*}Ob;(D?Ob$kItD*Bc}shvVWJu?vlHVE{J^F+)5up~ua*=TXUDb0&p{!Az~M z#r8b}B-D3uY@E>Zzc=%F-Z)ujPklAJXV7n-$nRpbn;dt*&*?!SS~F(;zI1#@O^U_m zLmH~1G*=l#6JdT>?1ogvnzXY^df4jeaZQ6lRJX11)b;!-#>d-wlx)QS|B$ufe0i;) zG4fh7$Ku^YuiBOPKpK8EPu^_+yB-~Q`|KWoyljwvtbZtYsCB?LB&@QHnXspz5IRbF zgR!_Q44;YQPRHHi?75pFe!NjKr9#_Ux1fh{;rrP}jIGz^=!UP3PU7a5K4vq<{7`2O z`p}9Xo-#QJC-zgg?W50-8=AeFr5-v34u#{oNT_1l%)uW$*cmX@qc3F!@IBFz^In(setS+sbIK@s{pav1ZLTMgfOfwj%>V}Ew5F`LTAJy1fqqfIVO z$%C$Hsp>6?kj8%(=P*eEoE)AaSx}XW2S6tu`^oCU>@*f%LDyb8mwC;!{6PVk&(mZ`8nxbU+VXyUnYwzIfp)Uk_tS6O)GaV ztJdIOtuk^Q(^UsvqHq^b%3QxB@xeOY1idfjQElSrY^_(LiPs?J4j%z)CahN9*-z58 zuXAf)&h**ML9)~R8zr>zpco944K@8fTCRHUO*6ZOp^H!74fo5f+((Z@cf@nM)8sM; z!4pczzsvR$1jW779}%Ob6*}W?wKozGOmxiiY_4ZEoujbp7G3(&Y85`8cv$~S^0-T! z2YfSg9!kWAJl&sesknbtruoZgHry(BS={#2>U%uv&vxm5`3^+y72Bg=v2V1UXEVR! z1|N{AHcRM~{E??2fjL=p**IS2f}c-EGjdrW(%biakxqY8ZiS&yGp%_+BrGiTa;m#- z7P;|I+t>=Vg!_NA15{PFdJAazA>N>KQa8lCd@C{u)A^vJaVI}(sc@B_X`qx3wH{tx zs$u6=>Dp42QA{8eA=_CR=EFC1U!Rb>V5*T^RJ1$g)t`;p(L8>sL^&Vdz1i7+Ox}xj z6A+mI&%ShyAc<6g{zq-r;(OM7&eM%>$8j0C1J`9*$|}Q+M(X*Ua=WW2xK36_k*;?i zON{APAB5=r#n1JZ7*e@S*X^`V{P1F5W@RV{|J~}3eOfqI2eKu#7Y#(12N*V|@0kM$ z!_MWYv~f$f_wV{#vpo$WCj1PWlmjZ_zn6SvHqS11#oy)oay5~2gmt||(Ah;aJu&IA z(gPR!9a(H5maFB*?E9=9$L-Uj;jt}FxD-r{t3IF-epG?gfcE@re|S@7n%bn^=fB6l;ZvYn1+pLTs~yX| zHw%VRIE6-)mMII<$+)m1r*Pcq1KtkT?{Clt3`1#|Lp~FD(H|#`(-J4ETw_2rOZQVi zlzoyTf43c-@07w#i>s_}c~YVIehF)U)*^fAZ+EqOTg6MIK4c22W=rg|*YR$Ls6b8@ z&n(L14PY~1tW_h!fazSe{grLs`_7b`Z0V@_((6C!rS{eVOd~H#W|dbu;GwUbm8Nn# z9Ae!XUz}Q*{vpcvdpvb2LzCnbaP`o3e3VDP=pA}kZPUQ;#&a}@6E5R~`n{u4aXEVq zuc@s_!Cx`f9XVRN?5C~I>vc2jt>LW~8WNu<#c@;3*(WcRKpB6$mV3@fv(v%F)f(Jm z!#Av)|6A&d>LICKNB2#l-21U;G}rh|(5>Rt${)kLxYNE6{S^wSZB8aqw8MPhk_-ioA!aW zT4pB8cDu8=d|*la_*7e4`IWOseB~|ne;XwW!3JsFj(1JGbzJN#`ed%mb${ICg#FdJ zxMa6QlqYkxpu>oxFr|>&r*w@f_c>F)UVqa~yegbbj|=*8sdoDEMwkc0Cb!mbX-pTd zqs9wdGVZ0d@O~|rOYLOwiQyjq!s**BEJiIbYm^#0OQHYnR8?6Vaz02n*NAKRwE5SxfJxwdu-cRHq5D56cMCo4K{TCSH%~Ee>9+Wyd z-e&+E8ZyT&crM)l2sdJNxPQX6N%QXuJSa~MQp-^FKTF|JtVRZo-Txzv?KKz7LgR0F zZ8U%g@J0Tm;q>G4@782^VF);4FKUmt3K2&3xsGZ1EXG)Rh!(P+e{=vYkF)v+a$Xk) z@1OP`rvLqSTOY9SZ?_hGqbnf7%4wdZibH@<1OxEt=AMm5vTG3pm-Dx|Z8T|UhjJNo z94qk{I^;8h4IHXvhtyxz2*Ph}K6M3t$68Cy?i5-Jg#Pz>jOvSB^LBu;d1W(Pi37pq z#2}}3p#=g-XAR8PqxPlV{|yN}$=?0zA$iF16kG(0)@_Cwa&rODcKFon8zf>HUyUJ` zyyjMKx3`XWr}JKBu;=UMaJ4dlKo9OzfE?7RdLQwBR~uA8Dh;B z6i;4f@*!sAeu`Rd)jyP|I z7X9k6nKsB^$m4uTj4zx8WPVR><_Gh*Sp}HSot(KRvRriC#f*qDMV%fpeeRnr6x+yr zdl9Gs6CJ1{1n})zfQyYb@BW$D?%}*6zvl~WZ70}^j(*QkmSH9*ZF6pBa9?I0C%z|U zpLCDOoUV=YTnNfv5@8gW98Kj;u{NwF^OD#g25R*$k`L_t?{+NN_ZiRKuzP!Pc@8e! zQe)68E`RC1nL^~ecvfw&eTnmGX2722MgP*^km7S&x(V48{e5*(Qv*ody#!;#oIfLh zE7o;Czb{lD^qesNMX5*(!|HvaPKnX?Q-8GT=V$^z3EP&;R`4seY9|uBX4g}tZI^kP zp6TSmu}{3)ekM)Kj5F8Q=dfo3tXsP`R&hRVMfrB+JSe02viCtvyFGwt zFvjc;F;Xp6#*_9t#_r9H)Na?g$Eh#dIq8+2j(@ksv*FG@na#7;WC-`C_^Nw0pE+YEG9=Switl;FbDDI6 z!!t>2T+~=kc3bU)6!LHDI%&6aKl`&y9?{?Ux-2fwgmHY7byOX4AWeWyTWsn zkml}`pO4T2V1v=U(ii(Vc9(5iz<((Z4U^T8WxmINcXr*xDzznbIu4>+?Or*zX7=#M zoj*3n4&b91Vv#a^bU;T=rK|7ciDjX1i(M{kzQY?UT_iQqzkAqeyhVsye&aMZ;vutq znY&&Nl&Wt6K%LD-CmZI7Q~pk{F=gKuIdQO^_PTBC8JDWfI)rr8E0AK@%KB&pojMtP zr*P8SAa&WL)7;;gy+gX`S21cUDeTYSJ01nf{eAm=9zE*vD77b_061!wdCZsUyYjHV zi#&EVh#xu0*nI)Mt3%dL)cqBr(w`*}4($r7x6ewcvly|rVRBv9_b?+KcP5kCd-}L3 zA7MZw<)BGr8Z4N2Rco-$tvaS+OFY>5Mz><`@mG&iLPK_9XEM`NVzGSmhMO_98XQ?Z zBLf2(m`k^0#^cw~?jh~X*|W4S0EJ;k`-SP60N&K{kR+M!k1S*wDd!gZ+@m%x=(Yxx-3No9W9cqWHm7hBA z-t+U~PV>{YnHb}u`!)}rY43|H66rjq1p-y2|MC4__9pd4SkM31``yIQCf2^%n0>S>nF=6s{FH%o*7_@u*0X3& zx5yn^PuoHC1XEd50%G6#(|ZkH1;6p%sj7d^grneT{8W;8m5o~2`Z3>fwmX?sb*lE^ z^4SS39Z|H3mMrAh`aD}d&NQ9@&<7IV|M+?97!!+fZ63mL&&bik1VZ{rtd6d*jGRcDUBen8G2 zE^*XcwI?NJi4FrR%t_=}mG?FKH_V*~mc8BJW|=hqvOlO4Z(9eg&PH2Axam)A615+T z)naXyz{lkai@)hOtLq0RgBjIr`S$8o{H3|K`F#b3$Apnts>-oFZ|z28W8Y0>?xgOV zdJ!cHY7Nj=x>ed2bsT89@*^~;LqSE4U3fD(8`@YNDkus{G^ zRb86%zuiUrGC4uc7z1yaV5kXp5-ngeERoFFYuR5-({A6$!<6AU;rqN~pSJVg#Td-@ z>xrHEunS_*;W)dx^Ec88K#84uMdU@E2%#tTzwI5{Y#_;Kl+&;hM3-B`w0%B}p3kyI zw4G~wyi#Yi#&m*`@Y!c0UZ+}C<*;R58)Y(sn=n5txk>xW)Xw_q;`}Y^qBf0qgb)4h zJ;(p>rsSgJb*bszfnL=&OEHP1<8e!w;bADVl&u)vrx`yQrb8+QWIub zLMv=n*eK;~DUW=y<=h66+_!jmue-F-RD+O7tF7`3R<-1f&-CtraW-fD`vYp5<`cpV zrE&A}E?F z*1x4vTl~(I`_r2r|B=6ufwt&mu5#5@lJ~dRE|87j`*EtRoP4$PQcmh!Fg86DyHYx8 zR9%J;TDD8~GkvwXp7FOyZeA1}RUO4CKORg!hdV->>>s?>3sBUW)iqp)8eYIxzI&m( z9g#xEv>WNgMT4(!s^b$5EB=z=?Vc+`SZPBA*_wT=?bb1k*^U;w*kS?C!xm>xFXIX! zLtS{$YDDuWyS?qE+z01hVXZ%r$|9SYwsU_P{jSw-srCd1udVRuVfzM(J+UTG4>;t< zY~2cP(H=3r@#Z}5tUG>TUK)Cq=mFc2Wt&tVFi2D&RGc3LDag)(7$*=;u`BY*1%K;< zn1!sNWt7}76+|ukH6K8P?QM&QH18{%SPv7$j)10GWD7iYc`U2#Qn&*dmcnL&>#XU* zW&71y?K%dYJ&gmoVTi`DRGkX7Mm$>mtK9j`zs)Wa@W^0SbG(NCko!xS^wE(w2HLCS zHr9*Zr-HuYVeJNDB{a9cns=b~H9?((N&m1YudnZj?bU0v6d8C+?GtLDpt)VQD90z?fRNb4p}~Dq zD5fo&DJH`^bdGHdwD1# zE>(I|4jrs3#Zs?ho!j>zv>B^I_AGFvcOa;ReXaSy4l64Tpcvr|4J%8Hez<+U{t=yS zslH&dqx|(F|8+*4VO~KVBD-@X$m}xp$)N&}qHrS3bnQ>9>%B^`RmBQ+?Qww9oqYbZ zJNSXTwGO-R3FH!KPd*}}a`Iv#tFDUWdCzr62GTfImYo{bt_I-4V&c{Cgsy%6v3MQH z_FkD%&ROk3DDpQGGtYPbt(|P-eynjcl zKB;5eY(a#|tJ4znJUdZPO{fG~*m5YB4*C_lB+Q#l9v=-Q<+un|i`1WUBznyY%AQuA z_V+exBg($n=rK|WT2L^G4RV0Dp3N4kkBe5G<>c|yX4m&&n;Uw$wEE7sIhcVw(!slT zdx_TCs{DLAT$5XyI6{|AdK5yW5w3t5tN*s^+^f9%#k|yu_Ii8s{A@3j8~fYx)FX-z z{*mU_Ja*}rY|Go(*p!6qc?9F_6u3HvxCRo+=vryl_DBob&2hcCrKg{tcRA9_bLAIY z^CSE|{QZUAf{HGWzx{$9%Zn6!{27^TlyB_{QY}os=QM5a8FU&BNO3AJ$EV6bMfSdz z`{d8jr+v!VU4Ju}*RsBdHNK{7D9u^6@4cmGN@F4Ld#vqE%4Lg2#@vkIGM4MFvCK{> zjK5uFkahOs$%v5<_agE|z~?bzha_6HsJb7$H#^}@bpU>{=vwQ~q|hm%+tA@^WvIRK z6PfJyFkkck^7D$f(Q3a<@?Ks<7Fw z7m8iA8r5PRU@SWUyG;5&d0_R6?ye5xSc(~MZum@}!2E(7O~)1hBE+)f#x}$E#qz_S z_<}+h=;AegFkIiZ+95D+Aa5=1d!1W_az|G(y`s+aq8+iu4l zV68dF=w0QMin^hZ;AXv#E*`zybzhjT7n5(ul$rr?tLsZIF?&9xVuPWM+KdK-0n`&HqJ-3!&|FQW9RT~32eEO=xe_0|4<#zfV5 zUs-l8lOrtXV$r!}`YYnJME!8{A>#f!w2#K6?{Oo&6={<2dX~rtD?Y=h+0VH_b3Hix zpvls>R94!d`s8zBmvLBl*F#(C{ilD~S(@>e@epfX763)L{Gp<-Ft!}ks7HW|I(!FX za7Bzpzw=*Lz`vU{7qMQ5(G@9oXmRo=EIs%S-2*7sxulGrm)*S-WQ5U0cU6hg83md> zHmg7-eDU5Kl3|_2hUFa$HD0`3zXHMJ+;if>NvggaTsBF=5HD^_)9n2zVjDGGm+`#;F55H4w zltxzmv1@hyuqUmjGaVjtQ=NI;m zjx~9~SOoB)tGX(k7=`ZP7H$8lZ17)==PajV0O_Lv1wh|dwvyFG>O(Ziycy`8yczaa z-RliIzZPn_cm|L7UEKZM4PTYB%~74wJ!CUrUS0pt-k&_qTeUj*!23F^E^QfjUi0 zi2>l@S;#M|$_s?ed({JXeOn6_6)Oxkh(nw&zlLlA1 zwH|4`+<9XfWrm2H8+@9%A+Syke>B|p{a){NU}g{@q^B#<$?a5 zyM2rA?tlU4jMepakoqRRU;)yk4(BQW(l`4SVYSC$5%vHO;N|P79Q3T#T022IZSyXJ ziF+qJMj6Oij=KQ(sfl~i*fKn|nHMfs9EJDfZ$BBvOB)Xfp$4L)3*CGJtzfM^UP>F? zZm;asyXBH%t(~1D-R<$}^-jV&Q||rQ{SdEA3SgQyK8Ne;u}PsAXUdD{amSd&@`a)E z*~H=V0b;KT9#fVQ79CF+^0ns=a&}Vcz~79_az+zdAAx0hyTGTa0uH{olLl^Sh$IdVR@_qX zduwx$?{(c}qemh&w#p0uLHy8c<3a4?`{IOMf1PD#6x%l@dR#K|+E0eH_G^jnV2o_v zo5`O#Db?l4sKCC^@Ee{R%m6>F(s8TC6^?Wp&rVrg~G?9L2#KfL0R{VKB4 zPbTRRsyv*mDYf0`sY?J+1tDA$wbHshaDBnTaJ6C)4F6^YdNE^?`Fq!@!F;x~vN0C= z;p@un!Xq)Dn_NB1)SH=cUEfzu*$Zq{)=aUVT=Qilua328T%Ap6O79vqw0ORME9?k# zj5lMGTN|$RH>WP3*n5h0Szby%7RXQtf6rs4lX0D8x!f|a%2T47o)t{LiqU!m-$u_) z;O-}e2!F7%x$>p(^z3^MrF$gan@9K-9Lx6H#q1mI;8RMEc(un}8?8)QtjT}N#;>;> z9;J16S7T(Q7V&*f^2VDS=Mm{+PE_{Q<~ejg{;wR0v z9>__jlj*(woE7ov4t~|;3K$>BdJL4y`>Fn@l*)b$_Yu7~DBztJSYr}?Dq_g6oW1LI z%PI-U3_q|p*0-qqXI6i}g##lp{aTBy34?qaFzw}L`}Quw{&%4WDoQ@ac6E6i-LF+F zdCxz8DVocUkLjcQt1|+{Q?ewNxJBZfkjGKz8&iL?HLjU>sso6ce0L!DF*yGE2z(xT zUS>ClA$70%ffMV*avn@=^gA)pKO2TE$)K~56UcSU%5Qy8&lAKim_|Cgm#)=o1~Rk( zaAZNG7L0FDUK)AJ>AC;R2TdCQkJ4|55*%BxqubniMHY7c@P21CAS<@6j~ph*(w+yN z;HeJhO7?L60-FVY7OGXZEX^gFUdtm|c*d#e)}=~Ok{cHtYJzqCFk23_M+{Q?AEi_` zn+tu|V%JYW0ejni)I6=7+y=Fr=0^laA%<_P-DOiw%%!EphsuiKyZuye*^2P49yXo( z%=N~$7W8I+k}&Px$y%PrnB6x zWG5bMI~=LbN>TCDyAyOinUML-_($I8!r0ryn*@y;Xe&$KKtql>R8s%$r?FF%D@R?+ zoaPNL%PxRuFk$Z4!5#pxHJ4sf2N_+=)D3eeV2~6Wc_U^Ju^w)zKh$i^SpXoc$Q@=m?Srwrya@jwm)gOa=7^S;mC zX3IKK&b&M{R2fZw0->|<%e7-JcS^Xy)hf>km2B}UQAw@sEfS8}Zui}@*hHgquNq{*~Snvrq!D`1NvF+JduD?9pdIS(oFaU}Rya5b&x2^rY9P;MlcHTsXt} zaAb>xLOhu6*8a{T{mM^$QcmDHyO>*YG{G$7M+1V%ZaJX%JDkS~<0$^kU&VFV3HNj{ zobPT|s((PbpUg|z_R?ozIMU%KlYN(D^HuHpH)_afr?>YGr$z#=Wn~X6gjVTc#NFH` zhr;ZXs%^e9EUDOH{U2^GxL*ZV^Y#AysgfJ~z1S1@lQq>InyBLZqKeD z^Hh*~vZvbh^Q)TQ9nu!)2&xS1BmUxj9Lo~uKPpmhrURD`@jZvM@7atd7iS|4m60Mt z5E~6o^Ao13xae*;(2eZ{na~b7C~*576AgU@4drh#{5|gRb9P_^u=m^Z!c4zthq|I|-0`zCDT(y(e z>dqB$@wg#07eS_W$m2=nL1YAmbvJuSK0FP;SAH7mV?N^+EJMP#4^L{;_A+7f+1Dq= z)98eKio&BOou4}JlM8)=3@S8w$Lg|)#*5&pr1L?h^_7fAPrt>7*EB3p`Ne{2o?5YX z_RW}HEdYDy-`dH3SgYNHvYH#+w(0j4tW8fn_b;<{SBNgXV0R$zw~F%)>6!^5-w@s_%2D1t$1- zqnJ+$426C}mcf=7;0F|D?PU{F&qjvtI}#t?ae&LH?G)g1~H z>A+os`MW3U-Boq*aU}M0Oqrt9M<{o?t(Gz`^UMZ^omyN%;@lKBs~6+x`Qpo~td*dX zHWHB;Jm;nA{xBB~(7#w_0X5se@ z-OxGYTHhL;Xf8Sl8{2L{KEpu@xRo5E%-Gyyj9C(;z#)ebSBThD+y*;(K|6->Q2$zW zx;ckzN*jA_AlKQF;`-r(Q>D!yz z%m?ackhMwC-Y~I!V&vrbZei%gIfAX!-S^{OF>^=O>E!w*kQ;8p#a3!yp7@=e|*)LCX&su|ec(C6irg&J?x-pU45 z#e}T6>&|=`zB1b|i+R3WBen5F=w60I@1OvYVP?-})%d0ibm@Prx zc!TAV0Re@Q)E+4)qc*%FarZx}ZG@fqynY}syDeoD%|6O4Mx^%b)X0zqEVM-d-!1s?o zv^YN!Z5+PfZ!EUHyIQj6(3Q8Aw&|VRwY2AKp0}Mm(8us(eA;CoZP4NGji*@GeveHS zHSF$v^a13ufsG1|OSj{dJY`U;#q&sySB4DE5ISR_r0-U?9W3pGOzNdB_HHQ~^gHoi zMyTdDsYx@9=eBugqg?T`tI?Hh|K{Jkn^X|nOt+F>k5*h=q`L8+dY;K!L9u7I<(tE% zpUcNCsP(e0;Kh-BzmNG0XkZ|}F7J%aaJ*$F{=7P$YzULWUW_@)84c5{UYyYr>dv<6!}d2gcTv8w;N9xDeHN)`_NFbi{wXim`ymo z7Ks`VWF37(BbJbcP4(}Z75 z01!i4X);*(g=rB3Y^L!Y1{ChhaG@S8!s_OgW9>U`7Co;tZ>|Co>{QqehC`Lc5<-Q( z)}?K?9)g2eynITs5e=H;^Q?NeYdtyB5Q1`6u~EVJS46a)@9p^dy7R)i385+!0M3Hi zGuQdkY2R5Rt;_Q8hhMUFJ16WZh@*INucJs6MyC41!{1+pa-(*vxZ0~LK=iF(mV zNelOK_(?h(I!0z13wUan*G$}>djdc+9|Cw0~C&R zF`6lCD&6z>#lC|bzT~tkXtbSrcGGQJ&p&d-ww>D}Zr=v$1E~h9^0k`kvkliQ*iT)V z_+05JnccTsesII2NHC>I1YD6m97}+U$Yp2M(dM!1P~Y}2r2=R5(T`64VZv%w59m(8A`7pZ{e(;eZU-F{^2ktWl4*!n~p1UQ%3} z9xQ@7@|=1tg!j+Et~UlG`nZqEPlkqmS$X2D%9B1(EvsJEg7S^74V}VL&op-4JB5>j z{t4G$9-Wc_zUd~XNpy$$Oy?1#Q${L@!@&$-`udxlMfq-V ztL=Plk3m9GK?mzO>?r%aCCL-^SMFTyp>vKQj=EE3q=L6PcW37g`*(E22-d5%@FH_X z>-G|rFb%m^r^vM2E!DO#Ibu0AL!Ya{Rs^y%!XvYU^wyot?5Oy9Ezs`Zn!*O&QjL)s z)!3er`Mp~J!Q69I_s7cV3cLH0*&KmlXY_)gywqy{d8JJLZ+`v|A|55gwEM8%lSl1jLV#>gv~@boZ{$R4HrV$lPCjNc2iVOcIF!b3SC=XW^&T{KEHh zTm1vvpnet0>2^tH!ZGt#>~DpZe;CfKDX3+Bx6MJTba$W29}c|#lyIOVKWR64yfGQ@1By5GrH^5PVpgZ9v0)( znXSV3zQ%vE(3x@-JooUP&Y}r7kbCi~kG-RSKcE)!9R!bk7p+F8KG6%M$L0^vrBiCr zO3zw`j6db_W2AGBUiBq@5G!s*y=M?%u?{eaO)T=mw>!uBoiTDub_QU%IrcYu8;ejZ zcZ4igYl4y12RE!vC%J9)nITWD;vRJ;Q@JjWD&%k)H!l4&zD*S;w>v2d@FBW%ypQVo z{gUmP!Kp6XgcCg%>siP#y0LVy_&{)00UjY!p(bQ@pPf|33XAl&8*^`w6lihhqhCYrNKp`&L0eKj>lZdTsQEe-f0Bk2J$Bcq=_st2w zLuhe7=SHPUtyMWcDs?y>SBkC;ITs5Hvrhc{I!X?^>pn@WM1Ew4Lf*J-l zndYv^@kV4W^dNgQtRUZmoq9tFDduwVX4th)F5z;GWkxEc7=oY`+d1Sz&Xzbx^U1XQi2#lg#FM z3uzv8Z)hXrp%_o?q?xNnZ$2ySP=CiyrP?75V?-N#x!fyt)fN z={WOU`?^r=FwI~Jv8tZem-6Ky1KRiXb=n2B;mz(Z;uHA<*Ajc?v$6eWE^B&{L|J#$ z_$@q*Tg`?*&{~wb%87-l{E=Mht0A(5+B-OkIhZ81A<(C?qibLcHprS<)h58m)Y^8B z35jZBnf(!cbTQ69OZ11Xcwyumi;tBzx<7A~`%DkdKm9@DBPjDeC>aLo*REW7E%q_p z|FP?Oy*TeLZe?ifd=1&xN^}_4m`cMyqWmPObf#Gn3Rj68r_&$q zWx~p24i$iLE~et;tOL%bv;&bQcg!n7AJwfAQw}TrEnQbn8f{c)Jr=^5 zF!)?7E~cUW?X@E6YKaxAqHBB`t^*ScK;c&Bi&P(ZQ01yFc*bo5AE&ZNrQ>t0bzJR)j z%Sh7)BqVFz^yoq!F@gAF)sGlSOs;O?G>4Zj0w>cv1HOWpkeaeMK$}f+cidM7=T-cc zt7_T~fURFY{dI;`a;-v#=%}JJm!ei&|ExQ{(ic$ zsMFKTPPPE(SW4c^qwY(PmOrn9oOD?<&8Cz9o8koM&PjfqeWou^LWGLTkDl8MF9f;2 zJbuW2M#tWnnTkC+^e7oTaRuhAL(bPKmwDNZ(x9@AKlX>RnAKz>+-&-emm(NBH7cEd8-u~KMiqj%+ zfCbQBwtMA_Nq~bs)$DwoY8zl@z;XS2R@+7F;^Hz<_Hn$MQ`?TiLJn6|d=Me?D zarN)K5P^b1UwRey_a++Er&TH9Ie6?VBf(60<$t;7n%_cu#+(n7!Xlq+KQ_ypp0T{h zP}MM&z}v{~-@ie)QJ&T^1%@KJCTX$TSQN*lg(V|Xx^Z{JNWhQVA7Mk;lXOG#+VYD8M6IlD2>JZ_^rP}O>JaIS#lcdQ92#O61xM#+I| zB&b4}KDhIYI55ofxQ)%qK1No?^ar0%i`}DFXST*_wi^$MTz0fTE+JL|u)P=(s&F2o?S8=eq>QJ-#mUs^R))4vN!J;nKoe7v4K&&Q>S-1f&h zhr51s(dSjdS{|frp=owqpfGaArFcZ;GgZ>h+iW#$ffW0WmLJ#Vb&CJ4f8P0z>C`%p z4ojg%_X<^?8YsKXrEnhS&bfP1yq9m|GcLvIX8SlbM0+~D*1|$Fsg=^Z=eHL~Ml%bX#8X`}C`B3lA@SUBD{lb-I4l9k>hXLLPspq22g4F{4fi~-)G(9=aM z2=) zQb9fuATAr*i}p;qo{ZHxF5fo?2T^XB1ag#+x8D!W#c3@z8)4-*4Eleifi4en?PB%U zp5JbZPbe?GXQc#EH$2pfrJ4LCEb+avRo#^|T)t+SDiT0f0-jCSGTST7? zZTGTQuw^e8a4yrWm>RBJ^YAHA*t>EPFW$x}9C+Up>0X9#`;o%@M7~;wZddviHKeRxlAYbY(Tl zr=<{x98Bg<;AKU_dDQgI>1G&Mb&eNWLXG^EGq{vfOaNoXe^K={Zie4Vd)1-yg~>)4 z<{}+yT*XM8Ze|RtFTL!1&}N)`GcIUZX4u)B7Ty*g&5QXe?!e1ZYUNIu+*=-|fy^1z zIuuVH1${c3^|nFicF|viqXW^IZGmLO0~o!oJG1$O-=e1x-~LEdIv2Sfm#H zRfKF6?Kt1Tuwk=(SJr34Ac~7Ir10BcqE@i0ACtfu4qeO0>Oh>-_tG69k~11rdsaI# zALWAM+xDSK^xHxG0A0IWd2>I%=fWkM#|pVm?7m$AJQb7fb&2Wy)Qx4j zc<&s_VLzu;2KK&`2?i2U&GGA^Ac*f)T)0%lnR&)PM-`=|1e`!l>G_^4qrct$hgMHJ z;arR-3sNCeGy5>V>~8kb&zB)MPx|A}cK??-E46cre{g?l;e|=U!YVt`yVns5 zM3cPK>4%b~f>r_NFAhf0Nd+!P93%#(qoK(bl4r_*M07$x<9FHKs|PUTy(iit&MoST z7nhN+*=-RArX@Uvx8HREm2SW66N|FIE$>4!XWxqA8XG@HrO^0-PXMZAu+9CU1JB$X zR^4S1PFo`wo-PYS8M;ksh8zrREv>_J;L4P$)~=HsUOMBX>q;Pd@S~2BotRLy(f#Va zsG-ERxSru30&CyKOg5d0)u5X?uwU4!15P0rQu=kNa}cfCJiJ zJexknX3W{*?lT_T`MIdAHcfwnbZ^;lE8ot-gZ$jf3jrHH$F2=^0OC>bwi(1sdL=Bl z)qdMc@ps?Qpz75yrtphAkVNv>PmA%5!QbIe5X4=ya~yZ#<;EfTJTb18Yg3YiZ{{DK zbY5fsjlg7SwB`W7=^MrGy7;Buv*D~)S$pC7+p!;9z{1W)Wye)L^axtp7S3mQtzq9psHzhg@0%@S$WImocm!JM8KBU8uiHZ~F zO`g$&X;X96kwPdet)V8#?&bh%4z1=#QhQ(=E=;^ zF2|L!gftiX%Fh)mCmHXh=8*!E@9&x(&0G9Q$-G>#lwbx_%T42QoyC#>=jjkB{Iuic zB7UxuqqG~dT1S9--97ts)KWNDk6L$VMeww*ZM3z|Pvh47&x_8(l@Mpeq(P-U-n~`N z_4@qq`k3gQAu;95#UPSP|I!B$tz7nRpRH%Q>;rrFhcG{Af@W_b_~>QZaV+E)hs^F# zEY(Q95kMNMmK2}OPD( zY7vdTq19kSffG`ddy<#H=mRri<68cWTC0I9ZLK>~zHR^l{wgZ@!CwVa2PJY?4D(^c zA6}U)L)KK&Y&|dZBj#G>PH2$ha!P%*7vv9Tr&5v}@5e47FV*E=+vw%`3o%(JdcE8< za*Z82ob`3p*;`n?RyZf?QijdZ-fl2#;!y|lCeRpepdu{i%gVF9@-U@@e`OfTct)4- zO2vZKenT{$o*p5xHQRH^yj6R4wg(&go2*yEG*|3q)?{~TNH?WFF4IN{U=HoA0}(Nb z@0ay*@&A^%Q@itDOPs3plxY75?>p$K7Eqw}ZHr!Ad-SjSu3wFGomDar7ai{yU8EjTdt`TIziRO(Amw5g+UPLZ~m!RZS#St4^E#yR{EVi zb7TEm$gCQZ1v123trZj@_uq8M)$sK`R2%otPqd1V0&RPAgUKU}q_if_FWWbr__uXt zrRZ<%Kj3ct<_%iquCG!t{&u*G2Hu;*cYk|%YQeNB{v6W3GF1}_>tojB$Dl0Kuam6# zy~!X$LP{N^ZyrDx)AedWy?Lbe-(sSYXUn%4pC~K0LT0N-vlryt6_c;?P<$8-O`1}1N|5;0={MhbW7xhllICpmV(f4-P^Ia}cpHR)ueUt`$@=+MR zovy;oD-Qi~#xi&U9c)@SF_X=Pro!fb6kUl3X?9DX5N<_I`MG(Fe%GOwzWB%0tHFOp z-8glLB?!keL_ z*mIlvL9|1Z^=k?hh%v==R*XGOaRtSESW3NPrdcxHzdHwtg#Pv%Twz$eSlrn_dfIOj zZF;h}e4>9wNnea9DmiYQfl;D%CkDz$>2VjchjRb+B3hNg4UY?+Sre*pUdK?8$gq%- z*`9*-J<*S@os$K?>1+-Fzq^CTb8`4&)WGlJ8|g@OVuK~!TkIko2zY+; z3}^*0R-AK~$7G}yp5_9j*RjPi`tiXe)!?^s7~r(|)AQdYTX6`V(zaJ5lyCT; z*LsycTK9Na zxSm=9VbwqWpT=CwS)DceSfMx6g;a z43PTyK3R|Nbw35X6_I>DwIwwD=Xc7mTGHtDn``IO^qh;NbISh#K09b%0xPNJE(4dh z>6i?hN+EJ4%x=)62enM`10%Rf8*n8btRYf(Sz()t)P!u$V!S}NIbppYEx*v+%we0| z6F(;B+4pQMf@tBa!W(HlgF)ZQsrv5P45R>&GVLr?DEIeYYk%^Zpa4tW>jhrB1o0h= zz0J93eVI<9F|~9;Ym(q$n9Nbb=>p)OBHQw>|3l~{*y;?}dQRP*qeL0%5zNXbWj9Qy z1JPWRHXb|R-TX68;HqqeKI%GcLZray<- zh3EN6rW(sz7ss_D^FM0?RexVs^*n!H3T5OZSnp0{7G5>h(dF&AJfjsg%7anzLKhLM zJR6;*#$sMW02Dti@UE~;GHPV@yo28EL&v-hsFI`kn{AD~4Fwy^Ygn}~us<+OdSb~FC-c|B}T z{zK~wC*9xwPgO`VJf;hcGqjR+FTvgCZ{!r)*z0tshV7%-DjXI4x775T4+wu1REqJp zI{woeSd>A_!E5%Z(D=dejmc=Rj1xS5N{<}TTS6@H8gmxvhOfIA&X;byYX`1s+BT?< zH~ISOt`_pVpTorWbcfE9v#c~Su~R?P;UJAR*x_r!@`5=sDd4@IwA}h4QS`$RH}mzk z7MveYI=al0RO*m}ruW>)1By5ixsEeg_=TZ3ci9~~Xk`|y7d4T=G=o&$h25&TTwWZq zzHD^Z56Ehp>-VBjLefLoIvt0#U~%aOV&wL5t~DGg7WHX?I+gtE+RaXJRHtW}l4*;V zdU)}`S;^kOHs1te;+S6M-)K8dC&?CarC4Ys4Z_8*J4(v-pP>8E+rr?1lWW%8ah}&Q zLMd&cUoD(&q4qw`qCj(hu4?mO=l^zG4PVLBZ;G({zP#!d-Wwx1xAWEh8aS(LfAgru zQ@42LlYCq}yTO>)Z}$mc1lWXU;p2Q5P)>)HN+I~WFz|#X;D~}tnk@e4*-aOAll%c> zCpF}LM(2|s_*o}Fo;vMR(8btoH>ohal;|gHG%Wqq}X4(D3 zn5^Jssp!uaUD28c`dD|3rxfL?CG^Ep`!}@)p@H{q3|0}?qu~WMNROkw>))56n3uOi zF{46Y{nYH63-Dw$gLi%F*5@>edrv7pAxe||19Pwa!>dp1Zvo1ah4Yl5(;N_g^&!8b z(eQL@Qsd=5d~7x{5z>oHj$`I8d+c{;apOID8a-4vrW7XItHe^ zeSx&ExDWP?s5KarA=6VS`mK+H3;q7|SP;}f-8@lvn@A@9WTXAn#hT#v^B^y50(kBn zby{k}bfwH(APPBD+coJO{pzO=98h$kU|p_?`WF9jbz>v^44;TE3Xy+1}~g zD6zBZ4pKbOcYCdyW7=%h?#w>zzB21?McWL)gNFK(+pi!ygHmts?pyrEWL=O=70O45 zOo7n8hRfH{A87~KT{$XI{zS%N&;e>fU4+_5vD9lZcd5;1l!;Y%P{DP)Eg_X*?b7|~ zbhg5@Iz7p1zCC%<>_IV!fJ*aUl6`8M+pLeo*DjhK2Di4U*u`b#{ny!qQk0aH zOY6pT&Xili$F$3z!R*+#Za8vJhHulvX|a{TR*JqF0euT6+jjQm>5)k>73q8z0(EHZ zyTik($W10jvb;;>dfyzvL%d^^#L&pjudSt37_+&}&YcPhlyq9C2R6^0lHV5F?ol3K zpAz)9KDakmoceZL_u4q1f$sBHjr@4>HFWu3E&jBszhn>Hj^qZaBicozmEsDm0ZHDm z?CFy^N%OTp8r22$d~oe?)L=_!a~{_zwa<>r=H(&mXRQyn2lW5@dw6C$_Nn5b>H<~Z z^z;J)6g{j|MMyXgq%{5-6$p}j{nmKwZ@e(5CbOBDW|AAMHv8^G07?CVG0NAk7PDMD>NGnQ=u@f3YA7irCNXO{fjO=E?gr~xXEVAVFI}n zcBhW3BHx3RfJQ(i%!qhR!Zy2DaL%W!_g3;|^LLVShxZ!5PKjCZITL~Vb0&f0VLsjF zPaM%dhZWxL736B=>K0P9v>l(%jUIlpzuhYM9;aAK<~iV2LF((Fws-hY{KH#RtkXdp zHHmIB3%%6LdOue{Y)>;TIi02P-}3f`gz{$4dQ=)zkiL^;k@Cv!rSuaITY2%$)wfIZHAlsihvGF3c*|J-DI* zKUOtGSJj{KP+O%IifqW)LaaM2(EiblWpxXGXX}0~ImcKbWBnGpj>W_^mAbHpxa=d; zV{5(V;tsF9vlHXWcV@@iyS;XoXy?)IAGhzKKMee)V*Lu}5<|pygPhEn{k&%RI}MueSvc4zOG#L%n9h&5hA7Oh;7($uO%0HnRH z0RWi8n#UvyS3J#Jy))Vod*5mxKjs_otFSBgH^2UdxIV%5 zvZe;s^!u@qa(>P8dpBJX(!kovwYbLjP1lP-yMtDHTip}u{qpK#A1qkr9N$5=rS7JK z>6uN6>?m(>PvBf#Y+mX@b6$12UxR84s-1V~=Yvmke2w4^Qn00SuQ!JVjhvH*{z1Jh zYz_94ztAWjwZ->3nZk5RBYqDud6_;N8qJa-#@waYH9e~;cY^rWwO7!Yo(3kJepPZJ z7d;5pLO!LvhP9deF6v%aVKs5Fu&YB{ZcM8usysE0MJ;)Yt#_ZsLRaY<$SHOl%ywA? zA%}f%;)dbk*#KwDp)t7lM5fB0P8zYU&sKD&mtP{&zxt}je%Z~Wv09;dIT8jf+l+vg z1^eqYV%3+Ftreu_Gd#3sz0$k)C=AEsJ}(Y^>afk0GZ3TR+`M!o|WpSByLf=+ukTI=4sd{d3hSV;7F!i=mL3B z*mF=riViefP_MV?@ zuK0@CgARn1=~zj-gp9Qq@zQ;hy-BJha`jum)~xFhyo#;CFR0(V*PIL%OxI0%eELO@ z8o$gYsddMx=p?(1#qxoqui9>D2bp*>`m!faDlVwMm}uEIv1S}zhEv*H9!YabpK5Z8 zFRJrTe=J^UhH?z!gCf_^?u)Epna_IIQ9?M$N_&Cn$%COPOZ(#dJKECCmc;%mLyZ6mw z>Q#1j4_Z^xggEL{26AUq?#Jo+J*kLTWzmoi5Sk0SMSn%Q zQhVO@x@b1*u}6$|Hyb8bqaZaB^}KaIbm)5eCXe}Mx65u?!`>|W{L667igPaC^Xu}Q zxpZ1k)moQ35az$A>-E+k#%rWGmbYqmTJ_h3@N>l;fIyz6m*-q&I>S|T$P3FEQW4(Z zcc$w@=%=U+Ae#O8<9AgUuzk4>3MB+JeQfUBup*8DZL_(rP;Ox-_RYfwtC{O=ts9oOOENm@>6<1OJMy z%34!AO9FE%>y_}LL2Pj~s$PuMQz#qj$h-e(*@v=z?=wu8&Q8{ZYpJQM6<|SD*}UM| zAn*q_cS$K_m@gVMIlr!wwV;l(H0r*S_nCVSTLrY<#+kAB;9?bxoJXsjypS-nw&97k zfAW)y{G-L~wzRvU9P5NS$JAchu>R8Z^WVr03e#?Vlm5M9?7?urTc}H6O@WI0K=C_xHfETMOuRuQuz zM}@|<^;ROnSzC)_dQxmk4qwqD-kLj1Dm!U^3rLqt7!e$39Zc~5P zilZq}WD|g7%qFYf41$p7O|R^W2xy9XH^@mb%D2Th#(G~el@@u|tBIzKU)Fp7I~&6c z7`Ju#dW^Y>IDZm127rBLI#g=UGGekQbxddX|v z8cg95QMH0ojgeBgbJ)naTOBH`na8OK4Du*rhCz?3*yF#!^h#*Pj`-_kJgw2j~jesYx{>wXZ&>BMV86>4}hbC zxlkUp+T#wfoK7qK_h@=Q9)`;`%`rc{n?rL~gW%)hM~slugV4uU16SbTK|xup?vm@k zEQwJje<~34=+A^#(8_7zfw%{I>Z$PPlT+BEjy@y%5PdI_A3GH^H(WtoLg}1vvW5Qu zq&At_i`k!Oz?HMy?p(*u@hG!sOn1oS3p~ zPV)A*xYvf4Hu9E$(M@+&9ll{}V)*wTmd>@^RB#K!-}@ItgoKi_C{d({P9!PE&;j%R z@3HpYpVzgnYpyk^_Z{PT?i-qo!D$^#LLpwRuN_NK4Od zoa>#^WgoGJRQrcg~ycBXslV_WIhPRoCqZLTa>W*RWruAxd;WI5xT($WZWbbRA z$(*A*p$8mliLw*i@~UcZz0m=ySBms)-Vn9fb~P34A!5r|slyH$+3xvV;miTh?W=|5 zu(^W+K*@e=2FY5_Rm#;Mn=A?$tF4wG{9U<>HE$4+r?JsutThMjPe@)&)m=PB5~o7_ zbjeT;+sc>U(-F)Grd6THRrbweQ>=i6>2$wvJL-Jj{x#T6$18wq^3aP`LQPiKe^LVA zmjf7}3tSAKm?7~VbIPZUo|~c|A9z-wnG3Uds=lG96*#t^-J+*(esR0QN|*I%d8hk! zX5M)G_b}#%zajZrNvCT(iYi2J0A73Ke#;+cX}UPPYV=v0r-{Gf!w3Foti0$K)ZqeC z&L#KLm4?H2IWnXOo^F=?QWhp}66J?H=&GW-$Sqzk^Yi6SEu;elr~HyfNvg6Qq%}h^nx73Tr#`X6BU+(7?3#aNb3VknxtB~NA6HE{! z*yUeCwG*#)E1XLJ=JN5&z&h&MKGs-^MYa4;Jb=;)~3EUMBe z9*;HVj7$ovYd(<7?OBf0Q*W@oW*9`_89;JIF9usYO# zX1Jusc}K4uMnZeL#L98ipF{-G<*5a~FhOKeo}-61219)=S9!ere;B;!sjd#UFSp@W zJ%vg`tvPB7Ie>?|YEbbo49}%R1;ff#-4D)A)v`+lV1C76vh9=<%{M2T#jb%sHBL#N z<-jm?&?m9}VqL#FQAK)8Ys7t(Da*{;bi^0ie3M}?j||~>O&5!BEO49Bq<^Z}MXcT7 z&s#Xo(%qg~MxKLp?lv;IggNNm2AY~_165d>8E*naS@9km*q}W44r!)8PSlVJc# zs7@v4L?vcffzl6#9f}{F!&gI!e|kt=V(K6W!Bo$AT}tk_);y()5)^6Xg~JXQ!&#}3 zZh(O_xl_Bau0lkBxnxU4tpCXzHi|w=pDFM*&fnR}=r-szjwFO#w{885<;#z7Tq8y` zP;b?xkB&L@Y|}Iw2Ybje!ka5NZeQrHzihtE!<#2uUaJbh4Rfz>y5|a$dR^WaH|2`N zGMwka&TrGz4C^+jGs)Ur3};N?ikg3iUb+fioFsn?-uW7T>h;#fWgM5YH-YVTh7UUb zXUG+WQSWg)CcuSAf;h58Qj4N3Y=EZs+@YMZSVzALRxp%Irjs9To(Aw6IZcS|huAsX zgsJ9^DtQL$Ra&3Wb;T>7Ke%tre;w@mRM@@79H8haQv;ac2U*;Tf!VAbGt@$x zRaClN{84$gG%-s|D?ULuD^|CUu%oO%?Jyz>bHXU8b3-U-8Eg(BlXuI_?chUfT@rJ8 zd5CL->q~@MK3r3Kii#%aXoBYYrPlGOFME^4&S9ENM$BPs|C{EiN;zltc8)Z9P(*3t zL0$iK%%yt8)IYO5_E6SWV5aTvEEJ#~P6jY(#Yk!R^lb*{EjT6x2TwD_)%qxP=a;M8 zSteTU%kCPA1z^+J784Yeg6v{XeLgcgIn14hb!&tej8Jof=E4~$qTZ&pR;R^H%lPuw z1k#8yDmBa<9t_?Ry}5PU%5vXdPSXt3Bg(!#YB_hT#lEj&YARvmFZ(x>en@G0-xq~K zwiTfvMh?^LZWUauTXkB`@}XNw6Ts0xLbDq#iiN|#`;5{XgVj+0Ymn1r^Lhe!JF5Hq zeIl~lpSy^V%ZaA?*`xn$$(JBgnlf`PtIxBx3Cr_gY8*T5`gGst=GV>(I9j-sQ~5XT z_O`g=-^o!Uar1nul{JD_{u)88`-_Bw5YRBvC zruoJ<+b$E*zIYTG=h2^7SUr2+_Bi^D3u^|X3Q5%H&mVUFw?UToQTK2J1}rtvk3(4M zchB>2YuG?%<}5CQbq^%*Dp4L*H+EWgsZv}o)nA>~{o>q3YJ0um>KLf|xnjq(GIkIz zk;9k<`Z&zZnZ^E%*7wld#&qACR`7h7nU}d2Vb~dU{GOFz!T>9)?c!D%?SI$RA32ym z2IYm1Wl*vs0_@m&%ek*0L@jA!=2u5Vo?p zBTZfEZ}iN(_)i}W`|EY@ujV+TO4gtJ^47c~-9XkcSD+;tz`e=B%j;eA5#LUZB;aVGM4`yh-((H5_oJ!cXB9<{3GTG%goexUE5239= z%V($4^-$Fq;T!||>U*g7UTHhish${PGskK@t4!=>hAT#Y-->C5uW+fw^_5Pd1?Gm@ zH6{~p>=E}m3|}_lwK8m}x&q@hrsp8I^e$O`?>q}o?fH0Q)5i!qo^$$fqLea(dR|oO z?0zws_46E*gR_+vR&+<1om8ION0(1p-M0{PTJ7TOaI!Sw!NM&CV#Na2u;~q*_f>}N zS3&({2RoM*zR?q;_Zac3em|(?F7lEHj20~}<=>@!e~OYbw7UEB5N$>^Wx8B80Kdf^ zp8P{ls(8}yx{dqjJ{WQGV=U$Ol}+)uVqe5WItZ+oQ>*=-@wX_5-^J~@!!Wn;7ogXE zquNt}SgWqr+iG@7-L~~usOKS@N^J!}T7^4r=aJPu@#vw2dYijluHAg^<|*gd@z{pU zIyJx5j(Du^%YVE#)30O6A*;w@D;!4$^w*13dmM2<`RUy{!FqXXewW**+=TtuyR^H% zfR#hp*RH)2KC#uDa<%Yj?~1Dv7^%ybmaw6g1b}T11cDhaNN(iW=HiQfdXI6q8nAB| zUA(32CkNVPv0~cI4K&sHxMgna*tMp!oD`8qxuqV50}nGtBw)e9+h20$7HWhLIbq)~ zU>Z>_v81KnMxDPyXR~dc>%%}k40m4zRz%$oTRGtuw78Bp?5J4AZN2N1^(Y}KBK?h_aUKlUN{b|0F`B_W{OVy~Y-(3nU7~Agyv>B^Lx+9$VmJP@|C+7Uxr(heUJD%5s$MBcJhVo#vJSg zt1vm8*=}w}#&>CFU4PEcZI-j#NTZiQJFoNEVr@yJcj-cPhQA|{h;4k785B9W7&|rq zAP;@}?9GS5UM$A^l0<;`YK}xt3F0sHTEe?eM<&*iFN<>kJpq9XNw{6s

z<7ww|c%PYXyLA8y#mhjH26KhS%L^ptGkchd(JcWubAXDud3_ z>wk`VVZGjruh~v`S3|aO?l4K!{VuAMLR0G*WkYXt?)4#c>=+#BTtHA(9Nw#9S9&d< zK_~S8=JH^4>1WcU&3Mszn18XwFPNj#52eq<)ZtTUpDmS(Oby=)`e4&VfR<(8!-Kj^p(bonX?Q0du4rj@3vIAV$rdS zE3;K|T17VZTV$S#r9GgEcDFu|!Zf1S+HduQolI$~e3(X^S`GA=8>hSRy7M*E;1Ata zF`U&)o18l6Hp6niTcD~Rlqai6L2ntN+43fK2v?<>Px@Vt7e+Iunc$$8sI z$6hVxD=p)$zujKatG`}KHvUx8)_1+#(1F`w&yS_752SF4R6Ckt36!{rJZS~i!xEG^ z2fccaefWp1c==PJaY%&kEcvKiyXagJjRP^XN?XgjNJK9_HLo8SZjeK%#b#d2Ekf9p zxCE*A{(5f6hu|&bm1PLl?t#30{_VS~Vt7lv+_}`B#5J3d1`0WD8221EEJkNLt9RRL z56kg?$t}B3ylh&WAhXN&bQreH*bY7`9J=b4t12n6@tIWa5_^3oPeaXDCvPuutJpzY zyiVZ;pA&%Mg4d-i@=EiH$H$Ja=@0HfBKkoBX6V;eA< zD1LiNs!;xog5uFSzR^F$BQC$s0bG`LT0P!t^uG2DVQZc&2>BG25d8F*pp6TAI8%|< zxpq50(8HdX*guX7DAX@^A6I_NIC`7zUHYVI3=G_;3EU%vU_%PC%;wP}DG85Wb zmE0%*3yK@l&n3@?t?q$XAcAsQw7lKm3R_SmE>;_jOhpWszf(#4+bFKW48>znUW3En8U|wF@t#Z zJ`-6ps{`eq?Od+YMQ%H!L<5z-kHJHq3h8o9Y?A|~cH2WTjNikeBznVhovR?IE@9JN z9zYHt1QDEZr`Yxa$oUovedF$E&P)6|U|Izp@)uC6e~s<|Bi@u+RwlwW-aCJtO`V@Q zcXFE+j9O%75oVaV0m!DzPq5A_VvQ^3&#CU9^LP8hh?6Zvv=<)p41*@r(Kk(MA z1UGEwOY6HE-7utPb6l^!s5W@kQAsnk0KKqbLWa)s#*6{Ufnw_C|e!TXQr zrg#F27JZHu)~Q+kwy}kBa^%>U8Qp#T%dAf8HtShO+Ke9hdW*DK(9rc+^6;+Ra$ioV zf#GMNsH=7#!c^|!9;(iVEI)Ds;jzN|U<`Q71i?se(fX5lFG;H1?m50IfecRZ$(?{A zt<;UqPOFxg=IljO5EK`U@LPdkx^+7kM%14y-VV>&-0JqrPDpWEGFjVf2oCY!Ivetr zcPhP;mvyr)&Ft}q&XBDJ&H2R^NH_|g{}!!}kv zV1Ky`_7P#h;c5Z41sm6Kx@`}X-}KSmUo6!I4O!Uk4&}ER&uWi^UF=Ge=DSrWN8x1r zyW=;`VZxc*mR6h5onDORg5(s2>JuLB&GDVH8ay@5tomhQM&&clY6Bs!kS$BCl_udS4P$W?IJ=C*A0MlB@jG5^#rp^u z>+)0Br^0?@kJ)AaiDjY|IlWggZ7k6ad;c5PZ*4Wnyp4)HDW=0wPt1%jCs|dOetkns z?(UBKbLiaoyh!)+=L1>0LT2^YW>!CXzPb{##>mEb?Ve#}ZQEhot=66j6! zKv=n&k~uo(0|h@p*Ya}EYWPC~{$%0U>hI}p2y zS0%LWE1A5?Hz~dA``01Qr+{v(l~z{g&8_#mu+qi6o2&jUc;kV@22j3!^=x9IOarN%&M{aA-MJ@UM zMFgCeJ2y$w3cR&%!9%fl*sjCIa`o`_+4mH$N-r|5z9+D<_qvD%uzsCgQaQHj&E{p- z`i0qMYI0IiFZB6^s-3_k{jQ7#&HHJ)gcC|ZT=2x@nkkOM@ND=0F0?T{SxuW+it=G= zAhu*0J-v{5=3wizZ~IEQF9H=I{f)!?YMLtK(>vOg&f;)SK1Cbgzl3tJ0k!dAkIM7n z?A2gj3=2j&|K%XAw+oPpFcl-w;;2rEI~uQebl9O@Tj}c4brFrp>wcdp^a_QqEWBoK z=q8o=Q|KER5xkl^1O1n3Y4fVX9C@=8L}ifuB0a6w!Y+b4KCS0u<9W!`kI*CQKkl7k z*K`~A-W3w352za#jyR1KXA@y2maFpqaNMY`UCmw&dzsRZ0>G?D(|?wX%sTx2F*NP< zQoRl?PPAoov4wuV-IrHbBsx!ngwK{`cm%+vt}Exz$i6@G1((%-n%fOO#qPB_t~1i8 ze_>9?-4x+Zar^55m`zyDO0z>@N^SDh1*CU5VN&Xz&F8h=Xx8W9aCEar&6Xw?pQOJ3 z2lcW!`fBj4Q(Ebuk9@lj+Kmf-!*_u$WU{66haSu;4qux+e)8<8PcHB1RR^w%GtD9F z9Gqb}{`k1tc%gq0u{h-X{qgTGR-uB(uetPZLAZJv4z`0MoIWzG zIN6-iiD_TIO?ME2RaP3jdvaA%Osg&D!mMu3>-C6oKgO80sKV_v*)A6o{(er*dHNi) zTxSxkxmID=m|C_ySVH6!mEH@znk++YD$}HE*9ST zk~hrVw0(dkkId+6S2Ipt9<7j9M}~@@H_K;-2PdM4+T<->MH2Qc`_fFT7K!%YhEjjG zscT^8cYzz#9@Py#pz!^cO=jn)wZ8n8zdm=iNVj0+4oH*xM;V^rrytpq+7?L&^wzpW8~ z-SmrAjo90@`OP7-^nB`>Qdn!v(lgBp1qk{+X^N!8{ecpH`pMyafpioaxQIYx@dB*Z%vY?XU^b7@$do&%9J2xylb~ zg@B)Ase<0Ec2qJ4Jm9|{ zqa)S#?0(+Jfq=8@MG!->zdK^p>55H8uW|LD?(enmsw9x)OFIQ0i%~5Hje$3?lx=@l-EG{ChU@6^ z0NBaILg-H))D?)3R8lv$FZuGdqs;vY9dl1Q*MGVHlV(Rc`g~o@Y;QUb4Q8A8uWySEZC45Pu{%)Y)!nmYnoV zdr{@B5-8{@X`C0;XYC*k%73jMigd7gFCgA-=W!O=>99gPG96V|Vy37nMEt7D-Icuf ze1gLWzbU;?0bIi1+?P7s5oO%r`0|;=4QV^dPyXl@lh2)yv5XfU9wmRQG@q3=XL*-j z0i&`aUT0cR=7&9@g-1arT|Q9oK@KZV?|IqU=Iw%w-rem}JPvhP-A=y+;&rN}To$qMir$|1 zTfftvtNDgJpO3nTzBhJJ=?yetvl+t2Ku2E>n{+C-rJDfRP1l`*^%t5n7;Kew>s8i$HQ7SRNVKc_HU}I8vE%@ukFH4dRCQ3>dl``+Req2|FtsRBG-p=btmG3iuHE|(91@E1*HTnPy(DHI8iS=g{ zJq`K**xYg47_0t{qIS0yqYUJn7k)kD1Os088zsSe^K8g_SE7 zKWU*rJV*A#Bu;c!)9?DdzPmo}4M5ULW*4?jEeqf`?@g(}7maVn&n&klIpv7>&)VGc z1A?q$FPUi(Pw9gn*@geTRV4S0+`RIUEDqbBa%FT}v^B!M82p;(FDKp8E{-$jJjdNu zF4NL#d*TM**;_I@@+1~Ap?hWe;Gw<|z=mcX1Uq}RN&}={+hw_*yrQ7J;?!^#JzBnYwl=d~P+vyMXGyVd{R$MrIMb5EqHpiOgw+vd9+^~EpPm2D| z$j^4>d0hGat!I`IptM-RP9`+?=0O>SHYg%2)z=VqG;D|PuauR-6y!K?|C|{6P-AL4 z`2!a*FdjYg`+y35&j2Lk#bd5JEG$j_g$u0riyKRG-@eI2gE~7%`T2eCV1Y1?wL?0} z=o5F(^q=xO7Bb~8Jxae+)IOGl?09Q0gC_s}@|9kRDiy@xco0>!Ax*uhgut<4JU!vX z@*dIXW;mdgiFg5P_#09u+sa~pA^Q6V3ioIycB4Ehpdk#B{}i(HQGUf{z6jnl38rVl zdoF@?>!W@fgh|8>cgMyE$JMx2hve8Ce>0uM^(Vdf5-_UUvR%G-m%KLmlzL;T(g*5yk;a!9_UP2AiEy_&FFy!pV|H+!*#Cr_TO7qnkR@Ll#Lo+nSO12 zSNOA;Vy0sN;!@vCL-?y0B{T}Pyfx=xa3=|c{42*Ab-Ir*^|SE~WR=M7zSF<>x$a`E zV}o@?R+mJYS@aoWeCX~v5I$Wg#YBt75Tq7c%gKo#=RJFWKJ_0*^259+JZ`*u7cin8 z-QDw>;DO>0m%F#!Ic}WNu)ee;J5Pn}I0(|N14sHV{`~jKW$Flyr9@ogLTs|-L-U&KjxW%Xhyf;2@x*|m*pI>#cA0jua=0;DZe|;Q@WycZ zID7Z1+a*6HTlKt|$(NkX!aFqc&4U{LAlbxl4o^MiP0A-X0%YyYtPlgI~BSj zvi{q>(V5$PJh;-V*Uh^{xx7uq<+Qp#jjvX{(ITpw=P)p796gKXa5kOiV)x{R=(=wG zfRC_8e>F~8UUH{(GT8o*232ZaJHog*>)iU=(r5n+0xgf~2LiaU?xhB}mHAboM~bHs zS0GP`KedG7(`)dnn5}!Yb?XtmV|;Z&I_ST6dyI67jr;{Z|J6+%D3$8)MYd6ZjKA48 zUMaWMqw%)0+F92K}J9O}o>6F-?ZAO|=w|1;5gMBic9e@#dWR zLn)QZH2%Lv?#WtAimptVbAE*X#T4)m&QGYPbc%=G(42;A@3S2y7{2k@2Y38Si=RU0 zystsG!fEdGKV>+V?ei*ulF2+Xe&v6XaFB7y-yXM{jO)z3Y}ciFd$I4l;Sm z`R?!=ix!D!zZK@@Z1M((`n-oc(~uSEN~1HsHU)^-_-RnXtI9IS9l2?{{KEm#hr1le zNHd|ra}Jv?)S@-bV8NjCPK+(eX}io@nDsq-*}}VZlGGdPcN(Iauy*z8Sej z`}7v=DOB1g&A#CLE%UNM9g68Y?i`m`ZrKQ3kU^rh3uv2uJ0F)%LdIcDsV4rn7iZlJkSgTYv8y~6^f0U#%k-dF$D4}%OqD)tKKN_0 zS;|>C`eURt}@#-yFU((awpg@6Cip2 zz{mN_u5xbijxSC&mL1ZHT~KyK5nNn|EjsO(dieEt+d1DJHGdooM<>S7x7}0pgR{Iw z&<&JG&d#<#c^_W)3^Gn4nx`_g>J6tnfnu-nQOno(Q~lVlA@;AWB`@gD_Q8gXllgI- z=+)?J`JLpLYQn8rHs>r|sLMI~jD%N02ar!I*LG_CrCRI6^9N?ProXAb>x$Vw@{b3C z_a>GYt(^1HduvzPDbvwhVd5Vo=cm228h5lOPMp7CYY9T<@$g&?wti@phWc8M@Adtx z1(kj~Y%cSOWN#|<5>j$j`^bThI9vUb&!q4YRn;A^RWv0=w`%d&dcskv3#X-AOT6L! z^6-k{T6S~hAzm5mw)7+<<2+le)d9%lpfDI8Z0Q_vZSFtv?p=-cqxu}wR9AMeoesqE z_zfJjbuRnXo7s*1N{ex@-SYy~WZ8$86ws$7%=e?8S;*Y=kaejc;Ulxqe}2Z+@mY0C zg=1soG(Yv%E;ggmWn%B2g3&7a&#T07`!@SGFwH6p5hPoZPG`R|wV<6n83lG|>%juv zjtcul7SBm#?K}L$sU7_H3kR*6kzrqJ`ddz*VQ|+5ClCUa?8;?3lA(_`LV-cIJNHM= zpTmJ^G5;!l?%8{(8n^NEGT0!4%Z_fvRs_-XePn`D68X0`HD;<^R(tthz^tsxjAldYrSc~!6l2ct2!l}ABAx7>y z63qH>%oUz;c|872PLLOYOe)qtNi%zQ{#&D+RnnVpRyevs-6E}h&<^o2f!mM^m`9;% z+!p$w=r;qUveC0h(m5|jGqCTbJUHz{-5bs?fwuXnQM303Dn-8swf^9;&cDaT3tgoi zw;NKKYtsG=&E2#;sb|W*)ajqdY5rcn+=j?zPkW>aw9UedfbGQ4wZhXwPj+G}dzE#b zt*exQ(uFp2e4iz!mo*)uL_SGU{TsD7IVWSU@8x`0uiqGPpJH1V+0K*O?hrQ)gIT@5 zo)oAOy#u1>qsIB_fom=o=u`f_HH^!A`3laTywGiTHpxq$ z@KUaQ%+Yv>ZsE@k|JuK5@rP^6^?O*fwAqO%Ao=`dsla=mTJx8koL7I~MAEY;iOLnP zT%M~?jzZ=3Pn*O9u}^Z<47eaRl}&Hi^I>C`4+;wZoDsWLUwsRe`M+FMU!`SpQ~Xutu(PwciLNQUhOh2> z8r+EYMMQGlMI?})X5pzxm%BbXK%%9?|1m?*XVSd(SCqKh_uL|i^$WFXyu#*oM>b?( zXmu{haT(>T@HYnU@t8Hl$oU>#dtYXnmuHEWAFZ$5%kvIG>ARree^wS+&IDCHnAoZC zu3eKfz5S^ana-$lB-HA^kbcgZpMo?`KKGpiwfi3fDS&ZUv7wyb|AE#H}{(c*Q(a{fhbJ9*N$(L zSlBeGQti{uFK2i+hsjW?e$PH7nbbb#qwD;cJ%6}Qqoeo^%0(j~8oX;Yq+@W$C77Oo_VH_dMbP(cxH`^>J*srj@SGw$780LnZosxRf z4Q|oZW~0leY;6&SlN9LEJ1Q6magN*ydG!M&ZiFeUZxP4zYmx2yk%tYg_gJuDu+qj`p?NJqmiSZ~+<( zMsI8c=JrExvEBMDZPAogyJ)mT22|s8W}fixd95?0w^|b5=>0ep;}uoh+(7TJYC^p2 zK!76}5Pb4Gn`Qb^4ZG~Ro5IA~fjeHx#@yJxSC>_;PHD*e$SKNe@pVU=!LOy^dOs43 zoI}_6tZ?WH6h5uI>sDv5d-YmT($DtPq`ArDFWiGn^5S;Ppsv1c(N9j-x07!EheYel zskWvQ_^ZE-zRCQ+?4g`Tsc|0^AZ`B{edTAh1l z)f=EVeh+fl&nYG3s?n7Up=A3jTYAoiePe@VE6x6U0kbrh++DxfhC4WW*i^vI%(&m) zws8l2@M~s781J7XnYU+FwzmM3#c=!m=Ofa+Uf55Z>VR@foenTTiWW2vbg)8t-Xbrb zGvC`w5$?MU(sMi&9d-_}2PN zB;+*28l7)-{MNbOx8C4$DfI9*k0{Hp4&ROqR$;z5seKPg#D?o{O@h8gRv}?fT)Wrw zzD9Ijjb;YZhNF7xIP4QV!s50hF3$x5ursaEnkX>!fdl~PVhV#M7Ix$Fz9+Bnm!6*z zs<#-^%_;94)}kU_(kTk3rRw`51)MKA&pgj$y9lm7jL^rrvD$?G=Ofx&fPmaoAq1Gb zCaGa8&3mbWclR1*NIjOXy8e+Qz3?sR7V*AzmIi4B+!RLBb*EO&O#r{n0nG1%4p zczNt>4Pp9WLpU$oWq3M$RVvuGwqRyl{#`z@a5k#@$-Cc|VbbF^eTlj6zG_6j2erBH zWLBt&mKWEq<&b~N0)CY5hwByl{o)_ug}$8{pVR{Oh&r#FFdpAjf&06!dx3{Gb?WjZ0s`D>A%wXQWfB;YD5}WuXE6;T+?tIq_qit~tV7C)ebIirJ3(WZ6T67xi zT6->8^E?nsvn&mtJQMZ@&Txx4=?)DR`p4Y7G^Y1=fB!|P}RhuN|KjWT2%9KmFNvTu= zSfbLK=5l;g2*Ih-uQB6T@n7?dqZ(@al7yvH{21Nl#$uC6i64O*;Pl$!J3sSjESfeo z(=coiy}~kL+_k^tJUzaK`>&k2@x_{S*GsdSh7xARIg8MC^6x&hUmJ3%U2F?_QQq$* zbTZ7L7eFwzak7=qyPvfY)RRSn-6Hw7Z>r9I+s)+$pUZLQ+spxd%ID$P?}i7lM*yN} zRSve{2P=1$&T@U_`X2NQ%C8u%*h{-ku5&@AZaw~T+4ChlAj0*SS)`fP@{*RM^J4x$ zZ!c@mq;myn{fnbVklt`d9F=Rs307Xq`OV=r`2p8-9**!+{v=4%`YffFxxbB_t?!gB z@+g|S%K^OaRGc3b;5%=;%GMD#NPB(Q{xrPRhZbB*W2Xi9A>QrGUcCK0a@xAR!6A%Z zN1MQ~H!Bd*hxb@pZ>LD7KO3fNv~M+J-f(`E+*b589&g?^5AeRV$_q-BcC(^AUxmQj zxz&al33SxndNA6l)uZ3wN9aCT;&V*N^~l(`BVa^St$o3} zbNM#9-|R6M<+Ii@>o<&2b-LVloooK8Rrz6D$l(5oGEBnV13&F(A3svx*<7prWMXDd zk`T)i1PTvbx{#^5N*tO#7hM6O>)Bp#_lBAZn`{9p%=A0zM!m%O8q9)Wpua~ZVeS0o*WW-_i_ zM)8IODAD|W>N~BMw^~j(^f09`I`-DB*Bs;6CSXhq9Q{C1M zPLPZmDz@>E>(|9rfhT|Me8fB{nrlw*jpF~ZT=Knq%_3as-XLp0r&V$5xBIo^Yc$(3 zC5@A0l;_;y_41kD@bQooFH#sab)?YTg457qU**=k35M5ljqwhn7q+?7dxI6%r&-+m zaigs79*8^U5+y;SvpU&i#69cTY0f#kx}V{tTmlb#sLpN(s?zDS_d$uzE;FnDH9Ehk zEmYL*UZ&5b`ug0d`MYn}HIjSTdD6-)<%8AcD-FkUoiFD}M^9cGD;#8Q$8;^s3knh? zH*si{h^lw1Lf9=#zLjU#pot_U`=u&MFb~vV6^uB3vEY&{>P*UCC6Z(!XRC*9 zazf48*vPz_S-D= zYq4J;nk_yBqq#2HVWU#`BdIRmgyR+wz+?Gs+8}t+By2LgFO?OpV!tdCVPK%`Sp4Y z?-_ZmqdG%tn!4Q<nm#Y&t%l>uiI}^ z_#{A1k!kbZ9T=W|R4riiE3e$LGH*Zo{%uiQ*62phObgXUsS8Uw?>pW#kzJ4lxCVcD zjgg<|?8eS;bXQ?<4wi27BJVRUK54H&Qa5yMa`o{3;=y-$*#`()^1hPsNRM?AyUP=3bVPPr0?paHz%K6iPd<0Xa^E&m(a8Ji#WcL zE3N@NIGznu*%MH^AAdF(nRnce3^prObY|<#vmsHVpMrSpf}N1pdBCu@lmRQ(`DrR( zgfhpUDQwpdG?%%S#`Nimr?%ZBtc83SRfs@*G}k+JCv+=|=RfNA_cAMh@g_}>g6SL| zzFFO(eo8_HWOp1*gXss~@T;q*m3PbXBGqsShuLmVeOG5>?>I~HvrT^^mLNbMb=cu+ zgzc(A?5|v-)J4*+Z~rc_lGiO$d}Fi5f7-jU?!jZ7ukSVoJIAQy@sHD@Y~b6&IR38(pbZyYXx!pU~mcHF~76wb-qo(aU(DmyS`Qnwaw&pe*?WIHDny+g{P zg@;v_ZNf!1D(sUVgiOWe=I&V3e~n7E7LLD7pMVn%X*BO0fsSuvLw>*Kp4mlh-wEurlQo&`yL~ckHP+)&f;O=kPZN z!pJc%gLfyXQ+o}pw0fN>I^jZZyH#o44P84c+LJQ=Ciq%>EubK$G6^)%~iLWZ&~Xu41#guj^?@6?KYR)xljp><%Y#I z4JBJGJw54^*=hNC<`fGrPaS^7Iwn-Eudvf)XEMYOQqq~a?|W?a=))|?t#kq0l|{hj zQMJo}m%?T&&qfcoH|w}P|InT1g<&0TSFV`XiQxm7gmXgl#TlC3j9T-FZKQ$HxH)gd z?AkLA)n^gwYtG?xH%2TEuBBSHVtNZ_4fnO1(j9KstBve!xLpXhdW;z0oBkIoxaF!j z!I-8h_`03yc$6FXUfd7S?)Z^%fm2$KkPOW9oVgwIHVYYLnxUr%T$#CE z4OR|uGJa}XI<3(%`>Iv)>6j&0da+t3V6N9_-LFG5-M_8t&T`EazA0~l&Q}k&@}|Ot zwQ06^d2%i{;hSbrPe-3@TsA@H{0Z}V0sioU!~1o-U)c@q(`Bh#=33s`E)2HjWAWvV zlODMeXP(~Kv(s`XTF*8S3bZr_bl zmWaLHbrsT%RMwllx^b{AMQj_JdV zJ7cZF~zJ zITnj~dlHGv6B|7~k=wms33s~@ldl6;QQRe@?WR{WS7u=)=G)aHJ5Mia=cC}vKO*|- z8)sCJ-HmNem$-1N{OzMA(&)`fkfMoF z6!yy&aeUW~IGX0F-Lr5N<9ds!F9X=^tTnihq;)4Xf6rmb)bTVsP}{9}mv-aGwA_9! zJ&smSb62c{?+T5f(;)f zt||-ipVph2L_{OeBLf2&L`D>NM2V7wIYddI z_(Q*E$RmcdB4p4_sFW{KWl-ZQD%-89FAvS$2Tv)@DO-K=1GU~rJhXdHjCUW)@T3Ef zNR#;Ywp7N-V6q(TtN-Xz(yjZnlXsE$m^KHNX{Gd41h#VjnDo_hD!Q-XEnU+G8)yzk zu?&yAEQ~UmLzosQUPc@be}zK>Nd7j06WHu35ejI4=6G8bz%t= zyD6SWEJXuvi5Qw-2{D+2kGkcmphV^tm|O&S#3LGy5p{y0!?-_;_qS>9Z5B8ye;Hr@ z0m_)`M47-dlq?}$q-2?amB$D%A_*S#;5W+Nm(%^Kx`*CT9}$%P*C@l+i86+vI7vVP zEm0gsSP&>IBS9jeqaM5}l>PE$k-bhRNjyzq0eLY`F|3FfM!=MeWR5ywNXbyVCKl^= zUKhFRgxP*wWN5My9@a&Szt=^wquTGY_CG&^`{J(?Wtang3nNa#6pLY_%94;klhC!U zi@)<+CR{bjSoRoY9RH27Uo)2-N%}s7I==sT*%u2!FA*3riJ>G3Ng{~CXvESa^gWmT z{1x-E-1vVUWf(YpKpEbn3@`^M1Ksg&l>HLE>`LYMEQnb_LczkNlMFCqf=n?WBcn;^ zdoKIwa~Zx;2|o*>3zk6~D^hTBX0ZJX;z1NE@ZTu=`6#aSos@RiE8_iTs3b-c43_?zv3 zsp=O99YKxdT_xw5HtHOaFER*DPYkw$GeY>A?f69~yHe3MhdHu^!Yoe-91~30Mu7_Tm1p4_y3pnZ`$%-xf>1@wqviz+5TJM2h8j;9)_X zA7I9?M0=6tN$4A9Kb?n##p5brw&!7u^%0Ck_B<@0?3;)E1yUc-!@5e8iK0aD67q@y zXv%^;2?Jq38%r_V)$(7z^O|`PWkp3-ms_;l&F6cT}XuNY4v zaH;xZi+q`dGE!r35zErBU}P^#vSNSq=jz^wk3obZeQ&~X5(X2euhLk6>sM(U*hGDm z2KG`XBp<&|Kn$VlVp~9q$>^6uR~?s=5JO}s7t6`8yZI^&r~@5>FA4&Az|MY^2o@7x zb`21Szw8^J&^VC0*jOOZI7hjhC?G@peK`^IZ@%mwAQIH`zaj;=^e=k|h{Rv^5D;nX zW-c}eh%|W6%ZV&-ZeKMBh%_dAqvVH8A$wU|RsrP0y)~N^8K(U|-r)i9=T@P<4BF~D z^^oNO4HfW%OM*vlxhyWZpRdM>VJ{20Pxd3ezOpwz+Sf}qyRrMvanT&|!1GYY5WUBdp+_FHe2*hGwI^{5&B-DRn6rEwFze@WJQ*Hm`2_mBtV|2A zD4ynnI0>eRld>``fwkOO9K%WH@0BqPW&>yWut=t()B0GJ9BrM&$>;RaGN|i*s*h*5 zPxbL44?TZcpTNQ<`8-bk+-8yg+-3=XZnMO*pZWlr4Ab!Gdu368adaF9R;(;ok)Owb zz596_1GZwH;=uU)JPz|@ZajSlY}C%a2Sc+W|0y5G!pX#0SplxiJCEZp#>t=a$@Hi9 z3N(H6J+`_snX9^KKdQl9=RaYnNMw-u%9^%C?@O64vm9#ZZQg Date: Wed, 26 Jul 2017 14:34:20 +0200 Subject: [PATCH 08/79] improved desription of config parameters --- src/fu_line_detection/cfg/LaneDetection.cfg | 38 ++++++++++----------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/src/fu_line_detection/cfg/LaneDetection.cfg b/src/fu_line_detection/cfg/LaneDetection.cfg index e051b314..ea367532 100755 --- a/src/fu_line_detection/cfg/LaneDetection.cfg +++ b/src/fu_line_detection/cfg/LaneDetection.cfg @@ -8,27 +8,27 @@ gen = ParameterGenerator() gen.add("defaultXLeft", int_t, 0, "LEFT lane border position", 20, 0, 160) gen.add("defaultXCenter", int_t, 0, "CENTER lane border position", 60, 0, 160) gen.add("defaultXRight", int_t, 0, "RIGHT lane border position", 100, 0, 160) -gen.add("interestDistancePoly", int_t, 0, "interestDistancePoly - to previous poly", 5, 0, 100) -gen.add("interestDistanceDefault", int_t, 0, "interestDistanceDefault - to default lines", 20, 0, 100) -gen.add("iterationsRansac", int_t, 0, "iterationsRansac", 30, 0, 100) -gen.add("maxYRoi", int_t, 0, "max Y roi", 159, 0, 160) -gen.add("minYDefaultRoi", int_t, 0, "min Y default roi", 110, 0, 160) -gen.add("minYPolyRoi", int_t, 0, "min Y poly roi", 50, 0, 160) -gen.add("polyY1", int_t, 0, "Y of first point for generating lane poly", 155, 0, 160) -gen.add("polyY2", int_t, 0, "Y of second point for generating lane poly", 145, 0, 160) -gen.add("polyY3", int_t, 0, "Y of third point for generating lane poly", 130, 0, 160) -gen.add("detectLaneStartX",int_t, 0, "beginning point for gradients etc", 155, 0, 160) -gen.add("maxAngleDiff", int_t, 0, "maxAngleDiff - optional smoothing when angle diff larger than this", 999, 1, 999) +gen.add("interestDistancePoly", int_t, 0, "Accepted horizontal distance between a potential point and a detected polynomial", 5, 0, 100) +gen.add("interestDistanceDefault", int_t, 0, "Accepted horizontal distance between a potential point and a default line", 20, 0, 100) +gen.add("iterationsRansac", int_t, 0, "Iterations during ransac", 30, 0, 100) +gen.add("maxYRoi", int_t, 0, "Maximum y value for points on a line", 159, 0, 160) +gen.add("minYDefaultRoi", int_t, 0, "Minimum y value for points to check distance to default lane positions (defaultXLeft etc) ", 110, 0, 160) +gen.add("minYPolyRoi", int_t, 0, "Minimum y value for points on lane marking if a polynomial was detected in a previous frame", 50, 0, 160) +gen.add("polyY1", int_t, 0, "Y of first point of generated polynomial during ransac iteration. Horizontal distance of this point is compared to default lane and to previous found polynomial.", 155, 0, 160) +gen.add("polyY2", int_t, 0, "Y of second point. See polyY1", 145, 0, 160) +gen.add("polyY3", int_t, 0, "Y of third point. See polyY1", 130, 0, 160) +gen.add("detectLaneStartX",int_t, 0, "X value used for gradient calculation in detectLane", 155, 0, 160) +gen.add("maxAngleDiff", int_t, 0, "Optional smoothing when angle diff larger than this", 999, 1, 999) gen.add("proj_y_start", int_t, 0, "Y position of processed window inside of IPmapped img", 50, 0, 80) -gen.add("roi_top_w", int_t, 0, "roi_top_width", 70, 0, 160) -gen.add("roi_bottom_w", int_t, 0, "roi_bottom_width", 70, 0, 160) -gen.add("proportionThreshould", double_t, 0, "supporter proportionThreshould", .6, 0, 1) -gen.add("m_gradientThreshold", int_t, 0, "m_gradientThreshold", 10, 0, 100) -gen.add("m_nonMaxWidth", int_t, 0, "m_nonMaxWidth", 10, 0, 100) -gen.add("laneMarkingSquaredThreshold", int_t, 0, "laneMarkingSquaredThreshold", 36, 0, 100) +gen.add("roi_top_w", int_t, 0, "Width of top of roi", 70, 0, 160) +gen.add("roi_bottom_w", int_t, 0, "Width of bottom of roi", 70, 0, 160) +gen.add("proportionThreshould", double_t, 0, "Minimum proportion of supporter points for possible polynomial to accept polynomial as valid during ransac iteration", .6, 0, 1) +gen.add("m_gradientThreshold", int_t, 0, "Edge Detection: Minimum value of sum after applying kernel", 10, 0, 100) +gen.add("m_nonMaxWidth", int_t, 0, "Edge Detection: Minimum width between 2 maxima points on a scanline", 10, 0, 100) +gen.add("laneMarkingSquaredThreshold", int_t, 0, "Lane Extraction: Maximum value of squared subtraction of possible start and end point of lane marking", 36, 0, 100) gen.add("angleAdjacentLeg", int_t, 0, "-Y position of where the angle of polynomial is computed", 100, 0, 160) -gen.add("scanlinesVerticalDistance", int_t, 0, "scanlinesVerticalDistance", 2, 0, 160) -gen.add("scanlinesMaxCount", int_t, 0, "scanlinesMaxCount", 100, 0, 160) +gen.add("scanlinesVerticalDistance", int_t, 0, "Distance between 2 vertical scanlines", 2, 0, 160) +gen.add("scanlinesMaxCount", int_t, 0, "Maximum of scanlines", 100, 0, 160) exit(gen.generate(PACKAGE, "line_detection_fu", "LaneDetection")) From 487f4e774b0862f0c88b9d4ea2929f34c82be9ee Mon Sep 17 00:00:00 2001 From: lars Date: Wed, 2 Aug 2017 11:23:32 +0200 Subject: [PATCH 09/79] minor formatting and replacement of the while loop in ransacInternal with a for loop for better readability --- src/fu_line_detection/src/laneDetection.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 11252774..3f8be783 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -438,7 +438,7 @@ vector> > cLaneDetectionFu::getScanlines() { (i/scanlinesVerticalDistance) < scanlinesMaxCount && i <= proj_image_h; i += scanlinesVerticalDistance) { scanline = vector>(); - + // walk along line for (int j = scanlineStart; j <= scanlineEnd; j ++) { bool isInside = pointPolygonTest(checkContour, cv::Point(j, i),false) >= 0; @@ -447,14 +447,14 @@ vector> > cLaneDetectionFu::getScanlines() { if (isInside && j < scanlineEnd) { if (segmentStart == -1) segmentStart = j; // found end of scanline segment, reset start - } else if (segmentStart != -1) { + } else if (segmentStart != -1) { scanline.push_back( LineSegment( FuPoint(segmentStart, i), FuPoint(j-1, i) ) ); - + segmentStart = -1; } } @@ -491,18 +491,18 @@ vector> cLaneDetectionFu::scanImage(cv::Mat image) { std::fill(scanlineVals.begin(), scanlineVals.end(), 0); int offset = 0; if (scanline.size()) { - offset = scanline.front().getStart().getY(); + offset = scanline.front().getStart().getY(); } // scanline consisting of multiple segments // walk over each but store kernel results for whole scanline - for (auto segment : scanline) { + for (auto segment : scanline) { int start = segment.getStart().getX(); int end = segment.getEnd().getX(); // walk along segment for (int i = start; i < end - g_kernel1DWidth; i++) { - int sum = 0; + int sum = 0; //cv::Mat uses ROW-major system -> .at(y,x) // use kernel width 5 and try sobel kernel @@ -524,7 +524,7 @@ vector> cLaneDetectionFu::scanImage(cv::Mat image) { sum += image.at(offset+1, i+2); sum += image.at(offset+1, i+4); - + // +4 because of sobel weighting sum = sum / (3 * g_kernel1DWidth + 4); //ROS_INFO_STREAM(sum << " is kernel sum."); @@ -1308,8 +1308,6 @@ bool cLaneDetectionFu::ransacInternal(ePosition position, return false; } - int iterations = 0; - // sort the lane marking edge points std::vector> sortedMarkings = laneMarkings; @@ -1360,8 +1358,7 @@ bool cLaneDetectionFu::ransacInternal(ePosition position, // save the polynomial from the previous picture prevPoly = poly; - while (iterations < iterationsRansac) { - iterations++; + for (int i = 0; i < iterationsRansac; i++) { // randomly select 3 different lane marking points from bottom, mid and // top From 112220037182c4ccfe3f93958a8496abd72446c2 Mon Sep 17 00:00:00 2001 From: lars Date: Wed, 2 Aug 2017 12:58:16 +0200 Subject: [PATCH 10/79] moved a large portion of debugging code (LanePolynomial window content would be computed regardless of the debug flags) into the corresponding #ifdef (PAINT_OUTPUT) for the window itself changed a few values around to experiment with proj_image_w vs. proj_image_h effects in the LanePolyominal window --- src/fu_line_detection/src/laneDetection.cpp | 100 +++++++++++--------- 1 file changed, 57 insertions(+), 43 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 3f8be783..29723c23 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -59,7 +59,7 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) priv_nh_.param(node_name+"/m_nonMaxWidth", m_nonMaxWidth, 10); priv_nh_.param(node_name+"/laneMarkingSquaredThreshold", laneMarkingSquaredThreshold, 25); - priv_nh_.param(node_name+"/angleAdjacentLeg", angleAdjacentLeg, 25); + priv_nh_.param(node_name+"/angleAdjacentLeg", angleAdjacentLeg, 20); priv_nh_.param(node_name+"/scanlinesVerticalDistance", scanlinesVerticalDistance, 1); priv_nh_.param(node_name+"/scanlinesMaxCount", scanlinesMaxCount, 100); @@ -362,46 +362,55 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) pubAngle(); - cv::Mat transformedImagePaintableLaneModel = transformedImage.clone(); - cv::cvtColor(transformedImagePaintableLaneModel,transformedImagePaintableLaneModel,CV_GRAY2BGR); + //---------------------- DEBUG OUTPUT LANE POLYNOMIAL ---------------------------------// + #ifdef PAINT_OUTPUT + cv::Mat transformedImagePaintableLaneModel = transformedImage.clone(); + cv::cvtColor(transformedImagePaintableLaneModel,transformedImagePaintableLaneModel,CV_GRAY2BGR); - if (lanePolynomial.hasDetected()) { - int r = lanePolynomial.getLastUsedPosition() == LEFT ? 255 : 0; - int g = lanePolynomial.getLastUsedPosition() == CENTER ? 255 : 0; - int b = lanePolynomial.getLastUsedPosition() == RIGHT ? 255 : 0; + if (lanePolynomial.hasDetected()) { + int r = lanePolynomial.getLastUsedPosition() == LEFT ? 255 : 0; + int g = lanePolynomial.getLastUsedPosition() == CENTER ? 255 : 0; + int b = lanePolynomial.getLastUsedPosition() == RIGHT ? 255 : 0; - - for(int i = minYPolyRoi; i < maxYRoi; i++) - { - cv::Point pointLoc = cv::Point(lanePolynomial.getLanePoly().at(i)+proj_image_w_half, i); - cv::circle(transformedImagePaintableLaneModel,pointLoc,0,cv::Scalar(b,g,r),-1); - } - std::vector> supps = lanePolynomial.getLastUsedPosition() == LEFT - ? supportersLeft - : lanePolynomial.getLastUsedPosition() == CENTER ? supportersCenter : supportersRight; + for(int i = minYPolyRoi; i < maxYRoi; i++) { + cv::Point pointLoc = cv::Point(lanePolynomial.getLanePoly().at(i)+proj_image_w_half, i); + cv::circle(transformedImagePaintableLaneModel, pointLoc, 0, cv::Scalar(b,g,r), -1); + } - for(int i = 0; i < (int)supps.size(); i++) - { - FuPoint supp = supps[i]; - cv::Point suppLoc = cv::Point(supp.getX(), supp.getY()); - cv::circle(transformedImagePaintableLaneModel,suppLoc,1,cv::Scalar(b,g,r),-1); - } - - cv::Point pointLoc = cv::Point(proj_image_w_half,proj_image_h); - cv::circle(transformedImagePaintableLaneModel,pointLoc,2,cv::Scalar(0,0,255),-1); - - cv:Point anglePointLoc = cv::Point(sin(lastAngle*PI/180)*angleAdjacentLeg+proj_image_w_half,proj_image_h-angleAdjacentLeg); - cv::line(transformedImagePaintableLaneModel,pointLoc,anglePointLoc,cv::Scalar(255,255,255)); - } else { - cv::Point pointLoc = cv::Point(5,5); - cv::circle(transformedImagePaintableLaneModel,pointLoc,3,cv::Scalar(0,0,255),0); - } + std::vector> supps; + switch (lanePolynomial.getLastUsedPosition()) { + case LEFT: + supps = supportersLeft; + break; + case CENTER: + supps = supportersCenter; + break; + case RIGHT: + supps = supportersRight; + break; + default: + break; + }; + + for(int i = 0; i < (int)supps.size(); i++) { + FuPoint supp = supps[i]; + cv::Point suppLoc = cv::Point(supp.getX(), supp.getY()); + cv::circle(transformedImagePaintableLaneModel, suppLoc, 1, cv::Scalar(b,g,r), -1); + } - pubRGBImageMsg(transformedImagePaintableLaneModel, image_publisher); + cv::Point pointLoc = cv::Point(proj_image_w_half, proj_image_h); + cv::circle(transformedImagePaintableLaneModel, pointLoc, 2, cv::Scalar(0,0,255), -1); + + cv:Point anglePointLoc = cv::Point(sin(lastAngle * PI / 180) * angleAdjacentLeg + proj_image_w_half, proj_image_h - angleAdjacentLeg); + cv::line(transformedImagePaintableLaneModel, pointLoc, anglePointLoc, cv::Scalar(255,255,255)); + } else { + cv::Point pointLoc = cv::Point(5, 5); + cv::circle(transformedImagePaintableLaneModel, pointLoc, 3, cv::Scalar(0,0,255), 0); + } + + pubRGBImageMsg(transformedImagePaintableLaneModel, image_publisher); - //---------------------- DEBUG OUTPUT LANE POLYNOMIAL ---------------------------------// - #ifdef PAINT_OUTPUT cv::namedWindow("Lane polynomial", WINDOW_NORMAL); cv::imshow("Lane polynomial", transformedImagePaintableLaneModel); cv::waitKey(1); @@ -1544,19 +1553,24 @@ void cLaneDetectionFu::pubRGBImageMsg(cv::Mat& rgb_mat, image_transport::CameraP void cLaneDetectionFu::pubAngle() { - if (!lanePolynomial.hasDetected()) { - return; - } + if (!lanePolynomial.hasDetected()) + return; - double oppositeLeg = lanePolynomial.getLanePoly().at(proj_image_h-angleAdjacentLeg); - double result = atan (oppositeLeg/angleAdjacentLeg) * 180 / PI; + // why proj_image_h? + double oppositeLeg = lanePolynomial.getLanePoly().at(proj_image_h - angleAdjacentLeg); + // Gegenkathete - oppositeLeg + // Ankathete - angleAdjacentLeg?! + // this seems to be wrong! + double result = atan(oppositeLeg / angleAdjacentLeg) * 180 / PI; + /* + * filter too rash steering angles / jitter in polynomial data + */ if (std::abs(result - lastAngle) > maxAngleDiff) { - if (result - lastAngle > 0) { + if (result - lastAngle > 0) result = lastAngle + maxAngleDiff; - } else { + else result = lastAngle - maxAngleDiff; - } } lastAngle = result; From 03f1403446db89a321de21f26a7db8fd820da668 Mon Sep 17 00:00:00 2001 From: lars Date: Wed, 2 Aug 2017 14:56:53 +0200 Subject: [PATCH 11/79] reverted an earlier change due to it being functionally incorrect --- src/fu_line_detection/src/laneDetection.cpp | 84 +++++++++++---------- 1 file changed, 43 insertions(+), 41 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 29723c23..0e28d001 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -362,55 +362,57 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) pubAngle(); - //---------------------- DEBUG OUTPUT LANE POLYNOMIAL ---------------------------------// - #ifdef PAINT_OUTPUT - cv::Mat transformedImagePaintableLaneModel = transformedImage.clone(); - cv::cvtColor(transformedImagePaintableLaneModel,transformedImagePaintableLaneModel,CV_GRAY2BGR); + cv::Mat transformedImagePaintableLaneModel = transformedImage.clone(); + cv::cvtColor(transformedImagePaintableLaneModel,transformedImagePaintableLaneModel,CV_GRAY2BGR); - if (lanePolynomial.hasDetected()) { - int r = lanePolynomial.getLastUsedPosition() == LEFT ? 255 : 0; - int g = lanePolynomial.getLastUsedPosition() == CENTER ? 255 : 0; - int b = lanePolynomial.getLastUsedPosition() == RIGHT ? 255 : 0; + if (lanePolynomial.hasDetected()) { + int r = lanePolynomial.getLastUsedPosition() == LEFT ? 255 : 0; + int g = lanePolynomial.getLastUsedPosition() == CENTER ? 255 : 0; + int b = lanePolynomial.getLastUsedPosition() == RIGHT ? 255 : 0; - for(int i = minYPolyRoi; i < maxYRoi; i++) { - cv::Point pointLoc = cv::Point(lanePolynomial.getLanePoly().at(i)+proj_image_w_half, i); - cv::circle(transformedImagePaintableLaneModel, pointLoc, 0, cv::Scalar(b,g,r), -1); - } + for(int i = minYPolyRoi; i < maxYRoi; i++) { + cv::Point pointLoc = cv::Point(lanePolynomial.getLanePoly().at(i)+proj_image_w_half, i); + cv::circle(transformedImagePaintableLaneModel, pointLoc, 0, cv::Scalar(b,g,r), -1); + } - std::vector> supps; - switch (lanePolynomial.getLastUsedPosition()) { - case LEFT: - supps = supportersLeft; - break; - case CENTER: - supps = supportersCenter; - break; - case RIGHT: - supps = supportersRight; - break; - default: - break; - }; - - for(int i = 0; i < (int)supps.size(); i++) { - FuPoint supp = supps[i]; - cv::Point suppLoc = cv::Point(supp.getX(), supp.getY()); - cv::circle(transformedImagePaintableLaneModel, suppLoc, 1, cv::Scalar(b,g,r), -1); - } + std::vector> supps; + switch (lanePolynomial.getLastUsedPosition()) { + case LEFT: + supps = supportersLeft; + break; + case CENTER: + supps = supportersCenter; + break; + case RIGHT: + supps = supportersRight; + break; + default: + ROS_ERROR("No last used position") + supps = supportersRight; + break; + }; + + for(int i = 0; i < (int)supps.size(); i++) { + FuPoint supp = supps[i]; + cv::Point suppLoc = cv::Point(supp.getX(), supp.getY()); + cv::circle(transformedImagePaintableLaneModel, suppLoc, 1, cv::Scalar(b,g,r), -1); + } - cv::Point pointLoc = cv::Point(proj_image_w_half, proj_image_h); - cv::circle(transformedImagePaintableLaneModel, pointLoc, 2, cv::Scalar(0,0,255), -1); + cv::Point pointLoc = cv::Point(proj_image_w_half, proj_image_h); + cv::circle(transformedImagePaintableLaneModel, pointLoc, 2, cv::Scalar(0,0,255), -1); - cv:Point anglePointLoc = cv::Point(sin(lastAngle * PI / 180) * angleAdjacentLeg + proj_image_w_half, proj_image_h - angleAdjacentLeg); - cv::line(transformedImagePaintableLaneModel, pointLoc, anglePointLoc, cv::Scalar(255,255,255)); - } else { - cv::Point pointLoc = cv::Point(5, 5); - cv::circle(transformedImagePaintableLaneModel, pointLoc, 3, cv::Scalar(0,0,255), 0); - } + cv:Point anglePointLoc = cv::Point(sin(lastAngle * PI / 180) * angleAdjacentLeg + proj_image_w_half, proj_image_h - angleAdjacentLeg); + cv::line(transformedImagePaintableLaneModel, pointLoc, anglePointLoc, cv::Scalar(255,255,255)); + } else { + cv::Point pointLoc = cv::Point(5, 5); + cv::circle(transformedImagePaintableLaneModel, pointLoc, 3, cv::Scalar(0,0,255), 0); + } - pubRGBImageMsg(transformedImagePaintableLaneModel, image_publisher); + pubRGBImageMsg(transformedImagePaintableLaneModel, image_publisher); + //---------------------- DEBUG OUTPUT LANE POLYNOMIAL ---------------------------------// + #ifdef PAINT_OUTPUT cv::namedWindow("Lane polynomial", WINDOW_NORMAL); cv::imshow("Lane polynomial", transformedImagePaintableLaneModel); cv::waitKey(1); From 55283fb0095f54dc012cda04513b56fd1fa2c463 Mon Sep 17 00:00:00 2001 From: lars Date: Wed, 2 Aug 2017 17:34:50 +0200 Subject: [PATCH 12/79] formatting added a missing ; changed the launch file (camera angle and height) to improve IPMapper performance --- .../launch/line_detection_fu.launch | 36 +++++++++---------- src/fu_line_detection/src/laneDetection.cpp | 4 +-- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/src/fu_line_detection/launch/line_detection_fu.launch b/src/fu_line_detection/launch/line_detection_fu.launch index 1fdfce68..859d05ee 100644 --- a/src/fu_line_detection/launch/line_detection_fu.launch +++ b/src/fu_line_detection/launch/line_detection_fu.launch @@ -1,13 +1,13 @@ - + - + - + @@ -26,28 +26,28 @@ - - + + - - - + + + - - + + - - - - - + + + + + diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 0e28d001..adbde1dc 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -388,7 +388,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) supps = supportersRight; break; default: - ROS_ERROR("No last used position") + ROS_ERROR("No last used position"); supps = supportersRight; break; }; @@ -976,7 +976,7 @@ void cLaneDetectionFu::createLanePoly(ePosition position) { double y1; double y2; double y3; - + if (position == LEFT) { usedPoly = polyLeft; dRight = defaultXLeft-5; From 99cfb354b7dd5643d9be57df89970bc3f74db9f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Wed, 2 Aug 2017 18:41:21 +0200 Subject: [PATCH 13/79] adjusted projection size --- .gitignore | 2 ++ .../launch/line_detection_fu.launch | 20 +++++++++---------- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/.gitignore b/.gitignore index 4aa27e6f..47041758 100644 --- a/.gitignore +++ b/.gitignore @@ -6,3 +6,5 @@ build/ devel/ logs/ *~ +.idea/ +cmake-build-debug/ diff --git a/src/fu_line_detection/launch/line_detection_fu.launch b/src/fu_line_detection/launch/line_detection_fu.launch index 859d05ee..5391a5ee 100644 --- a/src/fu_line_detection/launch/line_detection_fu.launch +++ b/src/fu_line_detection/launch/line_detection_fu.launch @@ -3,18 +3,18 @@ - - - + + + - + - - - - - - + + + + + + From 458e7791d3f478645fefe9245ac22c1801fdbb44 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Wed, 2 Aug 2017 19:10:00 +0200 Subject: [PATCH 14/79] adjusted parameter --- .../launch/line_detection_fu.launch | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/fu_line_detection/launch/line_detection_fu.launch b/src/fu_line_detection/launch/line_detection_fu.launch index 5391a5ee..5b7115fd 100644 --- a/src/fu_line_detection/launch/line_detection_fu.launch +++ b/src/fu_line_detection/launch/line_detection_fu.launch @@ -15,21 +15,21 @@ - - + + - - - + + + - - - + + + - + From 78970d0c03183094fdc5644f42ee9cafecee8050 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Wed, 2 Aug 2017 20:01:09 +0200 Subject: [PATCH 15/79] inserted threshold value for minimal distance between lowest and highest point of laneMarkings before Ransac is started --- src/fu_line_detection/src/laneDetection.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index adbde1dc..9d3dfd28 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -1327,6 +1327,12 @@ bool cLaneDetectionFu::ransacInternal(ePosition position, return a.getY() < b.getY(); }); + //ROS_ERROR("Pos: %d, length: %d", position, sortedMarkings.at(sortedMarkings.size() - 1).getY() - sortedMarkings.at(0).getY()); + + if (sortedMarkings.at(sortedMarkings.size() - 1).getY() - sortedMarkings.at(0).getY() < 30) { + return false; + } + std::vector> tmpSupporters = std::vector>(); // vectors for points selected from the bottom, mid and top of the sorted From 5f031b1a126177bd7cba0271545c3708afb7db27 Mon Sep 17 00:00:00 2001 From: lars Date: Wed, 2 Aug 2017 20:18:57 +0200 Subject: [PATCH 16/79] comment about threshold and reason for it being there added --- src/fu_line_detection/src/laneDetection.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 9d3dfd28..432886d2 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -1329,6 +1329,10 @@ bool cLaneDetectionFu::ransacInternal(ePosition position, //ROS_ERROR("Pos: %d, length: %d", position, sortedMarkings.at(sortedMarkings.size() - 1).getY() - sortedMarkings.at(0).getY()); + /* + * threshold value for minimal distance between lowest and highest point + * to prevent the left and center markings being chosen over the right markings + */ if (sortedMarkings.at(sortedMarkings.size() - 1).getY() - sortedMarkings.at(0).getY() < 30) { return false; } From bcd08a4e42f5daf8ae9be0fb059daa837bd960e1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Fri, 18 Aug 2017 21:03:56 +0200 Subject: [PATCH 17/79] - edge points that were found in extractLaneMarkings but were not assigned to any markings vector are printed in yellow in "Grouped Lane Markings" window - replaced buildLaneMarkingsLists: first point of a line is found as before but following points will be added if distance to last valid marking point is smaller (horizontal AND vertical) than specified in 'interestDistancePoly' - at this point only right lane is used --- .../launch/line_detection_fu.launch | 2 +- src/fu_line_detection/src/laneDetection.cpp | 118 ++++++++++++++---- src/fu_line_detection/src/laneDetection.h | 25 ++-- 3 files changed, 109 insertions(+), 36 deletions(-) diff --git a/src/fu_line_detection/launch/line_detection_fu.launch b/src/fu_line_detection/launch/line_detection_fu.launch index 5b7115fd..fa76a4e1 100644 --- a/src/fu_line_detection/launch/line_detection_fu.launch +++ b/src/fu_line_detection/launch/line_detection_fu.launch @@ -15,7 +15,7 @@ - + diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 432886d2..d769daea 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -299,6 +299,12 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Point markingLoc = cv::Point(marking.getX(), marking.getY()); cv::circle(transformedImagePaintable,markingLoc,1,cv::Scalar(255,0,0),-1); } + for(int i = 0; i < (int)laneMarkingsNotUsed.size(); i++) + { + FuPoint marking = laneMarkingsNotUsed[i]; + cv::Point markingLoc = cv::Point(marking.getX(), marking.getY()); + cv::circle(transformedImagePaintable,markingLoc,1,cv::Scalar(0,255,255),-1); + } cv::Point2d p1l(defaultXLeft,minYPolyRoi); cv::Point2d p2l(defaultXLeft,maxYRoi-1); @@ -664,7 +670,7 @@ std::vector> cLaneDetectionFu::extractLaneMarkings(const std::vecto * * @param laneMarkings a vector containing all detected lane markings */ -void cLaneDetectionFu::buildLaneMarkingsLists( +/*void cLaneDetectionFu::buildLaneMarkingsLists( const std::vector> &laneMarkings) { laneMarkingsLeft.clear(); laneMarkingsCenter.clear(); @@ -707,9 +713,70 @@ void cLaneDetectionFu::buildLaneMarkingsLists( continue; } } +}*/ + +void cLaneDetectionFu::buildLaneMarkingsLists( + const std::vector> &laneMarkings) { + laneMarkingsLeft.clear(); + laneMarkingsCenter.clear(); + laneMarkingsRight.clear(); + laneMarkingsNotUsed.clear(); + + // sort the lane marking edge points + std::vector> sortedMarkings = laneMarkings; + + std::sort(sortedMarkings.begin(), sortedMarkings.end(), + [](FuPoint a, FuPoint b) { + return a.getY() > b.getY(); + }); + + for (FuPoint laneMarking : sortedMarkings) { + if (laneMarkingsRight.size() == 0) { + if (polyDetectedRight) { + if (isInPolyRoi(polyRight, laneMarking)) { + laneMarkingsRight.push_back(laneMarking); + continue; + } + } + + if (isInDefaultRoi(RIGHT, laneMarking)) { + laneMarkingsRight.push_back(laneMarking); + continue; + } + } else { + if (isInRange(laneMarkingsRight.at(laneMarkingsRight.size() - 1), laneMarking)) { + laneMarkingsRight.push_back(laneMarking); + continue; + } + + /*double x1 = laneMarking.getX(); + double x2 = laneMarkingsRight.at(laneMarkingsRight.size() - 1).getX(); + double y1 = laneMarking.getY(); + double y2 = laneMarkingsRight.at(laneMarkingsRight.size() - 1).getY(); + + double x = std::abs(x1 - x2); + double y = std::abs(y1 - y2); + + ROS_ERROR("x1: %f, x2: %f, y1: %f, y2: %f, x: %f, y: %f", x1, x2, y1, y2, x, y);*/ + } + + laneMarkingsNotUsed.push_back(laneMarking); + } } +bool cLaneDetectionFu::isInRange(FuPoint &lanePoint, FuPoint &p) { + if (p.getY() < minYDefaultRoi || p.getY() > maxYRoi) { + return false; + } + if (p.getY() > lanePoint.getY()) { + return false; + } + + double distanceX = std::abs(p.getX() - lanePoint.getX()); + double distanceY = std::abs(p.getY() - lanePoint.getY()); + return distanceX < interestDistancePoly && distanceY < interestDistancePoly; +} /** @@ -1281,12 +1348,12 @@ void cLaneDetectionFu::detectLane() { * polynomials. */ void cLaneDetectionFu::ransac() { - polyDetectedLeft = ransacInternal(LEFT, laneMarkingsLeft, bestPolyLeft, + /*polyDetectedLeft = ransacInternal(LEFT, laneMarkingsLeft, bestPolyLeft, polyLeft, supportersLeft, prevPolyLeft, pointsLeft); polyDetectedCenter = ransacInternal(CENTER, laneMarkingsCenter, bestPolyCenter, polyCenter, supportersCenter, prevPolyCenter, - pointsCenter); + pointsCenter);*/ polyDetectedRight = ransacInternal(RIGHT, laneMarkingsRight, bestPolyRight, polyRight, supportersRight, prevPolyRight, pointsRight); @@ -1327,15 +1394,16 @@ bool cLaneDetectionFu::ransacInternal(ePosition position, return a.getY() < b.getY(); }); - //ROS_ERROR("Pos: %d, length: %d", position, sortedMarkings.at(sortedMarkings.size() - 1).getY() - sortedMarkings.at(0).getY()); + ROS_ERROR("length: %d", sortedMarkings.size()); /* * threshold value for minimal distance between lowest and highest point * to prevent the left and center markings being chosen over the right markings */ - if (sortedMarkings.at(sortedMarkings.size() - 1).getY() - sortedMarkings.at(0).getY() < 30) { + /*if (sortedMarkings.at(sortedMarkings.size() - 1).getY() - sortedMarkings.at(0).getY() < 30) { + ROS_ERROR("Minimal distance"); return false; - } + }*/ std::vector> tmpSupporters = std::vector>(); @@ -1490,6 +1558,26 @@ bool cLaneDetectionFu::ransacInternal(ePosition position, bool cLaneDetectionFu::polyValid(ePosition position, NewtonPolynomial poly, NewtonPolynomial prevPoly) { + if (prevPoly.getDegree() != -1) { + FuPoint p4 = FuPoint(poly.at(polyY1), polyY1); + FuPoint p5 = FuPoint(prevPoly.at(polyY1), polyY1); + + if (horizDistance(p4, p5) > 5) {//0.05 * meters) { + //ROS_INFO("Poly was to far away from previous poly at y = 25"); + return false; + } + + FuPoint p6 = FuPoint(poly.at(polyY2), polyY2); + FuPoint p7 = FuPoint(prevPoly.at(polyY2), polyY2); + + if (horizDistance(p6, p7) > 5) {//0.05 * meters) { + //ROS_INFO("Poly was to far away from previous poly at y = 30"); + return false; + } + + return true; + } + FuPoint p1 = FuPoint(poly.at(polyY1), polyY1); //y = 75 if (horizDistanceToDefaultLine(position, p1) > 10) { @@ -1511,24 +1599,6 @@ bool cLaneDetectionFu::polyValid(ePosition position, NewtonPolynomial poly, return false; } - if (prevPoly.getDegree() != -1) { - FuPoint p4 = FuPoint(poly.at(polyY1), polyY1); - FuPoint p5 = FuPoint(prevPoly.at(polyY1), polyY1); - - if (horizDistance(p4, p5) > 5) {//0.05 * meters) { - //ROS_INFO("Poly was to far away from previous poly at y = 25"); - return false; - } - - FuPoint p6 = FuPoint(poly.at(polyY2), polyY2); - FuPoint p7 = FuPoint(prevPoly.at(polyY2), polyY2); - - if (horizDistance(p6, p7) > 5) {//0.05 * meters) { - //ROS_INFO("Poly was to far away from previous poly at y = 30"); - return false; - } - } - //ROS_INFO("Poly is valid"); return true; diff --git a/src/fu_line_detection/src/laneDetection.h b/src/fu_line_detection/src/laneDetection.h index ea319410..b904d8ff 100644 --- a/src/fu_line_detection/src/laneDetection.h +++ b/src/fu_line_detection/src/laneDetection.h @@ -6,25 +6,25 @@ Redistribution and use in source and binary forms, with or without modification, 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 2. 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. -3. All advertising materials mentioning features or use of this software must display the following acknowledgement: “This product includes software developed by the Audi AG and its contributors for Audi Autonomous Driving Cup.” +3. All advertising materials mentioning features or use of this software must display the following acknowledgement: �This product includes software developed by the Audi AG and its contributors for Audi Autonomous Driving Cup.� 4. Neither the name of Audi nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. -THIS SOFTWARE IS PROVIDED BY AUDI AG AND CONTRIBUTORS “AS IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL AUDI AG OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +THIS SOFTWARE IS PROVIDED BY AUDI AG AND CONTRIBUTORS �AS IS� AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL AUDI AG OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. **/ // Christoph Brickl, AEV: /*! \brief cSpurerkennung * -• Zur Erkennung einer Spur wird das RGB Bild der Asus Xtion eingelesen und verarbeitet -• Weiteres Vorgehen: -• Zuschneiden des Orginal Bildes auf die eingestellte Größe -• Erzeugen eines Graustufen Bildes -• Anwenden eines Schwellwertfilters -• Kantendedektion -• Suchen nach Kanten auf den eingestellten cm-Marken -• Auswerten der gefundenen Punkte ob sinnvolle Linie erstellt werden kann -• Anzeigen der Linie mit Hilfe der GLC +� Zur Erkennung einer Spur wird das RGB Bild der Asus Xtion eingelesen und verarbeitet +� Weiteres Vorgehen: +� Zuschneiden des Orginal Bildes auf die eingestellte Gr��e +� Erzeugen eines Graustufen Bildes +� Anwenden eines Schwellwertfilters +� Kantendedektion +� Suchen nach Kanten auf den eingestellten cm-Marken +� Auswerten der gefundenen Punkte ob sinnvolle Linie erstellt werden kann +� Anzeigen der Linie mit Hilfe der GLC */ @@ -203,6 +203,7 @@ class cLaneDetectionFu std::vector> laneMarkingsLeft; std::vector> laneMarkingsCenter; std::vector> laneMarkingsRight; + std::vector> laneMarkingsNotUsed; /** * Newton interpolation data points selected for the best polynomial @@ -266,6 +267,8 @@ class cLaneDetectionFu void buildLaneMarkingsLists(const std::vector> &laneMarkings); + bool isInRange(FuPoint &lanePoint, FuPoint &p); + int horizDistanceToDefaultLine(ePosition &line, FuPoint &p); int horizDistanceToPolynomial(NewtonPolynomial& poly, FuPoint &p); From 49f9f24b5e049e71a44c68761791d349ea088e5b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Sun, 20 Aug 2017 23:57:38 +0200 Subject: [PATCH 18/79] - included cases for left and center lanes - started move poly function --- src/fu_line_detection/src/laneDetection.cpp | 217 ++++++++++++++++++-- src/fu_line_detection/src/laneDetection.h | 2 + 2 files changed, 204 insertions(+), 15 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index d769daea..0502c48f 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -731,25 +731,69 @@ void cLaneDetectionFu::buildLaneMarkingsLists( }); for (FuPoint laneMarking : sortedMarkings) { - if (laneMarkingsRight.size() == 0) { - if (polyDetectedRight) { - if (isInPolyRoi(polyRight, laneMarking)) { - laneMarkingsRight.push_back(laneMarking); - continue; - } + if (laneMarkingsRight.size() > 0) { + if (isInRange(laneMarkingsRight.at(laneMarkingsRight.size() - 1), laneMarking)) { + laneMarkingsRight.push_back(laneMarking); + continue; } + } - if (isInDefaultRoi(RIGHT, laneMarking)) { - laneMarkingsRight.push_back(laneMarking); + if (laneMarkingsCenter.size() > 0) { + if (isInRange(laneMarkingsCenter.at(laneMarkingsCenter.size() - 1), laneMarking)) { + laneMarkingsCenter.push_back(laneMarking); continue; } - } else { - if (isInRange(laneMarkingsRight.at(laneMarkingsRight.size() - 1), laneMarking)) { + } + + if (laneMarkingsLeft.size() > 0) { + if (isInRange(laneMarkingsLeft.at(laneMarkingsLeft.size() - 1), laneMarking)) { + laneMarkingsLeft.push_back(laneMarking); + continue; + } + } + + if (polyDetectedRight) { + if (isInPolyRoi(polyRight, laneMarking)) { laneMarkingsRight.push_back(laneMarking); continue; } + } + + if (polyDetectedCenter) { + if (isInPolyRoi(polyCenter, laneMarking)) { + laneMarkingsCenter.push_back(laneMarking); + continue; + } + } + + if (polyDetectedLeft) { + if (isInPolyRoi(polyLeft, laneMarking)) { + laneMarkingsLeft.push_back(laneMarking); + continue; + } + } + + if (isInDefaultRoi(RIGHT, laneMarking)) { + laneMarkingsRight.push_back(laneMarking); + continue; + } + + if (isInDefaultRoi(CENTER, laneMarking)) { + laneMarkingsCenter.push_back(laneMarking); + continue; + } + + if (isInDefaultRoi(LEFT, laneMarking)) { + laneMarkingsLeft.push_back(laneMarking); + continue; + } + + /*if (laneMarkingsRight.size() == 0) { - /*double x1 = laneMarking.getX(); + } else { + + + double x1 = laneMarking.getX(); double x2 = laneMarkingsRight.at(laneMarkingsRight.size() - 1).getX(); double y1 = laneMarking.getY(); double y2 = laneMarkingsRight.at(laneMarkingsRight.size() - 1).getY(); @@ -757,13 +801,156 @@ void cLaneDetectionFu::buildLaneMarkingsLists( double x = std::abs(x1 - x2); double y = std::abs(y1 - y2); - ROS_ERROR("x1: %f, x2: %f, y1: %f, y2: %f, x: %f, y: %f", x1, x2, y1, y2, x, y);*/ - } + ROS_ERROR("x1: %f, x2: %f, y1: %f, y2: %f, x: %f, y: %f", x1, x2, y1, y2, x, y); + }*/ laneMarkingsNotUsed.push_back(laneMarking); } } +void cLaneDetectionFu::movePoly(ePosition position, NewtonPolynomial &poly1, NewtonPolynomial &poly2) { + double x1 = 0.05; + double x2 = 0.4; + double x3 = 1.0; + + FuPoint pointRight1; + FuPoint pointRight2; + FuPoint pointRight3; + + double m1 = 0; + double m2 = 0; + double m3 = 0; + + double dRight = 0; + + NewtonPolynomial usedPoly; + + /* + * Depending on the sign of the gradient of the poly at the different + * x-values and depending on which position we are, we have to add or + * subtract the expected distance to the respective lane polynomial, to get + * the wanted points. + * + * The calculation is done for the x- and y-components of the points + * separately using the trigonometric ratios of right triangles and the fact + * that arctan of some gradient equals its angle to the x-axis in degree. + */ + if (position == LEFT) { + usedPoly = polyLeft; + m1 = gradient(x1, pointsLeft, usedPoly.getCoefficients()); + m2 = gradient(x2, pointsLeft, usedPoly.getCoefficients()); + m3 = gradient(x3, pointsLeft, usedPoly.getCoefficients()); + + dRight = defaultXLeft; + + if (m1 > 0) { + pointRight1 = FuPoint(x1 + dRight * cos(atan(-1 / m1)), + usedPoly.at(x1) + dRight * sin(atan(-1 / m1))); + } + else { + pointRight1 = FuPoint(x1 - dRight * cos(atan(-1 / m1)), + usedPoly.at(x1) - dRight * sin(atan(-1 / m1))); + } + + if (m2 > 0) { + pointRight2 = FuPoint(x2 + dRight * cos(atan(-1 / m2)), + usedPoly.at(x2) + dRight * sin(atan(-1 / m2))); + } + else { + pointRight2 = FuPoint(x2 - dRight * cos(atan(-1 / m2)), + usedPoly.at(x2) - dRight * sin(atan(-1 / m2))); + } + + if (m3 > 0) { + pointRight3 = FuPoint(x3 + dRight * cos(atan(-1 / m3)), + usedPoly.at(x3) + dRight * sin(atan(-1 / m3))); + } + else { + pointRight3 = FuPoint(x3 - dRight * cos(atan(-1 / m3)), + usedPoly.at(x3) - dRight * sin(atan(-1 / m3))); + } + } + else if (position == CENTER) { + usedPoly = polyCenter; + m1 = gradient(x1, pointsCenter, usedPoly.getCoefficients()); + m2 = gradient(x2, pointsCenter, usedPoly.getCoefficients()); + m3 = gradient(x3, pointsCenter, usedPoly.getCoefficients()); + + dRight = defaultXCenter; + + if (m1 > 0) { + pointRight1 = FuPoint(x1 + dRight * cos(atan(-1 / m1)), + usedPoly.at(x1) + dRight * sin(atan(-1 / m1))); + } + else { + pointRight1 = FuPoint(x1 - dRight * cos(atan(-1 / m1)), + usedPoly.at(x1) - dRight * sin(atan(-1 / m1))); + } + + if (m2 > 0) { + pointRight2 = FuPoint(x2 + dRight * cos(atan(-1 / m2)), + usedPoly.at(x2) + dRight * sin(atan(-1 / m2))); + } + else { + pointRight2 = FuPoint(x2 - dRight * cos(atan(-1 / m2)), + usedPoly.at(x2) - dRight * sin(atan(-1 / m2))); + } + + if (m3 > 0) { + pointRight3 = FuPoint(x3 + dRight * cos(atan(-1 / m3)), + usedPoly.at(x3) + dRight * sin(atan(-1 / m3))); + } + else { + pointRight3 = FuPoint(x3 - dRight * cos(atan(-1 / m3)), + usedPoly.at(x3) - dRight * sin(atan(-1 / m3))); + } + } + else if (position == RIGHT) { + usedPoly = polyRight; + m1 = gradient(x1, pointsRight, usedPoly.getCoefficients()); + m2 = gradient(x2, pointsRight, usedPoly.getCoefficients()); + m3 = gradient(x3, pointsRight, usedPoly.getCoefficients()); + + dRight = defaultXCenter; + + if (m1 > 0) { + pointRight1 = FuPoint(x1 - dRight * cos(atan(-1 / m1)), + usedPoly.at(x1) - dRight * sin(atan(-1 / m1))); + } + else { + pointRight1 = FuPoint(x1 + dRight * cos(atan(-1 / m1)), + usedPoly.at(x1) + dRight * sin(atan(-1 / m1))); + } + + if (m2 > 0) { + pointRight2 = FuPoint(x2 - dRight * cos(atan(-1 / m2)), + usedPoly.at(x2) - dRight * sin(atan(-1 / m2))); + } + else { + pointRight2 = FuPoint(x2 + dRight * cos(atan(-1 / m2)), + usedPoly.at(x2) + dRight * sin(atan(-1 / m2))); + } + + if (m3 > 0) { + pointRight3 = FuPoint(x3 - dRight * cos(atan(-1 / m3)), + usedPoly.at(x3) - dRight * sin(atan(-1 / m3))); + } + else { + pointRight3 = FuPoint(x3 + dRight * cos(atan(-1 / m3)), + usedPoly.at(x3) + dRight * sin(atan(-1 / m3))); + } + } + + // create the lane polynomial out of the shifted points + lanePoly.addDataXY(pointRight1); + lanePoly.addDataXY(pointRight2); + lanePoly.addDataXY(pointRight3); + + lanePolynomial.setLanePoly(lanePoly); + lanePolynomial.setDetected(); + lanePolynomial.setLastUsedPosition(position); +} + bool cLaneDetectionFu::isInRange(FuPoint &lanePoint, FuPoint &p) { if (p.getY() < minYDefaultRoi || p.getY() > maxYRoi) { return false; @@ -1348,12 +1535,12 @@ void cLaneDetectionFu::detectLane() { * polynomials. */ void cLaneDetectionFu::ransac() { - /*polyDetectedLeft = ransacInternal(LEFT, laneMarkingsLeft, bestPolyLeft, + polyDetectedLeft = ransacInternal(LEFT, laneMarkingsLeft, bestPolyLeft, polyLeft, supportersLeft, prevPolyLeft, pointsLeft); polyDetectedCenter = ransacInternal(CENTER, laneMarkingsCenter, bestPolyCenter, polyCenter, supportersCenter, prevPolyCenter, - pointsCenter);*/ + pointsCenter); polyDetectedRight = ransacInternal(RIGHT, laneMarkingsRight, bestPolyRight, polyRight, supportersRight, prevPolyRight, pointsRight); diff --git a/src/fu_line_detection/src/laneDetection.h b/src/fu_line_detection/src/laneDetection.h index b904d8ff..5379fd93 100644 --- a/src/fu_line_detection/src/laneDetection.h +++ b/src/fu_line_detection/src/laneDetection.h @@ -267,6 +267,8 @@ class cLaneDetectionFu void buildLaneMarkingsLists(const std::vector> &laneMarkings); + void movePoly(ePosition position, NewtonPolynomial &poly1, NewtonPolynomial &poly2); + bool isInRange(FuPoint &lanePoint, FuPoint &p); int horizDistanceToDefaultLine(ePosition &line, FuPoint &p); From 832f31d56069f7a511903b826525bc689171f62c Mon Sep 17 00:00:00 2001 From: lars Date: Mon, 21 Aug 2017 17:30:35 +0200 Subject: [PATCH 19/79] adapted and integrated movePoly() function --- src/fu_line_detection/src/laneDetection.cpp | 220 ++++++++++++-------- 1 file changed, 136 insertions(+), 84 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 0502c48f..31e2400d 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -722,6 +722,16 @@ void cLaneDetectionFu::buildLaneMarkingsLists( laneMarkingsRight.clear(); laneMarkingsNotUsed.clear(); + NewtonPolynomial poly1 = NewtonPolynomial(); + NewtonPolynomial poly2 = NewtonPolynomial(); + + if (polyDetectedLeft) + movePoly(LEFT, poly1, poly2); + if (polyDetectedCenter) + movePoly(CENTER, poly1, poly2); + if (polyDetectedRight) + movePoly(RIGHT, poly1, poly2); + // sort the lane marking edge points std::vector> sortedMarkings = laneMarkings; @@ -731,6 +741,49 @@ void cLaneDetectionFu::buildLaneMarkingsLists( }); for (FuPoint laneMarking : sortedMarkings) { + + if (polyDetectedRight) { + if (isInPolyRoi(poly1, laneMarking)) { + laneMarkingsCenter.push_back(laneMarking); + continue; + } + } + + if (polyDetectedRight) { + if (isInPolyRoi(poly2, laneMarking)) { + laneMarkingsLeft.push_back(laneMarking); + continue; + } + } + + if (polyDetectedCenter) { + if (isInPolyRoi(poly1, laneMarking)) { + laneMarkingsLeft.push_back(laneMarking); + continue; + } + } + + if (polyDetectedCenter) { + if (isInPolyRoi(poly2, laneMarking)) { + laneMarkingsRight.push_back(laneMarking); + continue; + } + } + + if (polyDetectedLeft) { + if (isInPolyRoi(poly1, laneMarking)) { + laneMarkingsCenter.push_back(laneMarking); + continue; + } + } + + if (polyDetectedLeft) { + if (isInPolyRoi(poly2, laneMarking)) { + laneMarkingsRight.push_back(laneMarking); + continue; + } + } + if (laneMarkingsRight.size() > 0) { if (isInRange(laneMarkingsRight.at(laneMarkingsRight.size() - 1), laneMarking)) { laneMarkingsRight.push_back(laneMarking); @@ -813,9 +866,15 @@ void cLaneDetectionFu::movePoly(ePosition position, NewtonPolynomial &poly1, New double x2 = 0.4; double x3 = 1.0; - FuPoint pointRight1; - FuPoint pointRight2; - FuPoint pointRight3; + double laneWidth = 20.f; + + FuPoint poly1pointRight1; + FuPoint poly1pointRight2; + FuPoint poly1pointRight3; + + FuPoint poly2pointRight1; + FuPoint poly2pointRight2; + FuPoint poly2pointRight3; double m1 = 0; double m2 = 0; @@ -841,34 +900,29 @@ void cLaneDetectionFu::movePoly(ePosition position, NewtonPolynomial &poly1, New m2 = gradient(x2, pointsLeft, usedPoly.getCoefficients()); m3 = gradient(x3, pointsLeft, usedPoly.getCoefficients()); - dRight = defaultXLeft; + poly1pointRight1 = (m1 > 0) + ? FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))) + : FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))); - if (m1 > 0) { - pointRight1 = FuPoint(x1 + dRight * cos(atan(-1 / m1)), - usedPoly.at(x1) + dRight * sin(atan(-1 / m1))); - } - else { - pointRight1 = FuPoint(x1 - dRight * cos(atan(-1 / m1)), - usedPoly.at(x1) - dRight * sin(atan(-1 / m1))); - } + poly2pointRight1 = (m1 > 0) + ? FuPoint(x1 + 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + 2 * laneWidth * sin(atan(-1 / m1))) + : FuPoint(x1 - 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - 2 * laneWidth * sin(atan(-1 / m1))); - if (m2 > 0) { - pointRight2 = FuPoint(x2 + dRight * cos(atan(-1 / m2)), - usedPoly.at(x2) + dRight * sin(atan(-1 / m2))); - } - else { - pointRight2 = FuPoint(x2 - dRight * cos(atan(-1 / m2)), - usedPoly.at(x2) - dRight * sin(atan(-1 / m2))); - } + poly1pointRight2 = (m2 > 0) + ? FuPoint(x2 + laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))) + : FuPoint(x2 - laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))); - if (m3 > 0) { - pointRight3 = FuPoint(x3 + dRight * cos(atan(-1 / m3)), - usedPoly.at(x3) + dRight * sin(atan(-1 / m3))); - } - else { - pointRight3 = FuPoint(x3 - dRight * cos(atan(-1 / m3)), - usedPoly.at(x3) - dRight * sin(atan(-1 / m3))); - } + poly2pointRight2 = (m2 > 0) + ? FuPoint(x2 + 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + 2 * laneWidth * sin(atan(-1 / m2))) + : FuPoint(x2 - 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - 2 * laneWidth * sin(atan(-1 / m2))); + + poly1pointRight3 = (m3 > 0) + ? FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))) + : FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))); + + poly2pointRight3 = (m3 > 0) + ? FuPoint(x3 + 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + 2 * laneWidth * sin(atan(-1 / m3))) + : FuPoint(x3 - 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - 2 * laneWidth * sin(atan(-1 / m3))); } else if (position == CENTER) { usedPoly = polyCenter; @@ -876,34 +930,29 @@ void cLaneDetectionFu::movePoly(ePosition position, NewtonPolynomial &poly1, New m2 = gradient(x2, pointsCenter, usedPoly.getCoefficients()); m3 = gradient(x3, pointsCenter, usedPoly.getCoefficients()); - dRight = defaultXCenter; + poly1pointRight1 = (m1 > 0) + ? FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))) + : FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))); - if (m1 > 0) { - pointRight1 = FuPoint(x1 + dRight * cos(atan(-1 / m1)), - usedPoly.at(x1) + dRight * sin(atan(-1 / m1))); - } - else { - pointRight1 = FuPoint(x1 - dRight * cos(atan(-1 / m1)), - usedPoly.at(x1) - dRight * sin(atan(-1 / m1))); - } + poly2pointRight1 = (m1 > 0) + ? FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))) + : FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))); - if (m2 > 0) { - pointRight2 = FuPoint(x2 + dRight * cos(atan(-1 / m2)), - usedPoly.at(x2) + dRight * sin(atan(-1 / m2))); - } - else { - pointRight2 = FuPoint(x2 - dRight * cos(atan(-1 / m2)), - usedPoly.at(x2) - dRight * sin(atan(-1 / m2))); - } + poly1pointRight2 = (m2 > 0) + ? FuPoint(x2 + laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))) + : FuPoint(x2 - laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))); - if (m3 > 0) { - pointRight3 = FuPoint(x3 + dRight * cos(atan(-1 / m3)), - usedPoly.at(x3) + dRight * sin(atan(-1 / m3))); - } - else { - pointRight3 = FuPoint(x3 - dRight * cos(atan(-1 / m3)), - usedPoly.at(x3) - dRight * sin(atan(-1 / m3))); - } + poly2pointRight2 = (m2 > 0) + ? FuPoint(x2 - laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))) + : FuPoint(x2 + laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))); + + poly1pointRight3 = (m3 > 0) + ? FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))) + : FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))); + + poly2pointRight3 = (m3 > 0) + ? FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))) + : FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))); } else if (position == RIGHT) { usedPoly = polyRight; @@ -911,44 +960,47 @@ void cLaneDetectionFu::movePoly(ePosition position, NewtonPolynomial &poly1, New m2 = gradient(x2, pointsRight, usedPoly.getCoefficients()); m3 = gradient(x3, pointsRight, usedPoly.getCoefficients()); - dRight = defaultXCenter; + poly1pointRight1 = (m1 > 0) + ? FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))) + : FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))); - if (m1 > 0) { - pointRight1 = FuPoint(x1 - dRight * cos(atan(-1 / m1)), - usedPoly.at(x1) - dRight * sin(atan(-1 / m1))); - } - else { - pointRight1 = FuPoint(x1 + dRight * cos(atan(-1 / m1)), - usedPoly.at(x1) + dRight * sin(atan(-1 / m1))); - } + poly2pointRight1 = (m1 > 0) + ? FuPoint(x1 - 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - 2 * laneWidth * sin(atan(-1 / m1))) + : FuPoint(x1 + 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + 2 * laneWidth * sin(atan(-1 / m1))); - if (m2 > 0) { - pointRight2 = FuPoint(x2 - dRight * cos(atan(-1 / m2)), - usedPoly.at(x2) - dRight * sin(atan(-1 / m2))); - } - else { - pointRight2 = FuPoint(x2 + dRight * cos(atan(-1 / m2)), - usedPoly.at(x2) + dRight * sin(atan(-1 / m2))); - } + poly1pointRight2 = (m2 > 0) + ? FuPoint(x2 - laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))) + : FuPoint(x2 + laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))); - if (m3 > 0) { - pointRight3 = FuPoint(x3 - dRight * cos(atan(-1 / m3)), - usedPoly.at(x3) - dRight * sin(atan(-1 / m3))); - } - else { - pointRight3 = FuPoint(x3 + dRight * cos(atan(-1 / m3)), - usedPoly.at(x3) + dRight * sin(atan(-1 / m3))); - } + poly2pointRight2 = (m2 > 0) + ? FuPoint(x2 - 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - 2 * laneWidth * sin(atan(-1 / m2))) + : FuPoint(x2 + 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + 2 * laneWidth * sin(atan(-1 / m2))); + + poly1pointRight3 = (m3 > 0) + ? FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))) + : FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))); + + poly2pointRight3 = (m3 > 0) + ? FuPoint(x3 - 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - 2 * laneWidth * sin(atan(-1 / m3))) + : FuPoint(x3 + 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + 2 * laneWidth * sin(atan(-1 / m3))); } + poly1.addData(poly1pointRight1); + poly1.addData(poly1pointRight2); + poly1.addData(poly1pointRight3); + + poly2.addData(poly2pointRight1); + poly2.addData(poly2pointRight2); + poly2.addData(poly2pointRight3); + // create the lane polynomial out of the shifted points - lanePoly.addDataXY(pointRight1); - lanePoly.addDataXY(pointRight2); - lanePoly.addDataXY(pointRight3); + // lanePoly.addDataXY(pointRight1); + // lanePoly.addDataXY(pointRight2); + // lanePoly.addDataXY(pointRight3); - lanePolynomial.setLanePoly(lanePoly); - lanePolynomial.setDetected(); - lanePolynomial.setLastUsedPosition(position); + // lanePolynomial.setLanePoly(lanePoly); + // lanePolynomial.setDetected(); + // lanePolynomial.setLastUsedPosition(position); } bool cLaneDetectionFu::isInRange(FuPoint &lanePoint, FuPoint &p) { From 6b9dc713fc606d73a0a59f4c6d0821d241e86ad6 Mon Sep 17 00:00:00 2001 From: lars Date: Mon, 21 Aug 2017 18:27:51 +0200 Subject: [PATCH 20/79] visualisation of the movePoly() output --- src/fu_line_detection/src/laneDetection.cpp | 29 ++++++++++++++------- src/fu_line_detection/src/laneDetection.h | 3 +++ 2 files changed, 23 insertions(+), 9 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 31e2400d..880e84cb 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -125,6 +125,9 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) lanePoly = NewtonPolynomial(); lanePolynomial = LanePolynomial(); + poly1 = NewtonPolynomial(); + poly2 = NewtonPolynomial(); + maxDistance = 1; lastAngle = 0; @@ -306,6 +309,14 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::circle(transformedImagePaintable,markingLoc,1,cv::Scalar(0,255,255),-1); } + for(int i = minYPolyRoi; i < maxYRoi; i++) + { + cv::Point pointPoly1 = cv::Point(poly1.at(i), i); + cv::circle(transformedImagePaintable,pointPoly1,0,cv::Scalar(139,0,139),-1); + cv::Point pointPoly2 = cv::Point(poly2.at(i), i); + cv::circle(transformedImagePaintable,pointPoly2,0,cv::Scalar(139,0,139),-1); + } + cv::Point2d p1l(defaultXLeft,minYPolyRoi); cv::Point2d p2l(defaultXLeft,maxYRoi-1); cv::line(transformedImagePaintable,p1l,p2l,cv::Scalar(0,0,255)); @@ -722,8 +733,8 @@ void cLaneDetectionFu::buildLaneMarkingsLists( laneMarkingsRight.clear(); laneMarkingsNotUsed.clear(); - NewtonPolynomial poly1 = NewtonPolynomial(); - NewtonPolynomial poly2 = NewtonPolynomial(); + poly1.clear(); + poly2.clear(); if (polyDetectedLeft) movePoly(LEFT, poly1, poly2); @@ -866,7 +877,7 @@ void cLaneDetectionFu::movePoly(ePosition position, NewtonPolynomial &poly1, New double x2 = 0.4; double x3 = 1.0; - double laneWidth = 20.f; + double laneWidth = 2.f; FuPoint poly1pointRight1; FuPoint poly1pointRight2; @@ -905,24 +916,24 @@ void cLaneDetectionFu::movePoly(ePosition position, NewtonPolynomial &poly1, New : FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))); poly2pointRight1 = (m1 > 0) - ? FuPoint(x1 + 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + 2 * laneWidth * sin(atan(-1 / m1))) - : FuPoint(x1 - 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - 2 * laneWidth * sin(atan(-1 / m1))); + ? FuPoint(x1 + 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))) + : FuPoint(x1 - 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))); poly1pointRight2 = (m2 > 0) ? FuPoint(x2 + laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))) : FuPoint(x2 - laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))); poly2pointRight2 = (m2 > 0) - ? FuPoint(x2 + 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + 2 * laneWidth * sin(atan(-1 / m2))) - : FuPoint(x2 - 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - 2 * laneWidth * sin(atan(-1 / m2))); + ? FuPoint(x2 + 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))) + : FuPoint(x2 - 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))); poly1pointRight3 = (m3 > 0) ? FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))) : FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))); poly2pointRight3 = (m3 > 0) - ? FuPoint(x3 + 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + 2 * laneWidth * sin(atan(-1 / m3))) - : FuPoint(x3 - 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - 2 * laneWidth * sin(atan(-1 / m3))); + ? FuPoint(x3 + 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))) + : FuPoint(x3 - 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))); } else if (position == CENTER) { usedPoly = polyCenter; diff --git a/src/fu_line_detection/src/laneDetection.h b/src/fu_line_detection/src/laneDetection.h index 5379fd93..52aa414d 100644 --- a/src/fu_line_detection/src/laneDetection.h +++ b/src/fu_line_detection/src/laneDetection.h @@ -233,6 +233,9 @@ class cLaneDetectionFu LanePolynomial lanePolynomial; + NewtonPolynomial poly1; + NewtonPolynomial poly2; + int laneMarkingSquaredThreshold; int angleAdjacentLeg; From d091d2bd7ad58cac012768e74b4a8e5095044d05 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Mon, 21 Aug 2017 19:01:36 +0200 Subject: [PATCH 21/79] draw moved polys in proper direction --- src/fu_line_detection/src/laneDetection.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 880e84cb..1e8c1667 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -311,9 +311,9 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) for(int i = minYPolyRoi; i < maxYRoi; i++) { - cv::Point pointPoly1 = cv::Point(poly1.at(i), i); + cv::Point pointPoly1 = cv::Point(i, poly1.at(i)); cv::circle(transformedImagePaintable,pointPoly1,0,cv::Scalar(139,0,139),-1); - cv::Point pointPoly2 = cv::Point(poly2.at(i), i); + cv::Point pointPoly2 = cv::Point(i, poly2.at(i)); cv::circle(transformedImagePaintable,pointPoly2,0,cv::Scalar(139,0,139),-1); } From 7faba02f766d3551c1bd0556c2172c03a49a9953 Mon Sep 17 00:00:00 2001 From: lars Date: Mon, 21 Aug 2017 21:39:27 +0200 Subject: [PATCH 22/79] changed values for debugging --- src/fu_line_detection/src/laneDetection.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 1e8c1667..f104cfbc 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -873,11 +873,11 @@ void cLaneDetectionFu::buildLaneMarkingsLists( } void cLaneDetectionFu::movePoly(ePosition position, NewtonPolynomial &poly1, NewtonPolynomial &poly2) { - double x1 = 0.05; - double x2 = 0.4; - double x3 = 1.0; + int x1 = 20; + int x2 = 40; + int x3 = 70; - double laneWidth = 2.f; + double laneWidth = 20.f; FuPoint poly1pointRight1; FuPoint poly1pointRight2; From 62e94b6f3ba672def66bd955084ea939c31411aa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Mon, 21 Aug 2017 23:29:44 +0200 Subject: [PATCH 23/79] polynoms moved properly for right lane --- src/fu_line_detection/src/laneDetection.cpp | 52 ++++++++++++--------- 1 file changed, 30 insertions(+), 22 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index f104cfbc..8c8954c8 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -311,9 +311,9 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) for(int i = minYPolyRoi; i < maxYRoi; i++) { - cv::Point pointPoly1 = cv::Point(i, poly1.at(i)); + cv::Point pointPoly1 = cv::Point(poly1.at(i), i); cv::circle(transformedImagePaintable,pointPoly1,0,cv::Scalar(139,0,139),-1); - cv::Point pointPoly2 = cv::Point(i, poly2.at(i)); + cv::Point pointPoly2 = cv::Point(poly2.at(i), i); cv::circle(transformedImagePaintable,pointPoly2,0,cv::Scalar(139,0,139),-1); } @@ -365,6 +365,14 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::circle(transformedImagePaintableRansac,pointLocRight,0,cv::Scalar(255,0,0),-1); } + for(int i = minYPolyRoi; i < maxYRoi; i++) + { + cv::Point pointPoly1 = cv::Point(poly1.at(i), i); + cv::circle(transformedImagePaintableRansac,pointPoly1,0,cv::Scalar(139,0,139),-1); + cv::Point pointPoly2 = cv::Point(poly2.at(i), i); + cv::circle(transformedImagePaintableRansac,pointPoly2,0,cv::Scalar(139,0,139),-1); + } + pubRGBImageMsg(transformedImagePaintableRansac, image_publisher_ransac); #ifdef PAINT_OUTPUT @@ -753,7 +761,7 @@ void cLaneDetectionFu::buildLaneMarkingsLists( for (FuPoint laneMarking : sortedMarkings) { - if (polyDetectedRight) { + /*if (polyDetectedRight) { if (isInPolyRoi(poly1, laneMarking)) { laneMarkingsCenter.push_back(laneMarking); continue; @@ -793,7 +801,7 @@ void cLaneDetectionFu::buildLaneMarkingsLists( laneMarkingsRight.push_back(laneMarking); continue; } - } + }*/ if (laneMarkingsRight.size() > 0) { if (isInRange(laneMarkingsRight.at(laneMarkingsRight.size() - 1), laneMarking)) { @@ -873,11 +881,11 @@ void cLaneDetectionFu::buildLaneMarkingsLists( } void cLaneDetectionFu::movePoly(ePosition position, NewtonPolynomial &poly1, NewtonPolynomial &poly2) { - int x1 = 20; - int x2 = 40; - int x3 = 70; + int x1 = minYPolyRoi+5; + int x2 = minYPolyRoi + ((proj_image_h-minYPolyRoi)/2); + int x3 = proj_image_h-5; - double laneWidth = 20.f; + double laneWidth = 45.f; FuPoint poly1pointRight1; FuPoint poly1pointRight2; @@ -971,38 +979,38 @@ void cLaneDetectionFu::movePoly(ePosition position, NewtonPolynomial &poly1, New m2 = gradient(x2, pointsRight, usedPoly.getCoefficients()); m3 = gradient(x3, pointsRight, usedPoly.getCoefficients()); - poly1pointRight1 = (m1 > 0) + poly1pointRight1 = (m1 < 0) ? FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))) : FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))); - poly2pointRight1 = (m1 > 0) + poly2pointRight1 = (m1 < 0) ? FuPoint(x1 - 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - 2 * laneWidth * sin(atan(-1 / m1))) : FuPoint(x1 + 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + 2 * laneWidth * sin(atan(-1 / m1))); - poly1pointRight2 = (m2 > 0) + poly1pointRight2 = (m2 < 0) ? FuPoint(x2 - laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))) : FuPoint(x2 + laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))); - poly2pointRight2 = (m2 > 0) + poly2pointRight2 = (m2 < 0) ? FuPoint(x2 - 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - 2 * laneWidth * sin(atan(-1 / m2))) : FuPoint(x2 + 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + 2 * laneWidth * sin(atan(-1 / m2))); - poly1pointRight3 = (m3 > 0) + poly1pointRight3 = (m3 < 0) ? FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))) : FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))); - poly2pointRight3 = (m3 > 0) + poly2pointRight3 = (m3 < 0) ? FuPoint(x3 - 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - 2 * laneWidth * sin(atan(-1 / m3))) : FuPoint(x3 + 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + 2 * laneWidth * sin(atan(-1 / m3))); } - poly1.addData(poly1pointRight1); - poly1.addData(poly1pointRight2); - poly1.addData(poly1pointRight3); + poly1.addDataXY(poly1pointRight1); + poly1.addDataXY(poly1pointRight2); + poly1.addDataXY(poly1pointRight3); - poly2.addData(poly2pointRight1); - poly2.addData(poly2pointRight2); - poly2.addData(poly2pointRight3); + poly2.addDataXY(poly2pointRight1); + poly2.addDataXY(poly2pointRight2); + poly2.addDataXY(poly2pointRight3); // create the lane polynomial out of the shifted points // lanePoly.addDataXY(pointRight1); @@ -1598,12 +1606,12 @@ void cLaneDetectionFu::detectLane() { * polynomials. */ void cLaneDetectionFu::ransac() { - polyDetectedLeft = ransacInternal(LEFT, laneMarkingsLeft, bestPolyLeft, + /*polyDetectedLeft = ransacInternal(LEFT, laneMarkingsLeft, bestPolyLeft, polyLeft, supportersLeft, prevPolyLeft, pointsLeft); polyDetectedCenter = ransacInternal(CENTER, laneMarkingsCenter, bestPolyCenter, polyCenter, supportersCenter, prevPolyCenter, - pointsCenter); + pointsCenter);*/ polyDetectedRight = ransacInternal(RIGHT, laneMarkingsRight, bestPolyRight, polyRight, supportersRight, prevPolyRight, pointsRight); From 711036be53011b68f87673ec5d34b954657dd3de Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Wed, 23 Aug 2017 20:25:21 +0200 Subject: [PATCH 24/79] missing polynomials moved properly --- src/fu_line_detection/src/laneDetection.cpp | 283 +++++++++++--------- src/fu_line_detection/src/laneDetection.h | 7 +- 2 files changed, 157 insertions(+), 133 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 8c8954c8..d12e4158 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -125,8 +125,8 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) lanePoly = NewtonPolynomial(); lanePolynomial = LanePolynomial(); - poly1 = NewtonPolynomial(); - poly2 = NewtonPolynomial(); + movedPolyLeft = NewtonPolynomial(); + movedPolyCenter = NewtonPolynomial(); maxDistance = 1; @@ -311,13 +311,15 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) for(int i = minYPolyRoi; i < maxYRoi; i++) { - cv::Point pointPoly1 = cv::Point(poly1.at(i), i); + cv::Point pointPoly1 = cv::Point(movedPolyLeft.at(i), i); cv::circle(transformedImagePaintable,pointPoly1,0,cv::Scalar(139,0,139),-1); - cv::Point pointPoly2 = cv::Point(poly2.at(i), i); - cv::circle(transformedImagePaintable,pointPoly2,0,cv::Scalar(139,0,139),-1); + cv::Point pointPoly2 = cv::Point(movedPolyCenter.at(i), i); + cv::circle(transformedImagePaintable,pointPoly2,0,cv::Scalar(0,0,0),-1); + cv::Point pointPoly3 = cv::Point(movedPolyRight.at(i), i); + cv::circle(transformedImagePaintable,pointPoly3,0,cv::Scalar(255,128,0),-1); } - cv::Point2d p1l(defaultXLeft,minYPolyRoi); + /*cv::Point2d p1l(defaultXLeft,minYPolyRoi); cv::Point2d p2l(defaultXLeft,maxYRoi-1); cv::line(transformedImagePaintable,p1l,p2l,cv::Scalar(0,0,255)); @@ -336,7 +338,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::line(transformedImagePaintable,p1,p2,cv::Scalar(0,200,0)); cv::line(transformedImagePaintable,p2,p3,cv::Scalar(0,200,0)); cv::line(transformedImagePaintable,p3,p4,cv::Scalar(0,200,0)); - cv::line(transformedImagePaintable,p4,p1,cv::Scalar(0,200,0)); + cv::line(transformedImagePaintable,p4,p1,cv::Scalar(0,200,0));*/ pubRGBImageMsg(transformedImagePaintable, image_publisher_lane_markings); @@ -367,10 +369,12 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) for(int i = minYPolyRoi; i < maxYRoi; i++) { - cv::Point pointPoly1 = cv::Point(poly1.at(i), i); + cv::Point pointPoly1 = cv::Point(movedPolyLeft.at(i), i); cv::circle(transformedImagePaintableRansac,pointPoly1,0,cv::Scalar(139,0,139),-1); - cv::Point pointPoly2 = cv::Point(poly2.at(i), i); - cv::circle(transformedImagePaintableRansac,pointPoly2,0,cv::Scalar(139,0,139),-1); + cv::Point pointPoly2 = cv::Point(movedPolyCenter.at(i), i); + cv::circle(transformedImagePaintableRansac,pointPoly2,0,cv::Scalar(0,0,0),-1); + cv::Point pointPoly3 = cv::Point(movedPolyRight.at(i), i); + cv::circle(transformedImagePaintableRansac,pointPoly3,0,cv::Scalar(255,128,0),-1); } pubRGBImageMsg(transformedImagePaintableRansac, image_publisher_ransac); @@ -741,15 +745,11 @@ void cLaneDetectionFu::buildLaneMarkingsLists( laneMarkingsRight.clear(); laneMarkingsNotUsed.clear(); - poly1.clear(); - poly2.clear(); + movedPolyLeft.clear(); + movedPolyCenter.clear(); + movedPolyRight.clear(); - if (polyDetectedLeft) - movePoly(LEFT, poly1, poly2); - if (polyDetectedCenter) - movePoly(CENTER, poly1, poly2); - if (polyDetectedRight) - movePoly(RIGHT, poly1, poly2); + generateMovedPolynomials(); // sort the lane marking edge points std::vector> sortedMarkings = laneMarkings; @@ -761,47 +761,26 @@ void cLaneDetectionFu::buildLaneMarkingsLists( for (FuPoint laneMarking : sortedMarkings) { - /*if (polyDetectedRight) { - if (isInPolyRoi(poly1, laneMarking)) { - laneMarkingsCenter.push_back(laneMarking); - continue; - } - } - - if (polyDetectedRight) { - if (isInPolyRoi(poly2, laneMarking)) { + if (movedPolyLeft.isInitialized()) { + if (isInPolyRoi(movedPolyLeft, laneMarking)) { laneMarkingsLeft.push_back(laneMarking); continue; } } - if (polyDetectedCenter) { - if (isInPolyRoi(poly1, laneMarking)) { - laneMarkingsLeft.push_back(laneMarking); - continue; - } - } - - if (polyDetectedCenter) { - if (isInPolyRoi(poly2, laneMarking)) { - laneMarkingsRight.push_back(laneMarking); - continue; - } - } - - if (polyDetectedLeft) { - if (isInPolyRoi(poly1, laneMarking)) { + if (movedPolyCenter.isInitialized()) { + if (isInPolyRoi(movedPolyCenter, laneMarking)) { laneMarkingsCenter.push_back(laneMarking); continue; } } - if (polyDetectedLeft) { - if (isInPolyRoi(poly2, laneMarking)) { + if (movedPolyRight.isInitialized()) { + if (isInPolyRoi(movedPolyRight, laneMarking)) { laneMarkingsRight.push_back(laneMarking); continue; } - }*/ + } if (laneMarkingsRight.size() > 0) { if (isInRange(laneMarkingsRight.at(laneMarkingsRight.size() - 1), laneMarking)) { @@ -880,20 +859,33 @@ void cLaneDetectionFu::buildLaneMarkingsLists( } } -void cLaneDetectionFu::movePoly(ePosition position, NewtonPolynomial &poly1, NewtonPolynomial &poly2) { +void cLaneDetectionFu::generateMovedPolynomials() { + if ((!polyDetectedLeft && !polyDetectedCenter && !polyDetectedRight) + || (polyDetectedLeft && polyDetectedCenter && polyDetectedRight)) { + return; + } + int x1 = minYPolyRoi+5; int x2 = minYPolyRoi + ((proj_image_h-minYPolyRoi)/2); int x3 = proj_image_h-5; double laneWidth = 45.f; - FuPoint poly1pointRight1; - FuPoint poly1pointRight2; - FuPoint poly1pointRight3; + FuPoint pointLeft1; + FuPoint pointLeft2; + FuPoint pointLeft3; + + FuPoint pointCenter1; + FuPoint pointCenter2; + FuPoint pointCenter3; + + FuPoint pointRight1; + FuPoint pointRight2; + FuPoint pointRight3; - FuPoint poly2pointRight1; - FuPoint poly2pointRight2; - FuPoint poly2pointRight3; + bool movedLeft = false; + bool movedCenter = false; + bool movedRight = false; double m1 = 0; double m2 = 0; @@ -913,113 +905,142 @@ void cLaneDetectionFu::movePoly(ePosition position, NewtonPolynomial &poly1, New * separately using the trigonometric ratios of right triangles and the fact * that arctan of some gradient equals its angle to the x-axis in degree. */ - if (position == LEFT) { + if (polyDetectedRight && !polyDetectedCenter) { + usedPoly = polyRight; + m1 = gradient(x1, pointsRight, usedPoly.getCoefficients()); + m2 = gradient(x2, pointsRight, usedPoly.getCoefficients()); + m3 = gradient(x3, pointsRight, usedPoly.getCoefficients()); + + movedCenter = true; + + pointCenter1 = (m1 < 0) + ? FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))) + : FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))); + + + pointCenter2 = (m2 < 0) + ? FuPoint(x2 - laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))) + : FuPoint(x2 + laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))); + + + pointCenter3 = (m3 < 0) + ? FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))) + : FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))); + + if (!polyDetectedLeft) { + movedLeft = true; + + pointLeft1 = (m1 < 0) + ? FuPoint(x1 - 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - 2 * laneWidth * sin(atan(-1 / m1))) + : FuPoint(x1 + 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + 2 * laneWidth * sin(atan(-1 / m1))); + + pointLeft2 = (m2 < 0) + ? FuPoint(x2 - 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - 2 * laneWidth * sin(atan(-1 / m2))) + : FuPoint(x2 + 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + 2 * laneWidth * sin(atan(-1 / m2))); + + + pointLeft3 = (m3 < 0) + ? FuPoint(x3 - 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - 2 * laneWidth * sin(atan(-1 / m3))) + : FuPoint(x3 + 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + 2 * laneWidth * sin(atan(-1 / m3))); + } + } + else if (polyDetectedLeft && !polyDetectedCenter) { usedPoly = polyLeft; m1 = gradient(x1, pointsLeft, usedPoly.getCoefficients()); m2 = gradient(x2, pointsLeft, usedPoly.getCoefficients()); m3 = gradient(x3, pointsLeft, usedPoly.getCoefficients()); - poly1pointRight1 = (m1 > 0) + movedCenter = true; + + pointCenter1 = (m1 < 0) ? FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))) : FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))); - poly2pointRight1 = (m1 > 0) - ? FuPoint(x1 + 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))) - : FuPoint(x1 - 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))); - - poly1pointRight2 = (m2 > 0) + pointCenter2 = (m2 < 0) ? FuPoint(x2 + laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))) : FuPoint(x2 - laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))); - poly2pointRight2 = (m2 > 0) - ? FuPoint(x2 + 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))) - : FuPoint(x2 - 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))); - - poly1pointRight3 = (m3 > 0) + pointCenter3 = (m3 < 0) ? FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))) : FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))); - poly2pointRight3 = (m3 > 0) - ? FuPoint(x3 + 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))) - : FuPoint(x3 - 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))); + if (!polyDetectedRight) { + movedRight = true; + + pointRight1 = (m1 < 0) + ? FuPoint(x1 + 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))) + : FuPoint(x1 - 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))); + + pointRight2 = (m2 < 0) + ? FuPoint(x2 + 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))) + : FuPoint(x2 - 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))); + + pointRight3 = (m3 < 0) + ? FuPoint(x3 + 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))) + : FuPoint(x3 - 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))); + + } } - else if (position == CENTER) { + else if (polyDetectedCenter) { usedPoly = polyCenter; m1 = gradient(x1, pointsCenter, usedPoly.getCoefficients()); m2 = gradient(x2, pointsCenter, usedPoly.getCoefficients()); m3 = gradient(x3, pointsCenter, usedPoly.getCoefficients()); - poly1pointRight1 = (m1 > 0) - ? FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))) - : FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))); - - poly2pointRight1 = (m1 > 0) - ? FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))) - : FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))); + if (!polyDetectedLeft) { + ROS_ERROR("%f, %f, %f", m1, m2, m3); - poly1pointRight2 = (m2 > 0) - ? FuPoint(x2 + laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))) - : FuPoint(x2 - laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))); - - poly2pointRight2 = (m2 > 0) - ? FuPoint(x2 - laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))) - : FuPoint(x2 + laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))); - - poly1pointRight3 = (m3 > 0) - ? FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))) - : FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))); + movedLeft = true; - poly2pointRight3 = (m3 > 0) - ? FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))) - : FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))); - } - else if (position == RIGHT) { - usedPoly = polyRight; - m1 = gradient(x1, pointsRight, usedPoly.getCoefficients()); - m2 = gradient(x2, pointsRight, usedPoly.getCoefficients()); - m3 = gradient(x3, pointsRight, usedPoly.getCoefficients()); + pointLeft1 = (m1 < 0) + ? FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))) + : FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))); - poly1pointRight1 = (m1 < 0) - ? FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))) - : FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))); + pointLeft2 = (m2 < 0) + ? FuPoint(x2 - laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))) + : FuPoint(x2 + laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))); - poly2pointRight1 = (m1 < 0) - ? FuPoint(x1 - 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - 2 * laneWidth * sin(atan(-1 / m1))) - : FuPoint(x1 + 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + 2 * laneWidth * sin(atan(-1 / m1))); + pointLeft3 = (m3 < 0) + ? FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))) + : FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))); + } - poly1pointRight2 = (m2 < 0) - ? FuPoint(x2 - laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))) - : FuPoint(x2 + laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))); + if (!polyDetectedRight) { + movedRight = true; - poly2pointRight2 = (m2 < 0) - ? FuPoint(x2 - 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - 2 * laneWidth * sin(atan(-1 / m2))) - : FuPoint(x2 + 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + 2 * laneWidth * sin(atan(-1 / m2))); + pointRight1 = (m1 < 0) + ? FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))) + : FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))); - poly1pointRight3 = (m3 < 0) - ? FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))) - : FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))); + pointRight2 = (m2 < 0) + ? FuPoint(x2 + laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))) + : FuPoint(x2 - laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))); - poly2pointRight3 = (m3 < 0) - ? FuPoint(x3 - 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - 2 * laneWidth * sin(atan(-1 / m3))) - : FuPoint(x3 + 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + 2 * laneWidth * sin(atan(-1 / m3))); + pointRight3 = (m3 < 0) + ? FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))) + : FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))); + } } - poly1.addDataXY(poly1pointRight1); - poly1.addDataXY(poly1pointRight2); - poly1.addDataXY(poly1pointRight3); + // create the lane polynomial out of the shifted points - poly2.addDataXY(poly2pointRight1); - poly2.addDataXY(poly2pointRight2); - poly2.addDataXY(poly2pointRight3); + if (movedLeft) { + movedPolyLeft.addDataXY(pointLeft1); + movedPolyLeft.addDataXY(pointLeft2); + movedPolyLeft.addDataXY(pointLeft3); + } - // create the lane polynomial out of the shifted points - // lanePoly.addDataXY(pointRight1); - // lanePoly.addDataXY(pointRight2); - // lanePoly.addDataXY(pointRight3); + if (movedCenter) { + movedPolyCenter.addDataXY(pointCenter1); + movedPolyCenter.addDataXY(pointCenter2); + movedPolyCenter.addDataXY(pointCenter3); + } - // lanePolynomial.setLanePoly(lanePoly); - // lanePolynomial.setDetected(); - // lanePolynomial.setLastUsedPosition(position); + if (movedRight) { + movedPolyRight.addDataXY(pointRight1); + movedPolyRight.addDataXY(pointRight2); + movedPolyRight.addDataXY(pointRight3); + } } bool cLaneDetectionFu::isInRange(FuPoint &lanePoint, FuPoint &p) { @@ -1606,12 +1627,12 @@ void cLaneDetectionFu::detectLane() { * polynomials. */ void cLaneDetectionFu::ransac() { - /*polyDetectedLeft = ransacInternal(LEFT, laneMarkingsLeft, bestPolyLeft, + polyDetectedLeft = ransacInternal(LEFT, laneMarkingsLeft, bestPolyLeft, polyLeft, supportersLeft, prevPolyLeft, pointsLeft); polyDetectedCenter = ransacInternal(CENTER, laneMarkingsCenter, bestPolyCenter, polyCenter, supportersCenter, prevPolyCenter, - pointsCenter);*/ + pointsCenter); polyDetectedRight = ransacInternal(RIGHT, laneMarkingsRight, bestPolyRight, polyRight, supportersRight, prevPolyRight, pointsRight); @@ -1641,6 +1662,8 @@ bool cLaneDetectionFu::ransacInternal(ePosition position, std::vector>& points) { if (laneMarkings.size() < 7) { + prevPoly = poly; + poly.clear(); return false; } @@ -1652,7 +1675,7 @@ bool cLaneDetectionFu::ransacInternal(ePosition position, return a.getY() < b.getY(); }); - ROS_ERROR("length: %d", sortedMarkings.size()); + //ROS_ERROR("length: %d", sortedMarkings.size()); /* * threshold value for minimal distance between lowest and highest point diff --git a/src/fu_line_detection/src/laneDetection.h b/src/fu_line_detection/src/laneDetection.h index 52aa414d..4982cb87 100644 --- a/src/fu_line_detection/src/laneDetection.h +++ b/src/fu_line_detection/src/laneDetection.h @@ -233,8 +233,9 @@ class cLaneDetectionFu LanePolynomial lanePolynomial; - NewtonPolynomial poly1; - NewtonPolynomial poly2; + NewtonPolynomial movedPolyLeft; + NewtonPolynomial movedPolyCenter; + NewtonPolynomial movedPolyRight; int laneMarkingSquaredThreshold; @@ -270,7 +271,7 @@ class cLaneDetectionFu void buildLaneMarkingsLists(const std::vector> &laneMarkings); - void movePoly(ePosition position, NewtonPolynomial &poly1, NewtonPolynomial &poly2); + void generateMovedPolynomials(); bool isInRange(FuPoint &lanePoint, FuPoint &p); From 4c317599e8c92fccd66f233d8dd385ee51277485 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Wed, 30 Aug 2017 14:40:23 +0200 Subject: [PATCH 25/79] started automatic detection of lane position --- src/fu_line_detection/src/laneDetection.cpp | 87 ++++++++++++++++++--- src/fu_line_detection/src/laneDetection.h | 2 + 2 files changed, 80 insertions(+), 9 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index d12e4158..573953b3 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -277,6 +277,8 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) #endif //---------------------- END DEBUG OUTPUT LANE MARKINGS ------------------------------// + findLanePositions(laneMarkings); + // start actual execution buildLaneMarkingsLists(laneMarkings); @@ -679,9 +681,84 @@ std::vector> cLaneDetectionFu::extractLaneMarkings(const std::vecto } } } + + // sort the lane marking edge points + std::sort(result.begin(), result.end(), + [](FuPoint a, FuPoint b) { + return a.getY() > b.getY(); + }); + return result; } +void cLaneDetectionFu::findLanePositions(const std::vector> &laneMarkings) { + if (defaultXLeft > 0) { + return; + } + + int score[proj_image_w]; + + for (FuPoint laneMarking : laneMarkings) { + if (laneMarking.getY() < 50) { + return; + } + + score[laneMarking.getX()]++; + } + + int max1 = 0; + int max2 = 0; + int max3 = 0; + for (int i = 1; i < score.length; i++) { + if (score[i] > score[max1]) { + max3 = max2; + max2 = max1; + max1 = i; + continue; + } + if (score[i] > score[max2]) { + max3 = max2; + max2 = i; + continue; + } + if (score[i] > score[max1]) { + max3 = i; + continue; + } + } + + if (max1 < max2 && max1 < max3) { + defaultXLeft = max1; + + if (max2 < max3) { + defaultXCenter = max2; + defaultXRight = max3; + } else { + defaultXRight = max2; + defaultXCenter = max3; + } + } else if (max2 < max1 && max2 < max3) { + defaultXCenter = max2; + + if (max1 < max3) { + defaultXCenter = max1; + defaultXRight = max3; + } else { + defaultXRight = max1; + defaultXCenter = max3; + } + } else { + defaultXRight = max3; + + if (max1 < max2) { + defaultXCenter = max1; + defaultXRight = max2; + } else { + defaultXRight = max1; + defaultXCenter = max2; + } + } +} /** * Creates three vectors of lane marking points out of the given lane marking @@ -751,15 +828,7 @@ void cLaneDetectionFu::buildLaneMarkingsLists( generateMovedPolynomials(); - // sort the lane marking edge points - std::vector> sortedMarkings = laneMarkings; - - std::sort(sortedMarkings.begin(), sortedMarkings.end(), - [](FuPoint a, FuPoint b) { - return a.getY() > b.getY(); - }); - - for (FuPoint laneMarking : sortedMarkings) { + for (FuPoint laneMarking : laneMarkings) { if (movedPolyLeft.isInitialized()) { if (isInPolyRoi(movedPolyLeft, laneMarking)) { diff --git a/src/fu_line_detection/src/laneDetection.h b/src/fu_line_detection/src/laneDetection.h index 4982cb87..bc26cd33 100644 --- a/src/fu_line_detection/src/laneDetection.h +++ b/src/fu_line_detection/src/laneDetection.h @@ -271,6 +271,8 @@ class cLaneDetectionFu void buildLaneMarkingsLists(const std::vector> &laneMarkings); + void findLanePositions(const std::vector> &laneMarkings); + void generateMovedPolynomials(); bool isInRange(FuPoint &lanePoint, FuPoint &p); From 7f12c4efb4fc3f667866f3fd0dc602640b0f32bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Wed, 30 Aug 2017 18:55:10 +0200 Subject: [PATCH 26/79] automatic detection of lane position: sum of edges in columns of picture --- src/fu_line_detection/src/laneDetection.cpp | 121 +++++++++++--------- 1 file changed, 69 insertions(+), 52 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 573953b3..73a452c3 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -162,6 +162,11 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) //the outer vector represents rows on image, inner vector is vector of line segments of one row, usualy just one line segment //we should generate this only once in the beginning! or even just have it pregenerated for our cam scanlines = getScanlines(); + + // TODO: remove rqt settings for these variables + defaultXLeft = 0; + defaultXCenter = 0; + defaultXRight = 0; } cLaneDetectionFu::~cLaneDetectionFu() @@ -321,7 +326,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::circle(transformedImagePaintable,pointPoly3,0,cv::Scalar(255,128,0),-1); } - /*cv::Point2d p1l(defaultXLeft,minYPolyRoi); + cv::Point2d p1l(defaultXLeft,minYPolyRoi); cv::Point2d p2l(defaultXLeft,maxYRoi-1); cv::line(transformedImagePaintable,p1l,p2l,cv::Scalar(0,0,255)); @@ -333,7 +338,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Point2d p2r(defaultXRight,maxYRoi-1); cv::line(transformedImagePaintable,p1r,p2r,cv::Scalar(255,0,0)); - cv::Point2d p1(proj_image_w_half-(roi_bottom_w/2),maxYRoi-1); + /*cv::Point2d p1(proj_image_w_half-(roi_bottom_w/2),maxYRoi-1); cv::Point2d p2(proj_image_w_half+(roi_bottom_w/2),maxYRoi-1); cv::Point2d p3(proj_image_w_half+(roi_top_w/2),minYPolyRoi); cv::Point2d p4(proj_image_w_half-(roi_top_w/2),minYPolyRoi); @@ -693,71 +698,78 @@ std::vector> cLaneDetectionFu::extractLaneMarkings(const std::vecto void cLaneDetectionFu::findLanePositions(const std::vector> &laneMarkings) { if (defaultXLeft > 0) { - return; + return; } - int score[proj_image_w]; + vector score(proj_image_w); + fill(score.begin(), score.end(), 0); for (FuPoint laneMarking : laneMarkings) { - if (laneMarking.getY() < 50) { - return; - } + /*if (laneMarking.getY() < 50) { + break; + }*/ + + score[laneMarking.getX()]++; + } - score[laneMarking.getX()]++; + for (int i = 0; i < score.size(); i++) { + ROS_ERROR("%d, %d", i, score[i]); } int max1 = 0; int max2 = 0; int max3 = 0; - for (int i = 1; i < score.length; i++) { - if (score[i] > score[max1]) { - max3 = max2; - max2 = max1; - max1 = i; - continue; - } - if (score[i] > score[max2]) { - max3 = max2; - max2 = i; - continue; - } - if (score[i] > score[max1]) { - max3 = i; - continue; - } + for (int i = 1; i < score.size(); i++) { + if (score[i] > score[max1]) { + max3 = max2; + max2 = max1; + max1 = i; + continue; + } + if (score[i] > score[max2]) { + max3 = max2; + max2 = i; + continue; + } + if (score[i] > score[max1]) { + max3 = i; + continue; + } } if (max1 < max2 && max1 < max3) { - defaultXLeft = max1; - - if (max2 < max3) { - defaultXCenter = max2; - defaultXRight = max3; - } else { - defaultXRight = max2; - defaultXCenter = max3; - } + defaultXLeft = max1; + + if (max2 < max3) { + defaultXCenter = max2; + defaultXRight = max3; + } else { + defaultXRight = max2; + defaultXCenter = max3; + } } else if (max2 < max1 && max2 < max3) { - defaultXCenter = max2; - - if (max1 < max3) { - defaultXCenter = max1; - defaultXRight = max3; - } else { - defaultXRight = max1; - defaultXCenter = max3; - } + defaultXCenter = max2; + + if (max1 < max3) { + defaultXCenter = max1; + defaultXRight = max3; + } else { + defaultXRight = max1; + defaultXCenter = max3; + } } else { - defaultXRight = max3; - - if (max1 < max2) { - defaultXCenter = max1; - defaultXRight = max2; - } else { - defaultXRight = max1; - defaultXCenter = max2; - } + defaultXRight = max3; + + if (max1 < max2) { + defaultXCenter = max1; + defaultXRight = max2; + } else { + defaultXRight = max1; + defaultXCenter = max2; + } } + + ROS_ERROR("left: %d, center: %d, right: %d", defaultXLeft, defaultXCenter, defaultXRight); } /** @@ -1057,7 +1069,7 @@ void cLaneDetectionFu::generateMovedPolynomials() { m3 = gradient(x3, pointsCenter, usedPoly.getCoefficients()); if (!polyDetectedLeft) { - ROS_ERROR("%f, %f, %f", m1, m2, m3); + //ROS_ERROR("%f, %f, %f", m1, m2, m3); movedLeft = true; @@ -2061,6 +2073,11 @@ void cLaneDetectionFu::config_callback(line_detection_fu::LaneDetectionConfig &c scanlinesMaxCount = config.scanlinesMaxCount; scanlines = getScanlines(); + + // TODO: remove rqt settings for these variables + defaultXLeft = 0; + defaultXCenter = 0; + defaultXRight = 0; } int main(int argc, char **argv) From c5f0f328a07ee4d38959ccc6ce7e322f1ffe3c00 Mon Sep 17 00:00:00 2001 From: lars Date: Wed, 30 Aug 2017 19:46:55 +0200 Subject: [PATCH 27/79] disabled the automatic lane detection adjusted lane shifting value for left lane in createLanePoly --- src/fu_line_detection/src/laneDetection.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 73a452c3..7486c4bf 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -164,9 +164,9 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) scanlines = getScanlines(); // TODO: remove rqt settings for these variables - defaultXLeft = 0; - defaultXCenter = 0; - defaultXRight = 0; +// defaultXLeft = 0; +// defaultXCenter = 0; +// defaultXRight = 0; } cLaneDetectionFu::~cLaneDetectionFu() @@ -282,7 +282,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) #endif //---------------------- END DEBUG OUTPUT LANE MARKINGS ------------------------------// - findLanePositions(laneMarkings); + //findLanePositions(laneMarkings); // start actual execution buildLaneMarkingsLists(laneMarkings); @@ -1406,7 +1406,7 @@ void cLaneDetectionFu::createLanePoly(ePosition position) { if (position == LEFT) { usedPoly = polyLeft; - dRight = defaultXLeft-5; + dRight = 10; } else if (position == CENTER) { usedPoly = polyCenter; @@ -2075,9 +2075,9 @@ void cLaneDetectionFu::config_callback(line_detection_fu::LaneDetectionConfig &c scanlines = getScanlines(); // TODO: remove rqt settings for these variables - defaultXLeft = 0; - defaultXCenter = 0; - defaultXRight = 0; +// defaultXLeft = 0; +// defaultXCenter = 0; +// defaultXRight = 0; } int main(int argc, char **argv) From cd4985fde603c8f333e76fbf214c4d2950fcf45a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Thu, 31 Aug 2017 14:46:37 +0200 Subject: [PATCH 28/79] automatic detection of lane positions finished --- src/fu_line_detection/cfg/LaneDetection.cfg | 3 - .../launch/line_detection_fu.launch | 3 - src/fu_line_detection/src/laneDetection.cpp | 133 +++++++++--------- src/fu_line_detection/src/laneDetection.h | 8 +- 4 files changed, 72 insertions(+), 75 deletions(-) diff --git a/src/fu_line_detection/cfg/LaneDetection.cfg b/src/fu_line_detection/cfg/LaneDetection.cfg index ea367532..d9c594de 100755 --- a/src/fu_line_detection/cfg/LaneDetection.cfg +++ b/src/fu_line_detection/cfg/LaneDetection.cfg @@ -5,9 +5,6 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("defaultXLeft", int_t, 0, "LEFT lane border position", 20, 0, 160) -gen.add("defaultXCenter", int_t, 0, "CENTER lane border position", 60, 0, 160) -gen.add("defaultXRight", int_t, 0, "RIGHT lane border position", 100, 0, 160) gen.add("interestDistancePoly", int_t, 0, "Accepted horizontal distance between a potential point and a detected polynomial", 5, 0, 100) gen.add("interestDistanceDefault", int_t, 0, "Accepted horizontal distance between a potential point and a default line", 20, 0, 100) gen.add("iterationsRansac", int_t, 0, "Iterations during ransac", 30, 0, 100) diff --git a/src/fu_line_detection/launch/line_detection_fu.launch b/src/fu_line_detection/launch/line_detection_fu.launch index fa76a4e1..7f48d213 100644 --- a/src/fu_line_detection/launch/line_detection_fu.launch +++ b/src/fu_line_detection/launch/line_detection_fu.launch @@ -12,9 +12,6 @@ - - - diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 73a452c3..8cb659f6 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -45,10 +45,6 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) priv_nh_.param(node_name+"/minYDefaultRoi", minYDefaultRoi, 39); priv_nh_.param(node_name+"/minYPolyRoi", minYPolyRoi, 39); - priv_nh_.param(node_name+"/defaultXLeft", defaultXLeft, 10); - priv_nh_.param(node_name+"/defaultXCenter", defaultXCenter, 30); - priv_nh_.param(node_name+"/defaultXRight", defaultXRight, 50); - priv_nh_.param(node_name+"/interestDistancePoly", interestDistancePoly, 10); priv_nh_.param(node_name+"/interestDistanceDefault", interestDistanceDefault, 10); @@ -696,79 +692,94 @@ std::vector> cLaneDetectionFu::extractLaneMarkings(const std::vecto return result; } -void cLaneDetectionFu::findLanePositions(const std::vector> &laneMarkings) { +/** + * Calculates x positions of the lanes. Lane markings of first 10 rows from the bottom + * of the image are used. Car must start between right and center lane because all lane + * marking points in left half of the image are considered as possible lane edge points + * of center lane (analog: right half of image for right lane). Lane marking points in + * range of other lane marking points are supporters because they form a line. The x-value + * of found lane points with maximum supporters will be used. This ensures that non-lane + * points are ignored. Start position of left lane is calculated after start positions + * of center and right lanes are found. + */ +void cLaneDetectionFu::findLanePositions(vector> &laneMarkings) { + // defaultXLeft is calculated after center and right lane position is found if (defaultXLeft > 0) { return; } - vector score(proj_image_w); - fill(score.begin(), score.end(), 0); + // counts how many lane marking points form a line with point in centerStart + // at same index + vector centerSupporter; + vector rightSupporter; - for (FuPoint laneMarking : laneMarkings) { - /*if (laneMarking.getY() < 50) { - break; - }*/ - - score[laneMarking.getX()]++; - } + // possible start points of center lane + vector*> centerStart; + vector*> rightStart; - for (int i = 0; i < score.size(); i++) { - ROS_ERROR("%d, %d", i, score[i]); - } + for (int j = 0; j < laneMarkings.size(); j++) { + FuPoint* laneMarking = &laneMarkings.at(j); - int max1 = 0; - int max2 = 0; - int max3 = 0; - for (int i = 1; i < score.size(); i++) { - if (score[i] > score[max1]) { - max3 = max2; - max2 = max1; - max1 = i; + if (laneMarking->getY() > maxYRoi) { continue; } - if (score[i] > score[max2]) { - max3 = max2; - max2 = i; - continue; - } - if (score[i] > score[max1]) { - max3 = i; - continue; + if (laneMarking->getY() < proj_image_h - (proj_image_h - maxYRoi) - 10) { + break; } - } - if (max1 < max2 && max1 < max3) { - defaultXLeft = max1; + bool isSupporter = false; + if (laneMarking->getX() < proj_image_w_half + proj_image_horizontal_offset) { + for (int i = 0; i < centerStart.size(); i++) { + if (isInRange(*centerStart.at(i), *laneMarking)) { + isSupporter = true; + centerSupporter.at(i)++; + break; + } + } - if (max2 < max3) { - defaultXCenter = max2; - defaultXRight = max3; + if (!isSupporter) { + centerStart.push_back(laneMarking); + centerSupporter.push_back(0); + } } else { - defaultXRight = max2; - defaultXCenter = max3; + for (int i = 0; i < rightStart.size(); i++) { + if (isInRange(*rightStart.at(i), *laneMarking)) { + isSupporter = true; + rightSupporter.at(i)++; + break; + } + } + + if (!isSupporter) { + rightStart.push_back(laneMarking); + rightSupporter.push_back(0); + } } - } else if (max2 < max1 && max2 < max3) { - defaultXCenter = max2; + } - if (max1 < max3) { - defaultXCenter = max1; - defaultXRight = max3; - } else { - defaultXRight = max1; - defaultXCenter = max3; + // use x-value of lane marking point with most (and at least 3) supporters + if (centerStart.size() > 0) { + vector::iterator maxCenterElement = max_element(centerSupporter.begin(), centerSupporter.end()); + + if (*maxCenterElement > 3) { + int position = distance(centerSupporter.begin(), maxCenterElement); + defaultXCenter = centerStart.at(position)->getX(); } - } else { - defaultXRight = max3; + } - if (max1 < max2) { - defaultXCenter = max1; - defaultXRight = max2; - } else { - defaultXRight = max1; - defaultXCenter = max2; + if (rightStart.size() > 0) { + vector::iterator maxRightElement = max_element(rightSupporter.begin(), rightSupporter.end()); + + if (*maxRightElement > 3) { + int position = distance(rightSupporter.begin(), maxRightElement); + defaultXRight = rightStart.at(position)->getX(); } } + if (defaultXCenter > 0 && defaultXRight > 0) { + defaultXLeft = defaultXCenter - (defaultXRight - defaultXCenter); + } + ROS_ERROR("left: %d, center: %d, right: %d", defaultXLeft, defaultXCenter, defaultXRight); } @@ -2047,9 +2058,6 @@ void cLaneDetectionFu::pubGradientAngle() void cLaneDetectionFu::config_callback(line_detection_fu::LaneDetectionConfig &config, uint32_t level) { ROS_ERROR("Reconfigure Request"); - defaultXLeft = config.defaultXLeft; - defaultXCenter = config.defaultXCenter; - defaultXRight = config.defaultXRight; interestDistancePoly = config.interestDistancePoly; interestDistanceDefault= config.interestDistanceDefault; iterationsRansac = config.iterationsRansac; @@ -2073,11 +2081,6 @@ void cLaneDetectionFu::config_callback(line_detection_fu::LaneDetectionConfig &c scanlinesMaxCount = config.scanlinesMaxCount; scanlines = getScanlines(); - - // TODO: remove rqt settings for these variables - defaultXLeft = 0; - defaultXCenter = 0; - defaultXRight = 0; } int main(int argc, char **argv) diff --git a/src/fu_line_detection/src/laneDetection.h b/src/fu_line_detection/src/laneDetection.h index bc26cd33..7e01f3e4 100644 --- a/src/fu_line_detection/src/laneDetection.h +++ b/src/fu_line_detection/src/laneDetection.h @@ -123,9 +123,9 @@ class cLaneDetectionFu * straight lane would show up in the relative coordinate system with the * car standing in the center of the right lane. */ - int defaultXLeft; - int defaultXCenter; - int defaultXRight; + int defaultXLeft = 0; + int defaultXCenter = 0; + int defaultXRight = 0; /** * The maximum distance of a point to a polynomial so that it counts as a @@ -271,7 +271,7 @@ class cLaneDetectionFu void buildLaneMarkingsLists(const std::vector> &laneMarkings); - void findLanePositions(const std::vector> &laneMarkings); + void findLanePositions(vector> &laneMarkings); void generateMovedPolynomials(); From b9487a23c1f2fff698c502fae4f42715cd882988 Mon Sep 17 00:00:00 2001 From: lars Date: Thu, 31 Aug 2017 19:13:30 +0200 Subject: [PATCH 29/79] minor refactoring --- src/fu_line_detection/src/laneDetection.cpp | 72 +++++++++++++++++---- src/fu_line_detection/src/tools/FuPoint.h | 3 + src/fu_line_detection/src/tools/FuVector.h | 4 ++ 3 files changed, 66 insertions(+), 13 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 7a2d8aee..b2aeb091 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -946,7 +946,20 @@ void cLaneDetectionFu::buildLaneMarkingsLists( } } -void cLaneDetectionFu::generateMovedPolynomials() { +void shiftPoint(FuPoint &p, double m, double offset, int x, int y) +{ + if (m < 0) { + p.setX(x - offset * cos(atan(-1 / m))); + p.setY(y - offset * sin(atan(-1 / m))); + return; + } + p.setX(x + offset * cos(atan(-1 / m))); + p.setY(y + offset * sin(atan(-1 / m))); + return; +} + +void cLaneDetectionFu::generateMovedPolynomials() +{ if ((!polyDetectedLeft && !polyDetectedCenter && !polyDetectedRight) || (polyDetectedLeft && polyDetectedCenter && polyDetectedRight)) { return; @@ -958,17 +971,17 @@ void cLaneDetectionFu::generateMovedPolynomials() { double laneWidth = 45.f; - FuPoint pointLeft1; - FuPoint pointLeft2; - FuPoint pointLeft3; + FuPoint pointLeft1 = FuPoint(); + FuPoint pointLeft2 = FuPoint(); + FuPoint pointLeft3 = FuPoint(); - FuPoint pointCenter1; - FuPoint pointCenter2; - FuPoint pointCenter3; + FuPoint pointCenter1 = FuPoint(); + FuPoint pointCenter2 = FuPoint(); + FuPoint pointCenter3 = FuPoint(); - FuPoint pointRight1; - FuPoint pointRight2; - FuPoint pointRight3; + FuPoint pointRight1 = FuPoint(); + FuPoint pointRight2 = FuPoint(); + FuPoint pointRight3 = FuPoint(); bool movedLeft = false; bool movedCenter = false; @@ -1000,6 +1013,10 @@ void cLaneDetectionFu::generateMovedPolynomials() { movedCenter = true; + shiftPoint(pointCenter1,m1, laneWidth, x1, usedPoly.at(x1)); + shiftPoint(pointCenter2,m2, laneWidth, x2, usedPoly.at(x2)); + shiftPoint(pointCenter3,m3, laneWidth, x3, usedPoly.at(x3)); +/* pointCenter1 = (m1 < 0) ? FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))) : FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))); @@ -1013,10 +1030,15 @@ void cLaneDetectionFu::generateMovedPolynomials() { pointCenter3 = (m3 < 0) ? FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))) : FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))); - +*/ if (!polyDetectedLeft) { movedLeft = true; + shiftPoint(pointLeft1,m1, 2 * laneWidth, x1, usedPoly.at(x1)); + shiftPoint(pointLeft2,m2, 2 * laneWidth, x2, usedPoly.at(x2)); + shiftPoint(pointLeft3,m3, 2 * laneWidth, x3, usedPoly.at(x3)); + +/* pointLeft1 = (m1 < 0) ? FuPoint(x1 - 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - 2 * laneWidth * sin(atan(-1 / m1))) : FuPoint(x1 + 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + 2 * laneWidth * sin(atan(-1 / m1))); @@ -1029,6 +1051,7 @@ void cLaneDetectionFu::generateMovedPolynomials() { pointLeft3 = (m3 < 0) ? FuPoint(x3 - 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - 2 * laneWidth * sin(atan(-1 / m3))) : FuPoint(x3 + 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + 2 * laneWidth * sin(atan(-1 / m3))); +*/ } } else if (polyDetectedLeft && !polyDetectedCenter) { @@ -1039,6 +1062,12 @@ void cLaneDetectionFu::generateMovedPolynomials() { movedCenter = true; + shiftPoint(pointCenter1,m1, -laneWidth, x1, usedPoly.at(x1)); + shiftPoint(pointCenter2,m2, -laneWidth, x2, usedPoly.at(x2)); + shiftPoint(pointCenter3,m3, -laneWidth, x3, usedPoly.at(x3)); + + +/* pointCenter1 = (m1 < 0) ? FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))) : FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))); @@ -1050,10 +1079,15 @@ void cLaneDetectionFu::generateMovedPolynomials() { pointCenter3 = (m3 < 0) ? FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))) : FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))); - +*/ if (!polyDetectedRight) { movedRight = true; + shiftPoint(pointRight1,m1, -laneWidth, x1, usedPoly.at(x1)); + shiftPoint(pointRight2,m2, -laneWidth, x2, usedPoly.at(x2)); + shiftPoint(pointRight3,m3, -laneWidth, x3, usedPoly.at(x3)); + +/* pointRight1 = (m1 < 0) ? FuPoint(x1 + 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))) : FuPoint(x1 - 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))); @@ -1065,7 +1099,7 @@ void cLaneDetectionFu::generateMovedPolynomials() { pointRight3 = (m3 < 0) ? FuPoint(x3 + 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))) : FuPoint(x3 - 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))); - +*/ } } else if (polyDetectedCenter) { @@ -1079,6 +1113,11 @@ void cLaneDetectionFu::generateMovedPolynomials() { movedLeft = true; + shiftPoint(pointLeft1,m1, laneWidth, x1, usedPoly.at(x1)); + shiftPoint(pointLeft2,m2, laneWidth, x2, usedPoly.at(x2)); + shiftPoint(pointLeft3,m3, laneWidth, x3, usedPoly.at(x3)); + +/* pointLeft1 = (m1 < 0) ? FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))) : FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))); @@ -1090,11 +1129,17 @@ void cLaneDetectionFu::generateMovedPolynomials() { pointLeft3 = (m3 < 0) ? FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))) : FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))); +*/ } if (!polyDetectedRight) { movedRight = true; + shiftPoint(pointRight1,m1, -laneWidth, x1, usedPoly.at(x1)); + shiftPoint(pointRight2,m2, -laneWidth, x2, usedPoly.at(x2)); + shiftPoint(pointRight3,m3, -laneWidth, x3, usedPoly.at(x3)); + +/* pointRight1 = (m1 < 0) ? FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))) : FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))); @@ -1106,6 +1151,7 @@ void cLaneDetectionFu::generateMovedPolynomials() { pointRight3 = (m3 < 0) ? FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))) : FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))); +*/ } } diff --git a/src/fu_line_detection/src/tools/FuPoint.h b/src/fu_line_detection/src/tools/FuPoint.h index d0b3076a..5b0d7df4 100644 --- a/src/fu_line_detection/src/tools/FuPoint.h +++ b/src/fu_line_detection/src/tools/FuPoint.h @@ -28,6 +28,9 @@ class FuPoint { MemberType getX() const { return originFuVector.getX(); } MemberType getY() const { return originFuVector.getY(); } + void setX(MemberType x) { originFuVector.setX(x); } + void setY(MemberType y) { originFuVector.setY(y); } + std::string toString() const { std::stringstream result; result << "FuPoint {" << getX().value() << ", " << getY().value() << "}"; diff --git a/src/fu_line_detection/src/tools/FuVector.h b/src/fu_line_detection/src/tools/FuVector.h index b57dd15c..cd96009b 100644 --- a/src/fu_line_detection/src/tools/FuVector.h +++ b/src/fu_line_detection/src/tools/FuVector.h @@ -47,6 +47,10 @@ class FuVector { MemberType getX() const { return x; } MemberType getY() const { return y; } + void setX(MemberType x) { this->x = x; } + void setY(MemberType y) { this->y = y; } + + std::string toString() const { std::stringstream result; result << "FuVector {" << getX() << ", " << getY() << "}"; From 2ac86d5c75e5cbbd162e27294cb4732f06fe3a27 Mon Sep 17 00:00:00 2001 From: lars Date: Thu, 31 Aug 2017 19:46:20 +0200 Subject: [PATCH 30/79] testing integration of dynamic lane width detection with the createLanePoly function --- src/fu_line_detection/src/laneDetection.cpp | 58 +++++++++++++++++++++ 1 file changed, 58 insertions(+) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index b2aeb091..02f5878b 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -1438,6 +1438,63 @@ ePosition cLaneDetectionFu::maxProportion() { void cLaneDetectionFu::createLanePoly(ePosition position) { lanePoly.clear(); + double laneWidth = (defaultXRight - defaultXCenter) / 2; //45.f; + double x1 = minYPolyRoi + 5; + double x2 = minYPolyRoi + ((proj_image_h-minYPolyRoi) / 2); + double x3 = proj_image_h - 5; + + FuPoint point1 = FuPoint(); + FuPoint point2 = FuPoint(); + FuPoint point3 = FuPoint(); + + NewtonPolynomial usedPoly; + + double m1 = 0; + double m2 = 0; + double m3 = 0; + + if (position == LEFT) { + usedPoly = polyLeft; + m1 = gradient(x1, pointsLeft, usedPoly.getCoefficients()); + m2 = gradient(x2, pointsLeft, usedPoly.getCoefficients()); + m3 = gradient(x3, pointsLeft, usedPoly.getCoefficients()); + + shiftPoint(point1,m1, laneWidth, x1, usedPoly.at(x1)); + shiftPoint(point2,m2, laneWidth, x2, usedPoly.at(x2)); + shiftPoint(point3,m3, laneWidth, x3, usedPoly.at(x3)); + } + else if (position == CENTER) { + usedPoly = polyCenter; + m1 = gradient(x1, pointsCenter, usedPoly.getCoefficients()); + m2 = gradient(x2, pointsCenter, usedPoly.getCoefficients()); + m3 = gradient(x3, pointsCenter, usedPoly.getCoefficients()); + + shiftPoint(point1,m1, laneWidth, x1, usedPoly.at(x1)); + shiftPoint(point2,m2, laneWidth, x2, usedPoly.at(x2)); + shiftPoint(point3,m3, laneWidth, x3, usedPoly.at(x3)); + } + else if (position == RIGHT) { + usedPoly = polyRight; + m1 = gradient(x1, pointsRight, usedPoly.getCoefficients()); + m2 = gradient(x2, pointsRight, usedPoly.getCoefficients()); + m3 = gradient(x3, pointsRight, usedPoly.getCoefficients()); + + shiftPoint(point1,m1, laneWidth, x1, usedPoly.at(x1)); + shiftPoint(point2,m2, laneWidth, x2, usedPoly.at(x2)); + shiftPoint(point3,m3, laneWidth, x3, usedPoly.at(x3)); + } + + // create the lane polynomial out of the shifted points + lanePoly.addDataXY(point1); + lanePoly.addDataXY(point2); + lanePoly.addDataXY(point3); + + lanePolynomial.setLanePoly(lanePoly); + lanePolynomial.setDetected(); + lanePolynomial.setLastUsedPosition(position); +/* + lanePoly.clear(); + double coef = 1.2; double x1 = minYPolyRoi+5; @@ -1482,6 +1539,7 @@ void cLaneDetectionFu::createLanePoly(ePosition position) { lanePolynomial.setLanePoly(lanePoly); lanePolynomial.setDetected(); lanePolynomial.setLastUsedPosition(position); +*/ } //original method, should be better, but doesn't work correctly in our case when RIGHT polynomial is used From 8322b455c5fb3239e6a1324b29054751586216f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Fri, 1 Sep 2017 16:19:39 +0200 Subject: [PATCH 31/79] adjusted angle calculation --- src/fu_line_detection/src/laneDetection.cpp | 122 +++++++++++++------- src/fu_line_detection/src/laneDetection.h | 5 + 2 files changed, 83 insertions(+), 44 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 02f5878b..74493c27 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -398,7 +398,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) int b = lanePolynomial.getLastUsedPosition() == RIGHT ? 255 : 0; - for(int i = minYPolyRoi; i < maxYRoi; i++) { + /*for(int i = minYPolyRoi; i < maxYRoi; i++) { cv::Point pointLoc = cv::Point(lanePolynomial.getLanePoly().at(i)+proj_image_w_half, i); cv::circle(transformedImagePaintableLaneModel, pointLoc, 0, cv::Scalar(b,g,r), -1); } @@ -424,13 +424,22 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) FuPoint supp = supps[i]; cv::Point suppLoc = cv::Point(supp.getX(), supp.getY()); cv::circle(transformedImagePaintableLaneModel, suppLoc, 1, cv::Scalar(b,g,r), -1); - } + }*/ cv::Point pointLoc = cv::Point(proj_image_w_half, proj_image_h); cv::circle(transformedImagePaintableLaneModel, pointLoc, 2, cv::Scalar(0,0,255), -1); - cv:Point anglePointLoc = cv::Point(sin(lastAngle * PI / 180) * angleAdjacentLeg + proj_image_w_half, proj_image_h - angleAdjacentLeg); + cv::Point anglePointLoc = cv::Point(sin(lastAngle * PI / 180) * angleAdjacentLeg + proj_image_w_half, proj_image_h - angleAdjacentLeg); cv::line(transformedImagePaintableLaneModel, pointLoc, anglePointLoc, cv::Scalar(255,255,255)); + + cv::Point targetPoint = cv::Point(movedPointForAngle.getX(), movedPointForAngle.getY()); + cv::circle(transformedImagePaintableLaneModel, targetPoint, 2, cv::Scalar(0,0,255), -1); + + cv::Point adjacentLegPoint = cv::Point(proj_image_w_half, proj_image_h - adjacentLeg); + cv::line(transformedImagePaintableLaneModel, pointLoc, adjacentLegPoint, cv::Scalar(255,0,0)); + + cv::Point oppositeLegPoint = cv::Point(proj_image_w_half + oppositeLeg, proj_image_h - adjacentLeg); + cv::line(transformedImagePaintableLaneModel, adjacentLegPoint, oppositeLegPoint, cv::Scalar(0,255,0)); } else { cv::Point pointLoc = cv::Point(5, 5); cv::circle(transformedImagePaintableLaneModel, pointLoc, 3, cv::Scalar(0,0,255), 0); @@ -969,6 +978,7 @@ void cLaneDetectionFu::generateMovedPolynomials() int x2 = minYPolyRoi + ((proj_image_h-minYPolyRoi)/2); int x3 = proj_image_h-5; + // TODO defaultXRight - defaultXCenter double laneWidth = 45.f; FuPoint pointLeft1 = FuPoint(); @@ -1303,12 +1313,20 @@ int cLaneDetectionFu::horizDistance(FuPoint &p1, FuPoint &p2) { * @param coeffs The coefficients under usage of the newton basis * @return The gradient of the polynomial at x */ -double cLaneDetectionFu::gradient(double x, std::vector> &points, - std::vector coeffs) { +double cLaneDetectionFu::gradient(double x, vector> &points, + vector coeffs) { return 2 * coeffs[2] * x + coeffs[1] - coeffs[2] * points[1].getY() - coeffs[2] * points[0].getY(); } +double cLaneDetectionFu::gradient(double x, NewtonPolynomial polynomial) { + vector coeffs = polynomial.getCoefficients(); + + + return 2 * coeffs[2] * x + coeffs[1] - coeffs[2] * polynomial.at(10); + - coeffs[2] * polynomial.at(50); +} + /** * Calculates the x value of the point where the normal of the tangent of a * polynomial at a given point p intersects with a second polynomial. @@ -1438,7 +1456,7 @@ ePosition cLaneDetectionFu::maxProportion() { void cLaneDetectionFu::createLanePoly(ePosition position) { lanePoly.clear(); - double laneWidth = (defaultXRight - defaultXCenter) / 2; //45.f; + //double laneWidth = (defaultXRight - defaultXCenter) / 2; //45.f; double x1 = minYPolyRoi + 5; double x2 = minYPolyRoi + ((proj_image_h-minYPolyRoi) / 2); double x3 = proj_image_h - 5; @@ -1459,9 +1477,9 @@ void cLaneDetectionFu::createLanePoly(ePosition position) { m2 = gradient(x2, pointsLeft, usedPoly.getCoefficients()); m3 = gradient(x3, pointsLeft, usedPoly.getCoefficients()); - shiftPoint(point1,m1, laneWidth, x1, usedPoly.at(x1)); - shiftPoint(point2,m2, laneWidth, x2, usedPoly.at(x2)); - shiftPoint(point3,m3, laneWidth, x3, usedPoly.at(x3)); + shiftPoint(point1,m1, defaultXLeft, x1, usedPoly.at(x1)); + shiftPoint(point2,m2, defaultXLeft, x2, usedPoly.at(x2)); + shiftPoint(point3,m3, defaultXLeft, x3, usedPoly.at(x3)); } else if (position == CENTER) { usedPoly = polyCenter; @@ -1469,9 +1487,9 @@ void cLaneDetectionFu::createLanePoly(ePosition position) { m2 = gradient(x2, pointsCenter, usedPoly.getCoefficients()); m3 = gradient(x3, pointsCenter, usedPoly.getCoefficients()); - shiftPoint(point1,m1, laneWidth, x1, usedPoly.at(x1)); - shiftPoint(point2,m2, laneWidth, x2, usedPoly.at(x2)); - shiftPoint(point3,m3, laneWidth, x3, usedPoly.at(x3)); + shiftPoint(point1,m1, defaultXCenter, x1, usedPoly.at(x1)); + shiftPoint(point2,m2, defaultXCenter, x2, usedPoly.at(x2)); + shiftPoint(point3,m3, defaultXCenter, x3, usedPoly.at(x3)); } else if (position == RIGHT) { usedPoly = polyRight; @@ -1479,9 +1497,9 @@ void cLaneDetectionFu::createLanePoly(ePosition position) { m2 = gradient(x2, pointsRight, usedPoly.getCoefficients()); m3 = gradient(x3, pointsRight, usedPoly.getCoefficients()); - shiftPoint(point1,m1, laneWidth, x1, usedPoly.at(x1)); - shiftPoint(point2,m2, laneWidth, x2, usedPoly.at(x2)); - shiftPoint(point3,m3, laneWidth, x3, usedPoly.at(x3)); + shiftPoint(point1,m1, defaultXRight, x1, usedPoly.at(x1)); + shiftPoint(point2,m2, defaultXRight, x2, usedPoly.at(x2)); + shiftPoint(point3,m3, defaultXRight, x3, usedPoly.at(x3)); } // create the lane polynomial out of the shifted points @@ -1858,14 +1876,6 @@ bool cLaneDetectionFu::ransacInternal(ePosition position, return false; } - // sort the lane marking edge points - std::vector> sortedMarkings = laneMarkings; - - std::sort(sortedMarkings.begin(), sortedMarkings.end(), - [](FuPoint a, FuPoint b) { - return a.getY() < b.getY(); - }); - //ROS_ERROR("length: %d", sortedMarkings.size()); /* @@ -1889,20 +1899,19 @@ bool cLaneDetectionFu::ransacInternal(ePosition position, // Points are selected from the bottom, mid and top. The selection regions // are spread apart for better results during RANSAC - for (std::vector>::size_type i = 0; i != sortedMarkings.size(); - i++) { - if (i < double(sortedMarkings.size()) / 7) { - markings1.push_back(sortedMarkings[i]); + for (std::vector>::size_type i = 0; i != laneMarkings.size(); i++) { + if (i < double(laneMarkings.size()) / 7) { + markings1.push_back(laneMarkings[i]); } - else if (i >= (double(sortedMarkings.size()) / 7) * 3 - && i < (double(sortedMarkings.size()) / 7) * 4) { - markings2.push_back(sortedMarkings[i]); + else if (i >= (double(laneMarkings.size()) / 7) * 3 + && i < (double(laneMarkings.size()) / 7) * 4) { + markings2.push_back(laneMarkings[i]); } - else if (i >= (double(sortedMarkings.size()) / 7) * 6) { - markings3.push_back(sortedMarkings[i]); + else if (i >= (double(laneMarkings.size()) / 7) * 6) { + markings3.push_back(laneMarkings[i]); } - if (sortedMarkings[i].getY() > 5) { + if (laneMarkings[i].getY() > 5) { highEnoughY = true; } } @@ -2050,7 +2059,7 @@ bool cLaneDetectionFu::polyValid(ePosition position, NewtonPolynomial poly, return true; } - FuPoint p1 = FuPoint(poly.at(polyY1), polyY1); //y = 75 + /*FuPoint p1 = FuPoint(poly.at(polyY1), polyY1); //y = 75 if (horizDistanceToDefaultLine(position, p1) > 10) { //ROS_INFO("Poly was to far away from default line at y = 25"); @@ -2069,7 +2078,7 @@ bool cLaneDetectionFu::polyValid(ePosition position, NewtonPolynomial poly, if (horizDistanceToDefaultLine(position, p3) > 40) { //ROS_INFO("Poly was to far away from default line at y = 30"); return false; - } + }*/ //ROS_INFO("Poly is valid"); @@ -2105,27 +2114,52 @@ void cLaneDetectionFu::pubRGBImageMsg(cv::Mat& rgb_mat, image_transport::CameraP publisher.publish(rgb_img, rgb_camera_info); } -void cLaneDetectionFu::pubAngle() -{ +void cLaneDetectionFu::pubAngle() { if (!lanePolynomial.hasDetected()) return; - // why proj_image_h? - double oppositeLeg = lanePolynomial.getLanePoly().at(proj_image_h - angleAdjacentLeg); + //double oppositeLeg = lanePolynomial.getLanePoly().at(proj_image_h - angleAdjacentLeg); // Gegenkathete - oppositeLeg - // Ankathete - angleAdjacentLeg?! - // this seems to be wrong! - double result = atan(oppositeLeg / angleAdjacentLeg) * 180 / PI; + // Ankathete - angleAdjacentLeg + //double result = atan(oppositeLeg / angleAdjacentLeg) * 180 / PI; + + int y = proj_image_h - angleAdjacentLeg; + double xRightLane; + double m; + + if (polyDetectedRight) { + xRightLane = polyRight.at(y); + m = gradient(y, pointsRight, polyRight.getCoefficients()); + } else { + xRightLane = movedPolyRight.at(y); + m = gradient(y, movedPolyRight); + } + + shiftPoint(movedPointForAngle, m, 22.f, y, xRightLane); + + //double xRightLane = polyDetectedRight ? polyRight.at(y) : movedPolyRight.at(y); + //double xCenterLane = polyDetectedCenter ? polyCenter.at(y) : movedPolyCenter.at(y); + + oppositeLeg = proj_image_w_half - movedPointForAngle.getX(); + adjacentLeg = proj_image_h - movedPointForAngle.getY(); + + /*if (xCenterLane + oppositeLeg < proj_image_w_half) { + oppositeLeg *= -1; + }*/ + + double result = atan(oppositeLeg / adjacentLeg) * 180 / PI; + + ROS_ERROR("oppositeLeg: %f, angle: %f", oppositeLeg, result); /* * filter too rash steering angles / jitter in polynomial data */ - if (std::abs(result - lastAngle) > maxAngleDiff) { + /*if (std::abs(result - lastAngle) > maxAngleDiff) { if (result - lastAngle > 0) result = lastAngle + maxAngleDiff; else result = lastAngle - maxAngleDiff; - } + }*/ lastAngle = result; diff --git a/src/fu_line_detection/src/laneDetection.h b/src/fu_line_detection/src/laneDetection.h index 7e01f3e4..33e87870 100644 --- a/src/fu_line_detection/src/laneDetection.h +++ b/src/fu_line_detection/src/laneDetection.h @@ -247,6 +247,9 @@ class cLaneDetectionFu int maxAngleDiff; + FuPoint movedPointForAngle = FuPoint(); + double oppositeLeg; + double adjacentLeg; public: @@ -304,6 +307,8 @@ class cLaneDetectionFu double gradient(double, std::vector>&, std::vector); + double gradient(double x, NewtonPolynomial polynomial); + double intersection(FuPoint&, double&, std::vector>&, std::vector&); From 0d0ec03381ad9faa4e684cb71fb7f4a89c62edd6 Mon Sep 17 00:00:00 2001 From: lars Date: Fri, 1 Sep 2017 16:40:29 +0200 Subject: [PATCH 32/79] fixed painting of targetPoint --- src/fu_line_detection/src/laneDetection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 74493c27..cf02df2e 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -432,7 +432,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Point anglePointLoc = cv::Point(sin(lastAngle * PI / 180) * angleAdjacentLeg + proj_image_w_half, proj_image_h - angleAdjacentLeg); cv::line(transformedImagePaintableLaneModel, pointLoc, anglePointLoc, cv::Scalar(255,255,255)); - cv::Point targetPoint = cv::Point(movedPointForAngle.getX(), movedPointForAngle.getY()); + cv::Point targetPoint = cv::Point(proj_image_w - movedPointForAngle.getX(), movedPointForAngle.getY()); cv::circle(transformedImagePaintableLaneModel, targetPoint, 2, cv::Scalar(0,0,255), -1); cv::Point adjacentLegPoint = cv::Point(proj_image_w_half, proj_image_h - adjacentLeg); From 664f13895b6b357aa2e6cb83fe48dce660adc0eb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Fri, 1 Sep 2017 19:07:02 +0200 Subject: [PATCH 33/79] further angle calculation --- src/fu_line_detection/src/laneDetection.cpp | 40 +++++++++++++++++++-- src/fu_line_detection/src/laneDetection.h | 2 ++ 2 files changed, 40 insertions(+), 2 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index cf02df2e..48f6ada3 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -440,6 +440,15 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Point oppositeLegPoint = cv::Point(proj_image_w_half + oppositeLeg, proj_image_h - adjacentLeg); cv::line(transformedImagePaintableLaneModel, adjacentLegPoint, oppositeLegPoint, cv::Scalar(0,255,0)); + + double n = pointForAngle.getY() - gradientForAngle * pointForAngle.getX(); + double x = 10; + double y = gradientForAngle * x + n; + + cv::Point startNormalPoint = cv::Point(pointForAngle.getX(), pointForAngle.getY()); + cv::Point endNormalPoint = cv::Point(x, y); + cv::line(transformedImagePaintableLaneModel, startNormalPoint, endNormalPoint, cv::Scalar(0,0,0)); + } else { cv::Point pointLoc = cv::Point(5, 5); cv::circle(transformedImagePaintableLaneModel, pointLoc, 3, cv::Scalar(0,0,255), 0); @@ -955,6 +964,14 @@ void cLaneDetectionFu::buildLaneMarkingsLists( } } +/** + * + * @param p + * @param m + * @param offset Positive if shifting to the left, negative to the right + * @param x + * @param y + */ void shiftPoint(FuPoint &p, double m, double offset, int x, int y) { if (m < 0) { @@ -964,7 +981,6 @@ void shiftPoint(FuPoint &p, double m, double offset, int x, int y) } p.setX(x + offset * cos(atan(-1 / m))); p.setY(y + offset * sin(atan(-1 / m))); - return; } void cLaneDetectionFu::generateMovedPolynomials() @@ -2131,11 +2147,31 @@ void cLaneDetectionFu::pubAngle() { xRightLane = polyRight.at(y); m = gradient(y, pointsRight, polyRight.getCoefficients()); } else { + lastAngle = 0; + return; + xRightLane = movedPolyRight.at(y); m = gradient(y, movedPolyRight); } - shiftPoint(movedPointForAngle, m, 22.f, y, xRightLane); + m = -m; + + ROS_ERROR("gradient: %f", m); + + //shiftPoint(movedPointForAngle, m, 10.f, y, xRightLane); + + if (m < 0) { + movedPointForAngle.setX(y + 22.f * cos(atan(-1 / m))); + movedPointForAngle.setY(xRightLane - 22.f * sin(atan(-1 / m))); + } else{ + movedPointForAngle.setX(y - 22.f * cos(atan(-1 / m))); + movedPointForAngle.setY(xRightLane + 22.f * sin(atan(-1 / m))); + } + + pointForAngle.setX(xRightLane); + pointForAngle.setY(y); + + gradientForAngle = m; //double xRightLane = polyDetectedRight ? polyRight.at(y) : movedPolyRight.at(y); //double xCenterLane = polyDetectedCenter ? polyCenter.at(y) : movedPolyCenter.at(y); diff --git a/src/fu_line_detection/src/laneDetection.h b/src/fu_line_detection/src/laneDetection.h index 33e87870..7914a0df 100644 --- a/src/fu_line_detection/src/laneDetection.h +++ b/src/fu_line_detection/src/laneDetection.h @@ -248,8 +248,10 @@ class cLaneDetectionFu int maxAngleDiff; FuPoint movedPointForAngle = FuPoint(); + FuPoint pointForAngle = FuPoint(); double oppositeLeg; double adjacentLeg; + double gradientForAngle; public: From b850256950af8d09d070e2c7c60cb773d0c523e8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Sun, 3 Sep 2017 13:26:01 +0200 Subject: [PATCH 34/79] reverted some parts back of angle calculation --- src/fu_line_detection/src/laneDetection.cpp | 26 ++++++++++----------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 48f6ada3..f5c3d907 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -385,14 +385,14 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) #endif //---------------------- END DEBUG OUTPUT RANSAC POLYNOMIALS ------------------------------// - detectLane(); + //detectLane(); pubAngle(); cv::Mat transformedImagePaintableLaneModel = transformedImage.clone(); cv::cvtColor(transformedImagePaintableLaneModel,transformedImagePaintableLaneModel,CV_GRAY2BGR); - if (lanePolynomial.hasDetected()) { + if (polyDetectedRight) { int r = lanePolynomial.getLastUsedPosition() == LEFT ? 255 : 0; int g = lanePolynomial.getLastUsedPosition() == CENTER ? 255 : 0; int b = lanePolynomial.getLastUsedPosition() == RIGHT ? 255 : 0; @@ -432,6 +432,9 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Point anglePointLoc = cv::Point(sin(lastAngle * PI / 180) * angleAdjacentLeg + proj_image_w_half, proj_image_h - angleAdjacentLeg); cv::line(transformedImagePaintableLaneModel, pointLoc, anglePointLoc, cv::Scalar(255,255,255)); + cv::Point startNormalPoint = cv::Point(pointForAngle.getX(), pointForAngle.getY()); + cv::circle(transformedImagePaintableLaneModel, startNormalPoint, 2, cv::Scalar(100,100,100), -1); + cv::Point targetPoint = cv::Point(proj_image_w - movedPointForAngle.getX(), movedPointForAngle.getY()); cv::circle(transformedImagePaintableLaneModel, targetPoint, 2, cv::Scalar(0,0,255), -1); @@ -445,7 +448,6 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) double x = 10; double y = gradientForAngle * x + n; - cv::Point startNormalPoint = cv::Point(pointForAngle.getX(), pointForAngle.getY()); cv::Point endNormalPoint = cv::Point(x, y); cv::line(transformedImagePaintableLaneModel, startNormalPoint, endNormalPoint, cv::Scalar(0,0,0)); @@ -2131,8 +2133,8 @@ void cLaneDetectionFu::pubRGBImageMsg(cv::Mat& rgb_mat, image_transport::CameraP } void cLaneDetectionFu::pubAngle() { - if (!lanePolynomial.hasDetected()) - return; + //if (!lanePolynomial.hasDetected()) + // return; //double oppositeLeg = lanePolynomial.getLanePoly().at(proj_image_h - angleAdjacentLeg); // Gegenkathete - oppositeLeg @@ -2154,24 +2156,22 @@ void cLaneDetectionFu::pubAngle() { m = gradient(y, movedPolyRight); } - m = -m; + ROS_ERROR("gradient: %f, atan: %f, atan2: %f", m, atan(-1 / m), atan2(-1, m)); - ROS_ERROR("gradient: %f", m); + shiftPoint(movedPointForAngle, -m, 22.f, y, xRightLane); - //shiftPoint(movedPointForAngle, m, 10.f, y, xRightLane); - - if (m < 0) { - movedPointForAngle.setX(y + 22.f * cos(atan(-1 / m))); + /*if (m > 0) { + movedPointForAngle.setX(y - 22.f * cos(atan(-1 / m))); movedPointForAngle.setY(xRightLane - 22.f * sin(atan(-1 / m))); } else{ movedPointForAngle.setX(y - 22.f * cos(atan(-1 / m))); movedPointForAngle.setY(xRightLane + 22.f * sin(atan(-1 / m))); - } + }*/ pointForAngle.setX(xRightLane); pointForAngle.setY(y); - gradientForAngle = m; + gradientForAngle = -m; //double xRightLane = polyDetectedRight ? polyRight.at(y) : movedPolyRight.at(y); //double xCenterLane = polyDetectedCenter ? polyCenter.at(y) : movedPolyCenter.at(y); From a5346df4d049045d6a5cb07c521d3311a999f1b8 Mon Sep 17 00:00:00 2001 From: lars Date: Sun, 3 Sep 2017 14:33:30 +0200 Subject: [PATCH 35/79] added new debug window to visualize the results of the pubAngle() function --- src/fu_line_detection/src/laneDetection.cpp | 48 +++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index f5c3d907..380e9066 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -389,6 +389,54 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) pubAngle(); + #ifdef PAINT_OUTPUT + cv::Mat angleImg(120, 120, CV_8UC3, Scalar(255, 255, 255)); + if (polyDetectedRight) { + + for(int i = minYPolyRoi; i < maxYRoi; i++) + { + cv::Point pointLocRight = cv::Point(polyRight.at(i), i); + cv::circle(angleImg,pointLocRight,0,cv::Scalar(255,0,0),-1); + } + + cv::Point pointLoc = cv::Point(proj_image_w_half, proj_image_h); + cv::circle(angleImg, pointLoc, 2, cv::Scalar(0,0,255), -1); + + cv::Point anglePointLoc = cv::Point(sin(lastAngle * PI / 180) * angleAdjacentLeg + proj_image_w_half, proj_image_h - angleAdjacentLeg); + cv::line(angleImg, pointLoc, anglePointLoc, cv::Scalar(0,0,255)); + + cv::Point startNormalPoint = cv::Point(pointForAngle.getX(), pointForAngle.getY()); + cv::circle(angleImg, startNormalPoint, 2, cv::Scalar(100,100,100), -1); + + cv::Point targetPoint = cv::Point(proj_image_w - movedPointForAngle.getX(), movedPointForAngle.getY()); + cv::circle(angleImg, targetPoint, 2, cv::Scalar(0,0,255), -1); + + cv::Point adjacentLegPoint = cv::Point(proj_image_w_half, proj_image_h - adjacentLeg); + cv::line(angleImg, pointLoc, adjacentLegPoint, cv::Scalar(255,0,0)); + + cv::Point oppositeLegPoint = cv::Point(proj_image_w_half + oppositeLeg, proj_image_h - adjacentLeg); + cv::line(angleImg, adjacentLegPoint, oppositeLegPoint, cv::Scalar(0,255,0)); + + // tangent + double n = pointForAngle.getY() - (-gradientForAngle) * pointForAngle.getX(); + double x = 10; + double y = gradientForAngle * x + n; + cv::Point endNormalPoint = cv::Point(x, y); + cv::line(angleImg, startNormalPoint, endNormalPoint, cv::Scalar(102,0,204)); + + // normal vector + n = pointForAngle.getY() - gradientForAngle * pointForAngle.getX(); + x = 10; + y = gradientForAngle * x + n; + + endNormalPoint = cv::Point(x, y); + cv::line(angleImg, startNormalPoint, endNormalPoint, cv::Scalar(0,0,0)); + } + cv::namedWindow("pubAngle", WINDOW_NORMAL); + cv::imshow("pubAngle", angleImg); + cv::waitKey(1); + #endif + cv::Mat transformedImagePaintableLaneModel = transformedImage.clone(); cv::cvtColor(transformedImagePaintableLaneModel,transformedImagePaintableLaneModel,CV_GRAY2BGR); From 253c914c51f3fbff9dca1f3ccb226de564bff3a9 Mon Sep 17 00:00:00 2001 From: lars Date: Sun, 3 Sep 2017 15:36:41 +0200 Subject: [PATCH 36/79] fixed display of the tangent --- src/fu_line_detection/src/laneDetection.cpp | 25 ++++++++++++--------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 380e9066..d3be9e63 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -418,18 +418,22 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::line(angleImg, adjacentLegPoint, oppositeLegPoint, cv::Scalar(0,255,0)); // tangent - double n = pointForAngle.getY() - (-gradientForAngle) * pointForAngle.getX(); - double x = 10; - double y = gradientForAngle * x + n; - cv::Point endNormalPoint = cv::Point(x, y); - cv::line(angleImg, startNormalPoint, endNormalPoint, cv::Scalar(102,0,204)); + double mLocal = (-1 / gradientForAngle); + double n = pointForAngle.getY() - mLocal * pointForAngle.getX(); + double x = 0; + double y = mLocal * x + n; + cv::Point startTangent = cv::Point(x, y); + x = 120; + y = mLocal * x + n; + cv::Point endTangent = cv::Point(x, y); + cv::line(angleImg, startTangent, endTangent, cv::Scalar(102,0,204)); // normal vector n = pointForAngle.getY() - gradientForAngle * pointForAngle.getX(); x = 10; y = gradientForAngle * x + n; - endNormalPoint = cv::Point(x, y); + cv::Point endNormalPoint = cv::Point(x, y); cv::line(angleImg, startNormalPoint, endNormalPoint, cv::Scalar(0,0,0)); } cv::namedWindow("pubAngle", WINDOW_NORMAL); @@ -1381,8 +1385,9 @@ int cLaneDetectionFu::horizDistance(FuPoint &p1, FuPoint &p2) { */ double cLaneDetectionFu::gradient(double x, vector> &points, vector coeffs) { - return 2 * coeffs[2] * x + coeffs[1] - coeffs[2] * points[1].getY() - - coeffs[2] * points[0].getY(); + return (2 * coeffs[2] * x + coeffs[1]) + - (coeffs[2] * points[1].getY()) + - (coeffs[2] * points[0].getY()); } double cLaneDetectionFu::gradient(double x, NewtonPolynomial polynomial) { @@ -2191,7 +2196,7 @@ void cLaneDetectionFu::pubAngle() { int y = proj_image_h - angleAdjacentLeg; double xRightLane; - double m; + double m = 0.f; if (polyDetectedRight) { xRightLane = polyRight.at(y); @@ -2206,7 +2211,7 @@ void cLaneDetectionFu::pubAngle() { ROS_ERROR("gradient: %f, atan: %f, atan2: %f", m, atan(-1 / m), atan2(-1, m)); - shiftPoint(movedPointForAngle, -m, 22.f, y, xRightLane); + shiftPoint(movedPointForAngle, m, 22.f, y, xRightLane); /*if (m > 0) { movedPointForAngle.setX(y - 22.f * cos(atan(-1 / m))); From 1043cf8143dd14ff41acc8372efae73467fcca63 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Sun, 3 Sep 2017 19:00:34 +0200 Subject: [PATCH 37/79] shift points adjusted --- src/fu_line_detection/src/laneDetection.cpp | 59 +++++++++++++++------ src/fu_line_detection/src/laneDetection.h | 2 + 2 files changed, 44 insertions(+), 17 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index d3be9e63..5851b9ea 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -402,8 +402,8 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Point pointLoc = cv::Point(proj_image_w_half, proj_image_h); cv::circle(angleImg, pointLoc, 2, cv::Scalar(0,0,255), -1); - cv::Point anglePointLoc = cv::Point(sin(lastAngle * PI / 180) * angleAdjacentLeg + proj_image_w_half, proj_image_h - angleAdjacentLeg); - cv::line(angleImg, pointLoc, anglePointLoc, cv::Scalar(0,0,255)); + //cv::Point anglePointLoc = cv::Point(sin(lastAngle * PI / 180) * angleAdjacentLeg + proj_image_w_half, proj_image_h - angleAdjacentLeg); + //cv::line(angleImg, pointLoc, anglePointLoc, cv::Scalar(0,0,255)); cv::Point startNormalPoint = cv::Point(pointForAngle.getX(), pointForAngle.getY()); cv::circle(angleImg, startNormalPoint, 2, cv::Scalar(100,100,100), -1); @@ -411,14 +411,14 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Point targetPoint = cv::Point(proj_image_w - movedPointForAngle.getX(), movedPointForAngle.getY()); cv::circle(angleImg, targetPoint, 2, cv::Scalar(0,0,255), -1); - cv::Point adjacentLegPoint = cv::Point(proj_image_w_half, proj_image_h - adjacentLeg); - cv::line(angleImg, pointLoc, adjacentLegPoint, cv::Scalar(255,0,0)); + //cv::Point adjacentLegPoint = cv::Point(proj_image_h - adjacentLeg, proj_image_w_half); + //cv::line(angleImg, pointLoc, adjacentLegPoint, cv::Scalar(255,0,0)); - cv::Point oppositeLegPoint = cv::Point(proj_image_w_half + oppositeLeg, proj_image_h - adjacentLeg); - cv::line(angleImg, adjacentLegPoint, oppositeLegPoint, cv::Scalar(0,255,0)); + //cv::Point oppositeLegPoint = cv::Point(proj_image_h - adjacentLeg, proj_image_w_half + oppositeLeg); + //cv::line(angleImg, adjacentLegPoint, oppositeLegPoint, cv::Scalar(0,255,0)); // tangent - double mLocal = (-1 / gradientForAngle); + double mLocal = gradientForAngle; double n = pointForAngle.getY() - mLocal * pointForAngle.getX(); double x = 0; double y = mLocal * x + n; @@ -429,12 +429,27 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::line(angleImg, startTangent, endTangent, cv::Scalar(102,0,204)); // normal vector - n = pointForAngle.getY() - gradientForAngle * pointForAngle.getX(); + /*n = pointForAngle.getY() - gradientForAngle * pointForAngle.getX(); x = 10; y = gradientForAngle * x + n; - cv::Point endNormalPoint = cv::Point(x, y); - cv::line(angleImg, startNormalPoint, endNormalPoint, cv::Scalar(0,0,0)); + cv::Point endNormalPoint = cv::Point(y, x); + cv::line(angleImg, startNormalPoint, endNormalPoint, cv::Scalar(0,0,0));*/ + + + FuPoint test = FuPoint(); + FuPoint test2 = FuPoint(); + shiftPoint(test, -0.9, 5.f, 20, 20); + shiftPoint(test2, 0.9, 10.f, 20, 20); + + cv::Point p1 = cv::Point(20, 20); + cv::circle(angleImg, p1, 2, cv::Scalar(0,0,0), -1); + cv::Point p2 = cv::Point(test.getX(), test.getY()); + cv::circle(angleImg, p2, 2, cv::Scalar(0,0,255), -1); + cv::Point p3 = cv::Point(test2.getX(), test2.getY()); + cv::circle(angleImg, p3, 2, cv::Scalar(0,255,0), -1); + + ROS_ERROR("p2.x: %f, p2.y: %f", test.getX(), test.getY()); } cv::namedWindow("pubAngle", WINDOW_NORMAL); cv::imshow("pubAngle", angleImg); @@ -1026,15 +1041,15 @@ void cLaneDetectionFu::buildLaneMarkingsLists( * @param x * @param y */ -void shiftPoint(FuPoint &p, double m, double offset, int x, int y) +void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, int x, int y) { if (m < 0) { - p.setX(x - offset * cos(atan(-1 / m))); - p.setY(y - offset * sin(atan(-1 / m))); + p.setX(x + offset * cos(atan2(-1, m))); + p.setY(y + offset * sin(atan2(-1, m))); return; } - p.setX(x + offset * cos(atan(-1 / m))); - p.setY(y + offset * sin(atan(-1 / m))); + p.setX(x - offset * cos(atan2(-1, m))); + p.setY(y - offset * sin(atan2(-1, m))); } void cLaneDetectionFu::generateMovedPolynomials() @@ -2211,7 +2226,17 @@ void cLaneDetectionFu::pubAngle() { ROS_ERROR("gradient: %f, atan: %f, atan2: %f", m, atan(-1 / m), atan2(-1, m)); - shiftPoint(movedPointForAngle, m, 22.f, y, xRightLane); + //shiftPoint(movedPointForAngle, m, 22.f, y, xRightLane); + + + if (m < 0) { + movedPointForAngle.setX(y - 22.f * cos(atan(-1 / m))); + movedPointForAngle.setY(xRightLane - 22.f * sin(atan(-1 / m))); + } else { + movedPointForAngle.setX(y + 22.f * cos(atan(-1 / m))); + movedPointForAngle.setY(xRightLane + 22.f * sin(atan(-1 / m))); + } + /*if (m > 0) { movedPointForAngle.setX(y - 22.f * cos(atan(-1 / m))); @@ -2224,7 +2249,7 @@ void cLaneDetectionFu::pubAngle() { pointForAngle.setX(xRightLane); pointForAngle.setY(y); - gradientForAngle = -m; + gradientForAngle = m; //double xRightLane = polyDetectedRight ? polyRight.at(y) : movedPolyRight.at(y); //double xCenterLane = polyDetectedCenter ? polyCenter.at(y) : movedPolyCenter.at(y); diff --git a/src/fu_line_detection/src/laneDetection.h b/src/fu_line_detection/src/laneDetection.h index 7914a0df..0ba8d164 100644 --- a/src/fu_line_detection/src/laneDetection.h +++ b/src/fu_line_detection/src/laneDetection.h @@ -278,6 +278,8 @@ class cLaneDetectionFu void findLanePositions(vector> &laneMarkings); + void shiftPoint(FuPoint &p, double m, double offset, int x, int y); + void generateMovedPolynomials(); bool isInRange(FuPoint &lanePoint, FuPoint &p); From 7471e5c07a31e016cf65a22d71c35a9638a419f8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Mon, 4 Sep 2017 11:01:17 +0200 Subject: [PATCH 38/79] added line for testing --- src/fu_line_detection/src/laneDetection.cpp | 31 +++++++++++---------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 5851b9ea..4e0ea8c3 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -436,20 +436,23 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Point endNormalPoint = cv::Point(y, x); cv::line(angleImg, startNormalPoint, endNormalPoint, cv::Scalar(0,0,0));*/ - - FuPoint test = FuPoint(); - FuPoint test2 = FuPoint(); - shiftPoint(test, -0.9, 5.f, 20, 20); - shiftPoint(test2, 0.9, 10.f, 20, 20); - - cv::Point p1 = cv::Point(20, 20); - cv::circle(angleImg, p1, 2, cv::Scalar(0,0,0), -1); - cv::Point p2 = cv::Point(test.getX(), test.getY()); - cv::circle(angleImg, p2, 2, cv::Scalar(0,0,255), -1); - cv::Point p3 = cv::Point(test2.getX(), test2.getY()); - cv::circle(angleImg, p3, 2, cv::Scalar(0,255,0), -1); - - ROS_ERROR("p2.x: %f, p2.y: %f", test.getX(), test.getY()); + int startX = 20; + int startY = 20; + int endX = 20; + int endY = 30; + + FuPoint shiftStart = FuPoint(); + FuPoint shiftEnd = FuPoint(); + shiftPoint(shiftStart, 0.9, 10.f, startX, startY); + shiftPoint(shiftEnd, 0.9, 10.f, endX, endY); + + cv::Point startP = cv::Point(startX, startY); + cv::Point endP = cv::Point(endX, endY); + cv::line(angleImg, startP, endP, cv::Scalar(0,0,0)); + + cv::Point shiftStartP = cv::Point(shiftStart.getX(), shiftStart.getY()); + cv::Point shiftEndP = cv::Point(shiftEnd.getX(), shiftEnd.getY()); + cv::line(angleImg, shiftStartP, shiftEndP, cv::Scalar(0,0,255)); } cv::namedWindow("pubAngle", WINDOW_NORMAL); cv::imshow("pubAngle", angleImg); From 3c25e91bc72f1f35652a5dc7c1e983e19f9254a9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Mon, 4 Sep 2017 19:39:53 +0200 Subject: [PATCH 39/79] adjusted shift point example --- src/fu_line_detection/src/laneDetection.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 4e0ea8c3..5b668370 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -435,16 +435,20 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Point endNormalPoint = cv::Point(y, x); cv::line(angleImg, startNormalPoint, endNormalPoint, cv::Scalar(0,0,0));*/ - + } + int startX = 20; int startY = 20; int endX = 20; int endY = 30; + double m = -1.0f; + double offset = 10.f; + FuPoint shiftStart = FuPoint(); FuPoint shiftEnd = FuPoint(); - shiftPoint(shiftStart, 0.9, 10.f, startX, startY); - shiftPoint(shiftEnd, 0.9, 10.f, endX, endY); + shiftPoint(shiftStart, m, offset, startX, startY); + shiftPoint(shiftEnd, m, offset, endX, endY); cv::Point startP = cv::Point(startX, startY); cv::Point endP = cv::Point(endX, endY); @@ -453,7 +457,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Point shiftStartP = cv::Point(shiftStart.getX(), shiftStart.getY()); cv::Point shiftEndP = cv::Point(shiftEnd.getX(), shiftEnd.getY()); cv::line(angleImg, shiftStartP, shiftEndP, cv::Scalar(0,0,255)); - } + cv::namedWindow("pubAngle", WINDOW_NORMAL); cv::imshow("pubAngle", angleImg); cv::waitKey(1); From 0426a90f0726db7845f598745434f75faa2d0b4e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Tue, 5 Sep 2017 17:29:26 +0200 Subject: [PATCH 40/79] - added new defines to disable selected debug windows - fixed drawings in lane polynomial window - fixed angle calculation if right poly is available - improved variable names for x and y coordinates (now always image convention is used) --- src/fu_line_detection/src/laneDetection.cpp | 254 +++++------------- .../src/tools/NewtonPolynomial.cpp | 4 +- 2 files changed, 72 insertions(+), 186 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 5b668370..e4e29e4d 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -3,6 +3,10 @@ using namespace std; #define PAINT_OUTPUT +//#define PAINT_OUTPUT_CUT_IMAGE +//#define PAINT_OUTPUT_IP_MAPPED_IMAGE +//#define PAINT_OUTPUT_ROI +//#define PAINT_OUTPUT_LANE_MARKINGS #define PUBLISH_DEBUG_OUTPUT static const uint32_t MY_ROS_QUEUE_SIZE = 1; @@ -186,7 +190,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) Mat cut_image = image(cv::Rect(0,cam_h * 0.25f,cam_w,cam_h * 0.75f)); - #ifdef PAINT_OUTPUT + #ifdef PAINT_OUTPUT_CUT_IMAGE cv::imshow("Cut image", cut_image); cv::waitKey(1); #endif @@ -194,7 +198,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) //Mat cut_image = image(cv::Rect(0,cam_h/2,cam_w,cam_h/2)); Mat remapped_image = ipMapper.remap(cut_image); - #ifdef PAINT_OUTPUT + #ifdef PAINT_OUTPUT_IP_MAPPED_IMAGE cv::imshow("IPmapped image", remapped_image); cv::waitKey(1); #endif @@ -217,7 +221,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Mat transformedImagePaintable; //---------------------- DEBUG OUTPUT EDGES ---------------------------------// - #ifdef PAINT_OUTPUT + #ifdef PAINT_OUTPUT_ROI transformedImagePaintable = transformedImage.clone(); cv::cvtColor(transformedImagePaintable,transformedImagePaintable,CV_GRAY2BGR); for(int i = 0; i < (int)edges.size(); i++) @@ -257,7 +261,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) vector> laneMarkings = cLaneDetectionFu::extractLaneMarkings(edges); //---------------------- DEBUG OUTPUT LANE MARKINGS ---------------------------------// - #ifdef PAINT_OUTPUT + #ifdef PAINT_OUTPUT_LANE_MARKINGS transformedImagePaintable = transformedImage.clone(); cv::cvtColor(transformedImagePaintable,transformedImagePaintable,CV_GRAY2BGR); for(int i = 0; i < (int)laneMarkings.size(); i++) @@ -418,7 +422,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) //cv::line(angleImg, adjacentLegPoint, oppositeLegPoint, cv::Scalar(0,255,0)); // tangent - double mLocal = gradientForAngle; + double mLocal = -1 / gradientForAngle; double n = pointForAngle.getY() - mLocal * pointForAngle.getX(); double x = 0; double y = mLocal * x + n; @@ -437,13 +441,13 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::line(angleImg, startNormalPoint, endNormalPoint, cv::Scalar(0,0,0));*/ } - int startX = 20; - int startY = 20; - int endX = 20; - int endY = 30; + int startX = 20; + int startY = 20; + int endX = 20; + int endY = 30; - double m = -1.0f; - double offset = 10.f; + double m = 1.0f; + double offset = -10.f; FuPoint shiftStart = FuPoint(); FuPoint shiftEnd = FuPoint(); @@ -454,7 +458,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Point endP = cv::Point(endX, endY); cv::line(angleImg, startP, endP, cv::Scalar(0,0,0)); - cv::Point shiftStartP = cv::Point(shiftStart.getX(), shiftStart.getY()); + cv::Point shiftStartP = cv::Point(shiftStart.getX(), shiftStart.getY()); cv::Point shiftEndP = cv::Point(shiftEnd.getX(), shiftEnd.getY()); cv::line(angleImg, shiftStartP, shiftEndP, cv::Scalar(0,0,255)); @@ -509,7 +513,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Point startNormalPoint = cv::Point(pointForAngle.getX(), pointForAngle.getY()); cv::circle(transformedImagePaintableLaneModel, startNormalPoint, 2, cv::Scalar(100,100,100), -1); - cv::Point targetPoint = cv::Point(proj_image_w - movedPointForAngle.getX(), movedPointForAngle.getY()); + cv::Point targetPoint = cv::Point(movedPointForAngle.getX(), movedPointForAngle.getY()); cv::circle(transformedImagePaintableLaneModel, targetPoint, 2, cv::Scalar(0,0,255), -1); cv::Point adjacentLegPoint = cv::Point(proj_image_w_half, proj_image_h - adjacentLeg); @@ -518,9 +522,11 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Point oppositeLegPoint = cv::Point(proj_image_w_half + oppositeLeg, proj_image_h - adjacentLeg); cv::line(transformedImagePaintableLaneModel, adjacentLegPoint, oppositeLegPoint, cv::Scalar(0,255,0)); - double n = pointForAngle.getY() - gradientForAngle * pointForAngle.getX(); + double m = -gradientForAngle; + + double n = pointForAngle.getY() - m * pointForAngle.getX(); double x = 10; - double y = gradientForAngle * x + n; + double y = m * x + n; cv::Point endNormalPoint = cv::Point(x, y); cv::line(transformedImagePaintableLaneModel, startNormalPoint, endNormalPoint, cv::Scalar(0,0,0)); @@ -1045,18 +1051,18 @@ void cLaneDetectionFu::buildLaneMarkingsLists( * @param p * @param m * @param offset Positive if shifting to the left, negative to the right - * @param x * @param y + * @param x */ void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, int x, int y) { - if (m < 0) { - p.setX(x + offset * cos(atan2(-1, m))); - p.setY(y + offset * sin(atan2(-1, m))); + if (m > 0) { + p.setY(y - offset * cos(atan(-1 / m))); + p.setX(x - offset * sin(atan(-1 / m))); return; } - p.setX(x - offset * cos(atan2(-1, m))); - p.setY(y - offset * sin(atan2(-1, m))); + p.setY(y + offset * cos(atan(-1 / m))); + p.setX(x + offset * sin(atan(-1 / m))); } void cLaneDetectionFu::generateMovedPolynomials() @@ -1066,9 +1072,9 @@ void cLaneDetectionFu::generateMovedPolynomials() return; } - int x1 = minYPolyRoi+5; - int x2 = minYPolyRoi + ((proj_image_h-minYPolyRoi)/2); - int x3 = proj_image_h-5; + int y1 = minYPolyRoi+5; + int y2 = minYPolyRoi + ((proj_image_h-minYPolyRoi)/2); + int y3 = proj_image_h-5; // TODO defaultXRight - defaultXCenter double laneWidth = 45.f; @@ -1109,172 +1115,87 @@ void cLaneDetectionFu::generateMovedPolynomials() */ if (polyDetectedRight && !polyDetectedCenter) { usedPoly = polyRight; - m1 = gradient(x1, pointsRight, usedPoly.getCoefficients()); - m2 = gradient(x2, pointsRight, usedPoly.getCoefficients()); - m3 = gradient(x3, pointsRight, usedPoly.getCoefficients()); + m1 = gradient(y1, pointsRight, usedPoly.getCoefficients()); + m2 = gradient(y2, pointsRight, usedPoly.getCoefficients()); + m3 = gradient(y3, pointsRight, usedPoly.getCoefficients()); movedCenter = true; - shiftPoint(pointCenter1,m1, laneWidth, x1, usedPoly.at(x1)); - shiftPoint(pointCenter2,m2, laneWidth, x2, usedPoly.at(x2)); - shiftPoint(pointCenter3,m3, laneWidth, x3, usedPoly.at(x3)); -/* - pointCenter1 = (m1 < 0) - ? FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))) - : FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))); - - - pointCenter2 = (m2 < 0) - ? FuPoint(x2 - laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))) - : FuPoint(x2 + laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))); + shiftPoint(pointCenter1,m1, -laneWidth, usedPoly.at(y1), y1); + shiftPoint(pointCenter2,m2, -laneWidth, usedPoly.at(y2), y2); + shiftPoint(pointCenter3,m3, -laneWidth, usedPoly.at(y3), y3); - - pointCenter3 = (m3 < 0) - ? FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))) - : FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))); -*/ if (!polyDetectedLeft) { movedLeft = true; - shiftPoint(pointLeft1,m1, 2 * laneWidth, x1, usedPoly.at(x1)); - shiftPoint(pointLeft2,m2, 2 * laneWidth, x2, usedPoly.at(x2)); - shiftPoint(pointLeft3,m3, 2 * laneWidth, x3, usedPoly.at(x3)); - -/* - pointLeft1 = (m1 < 0) - ? FuPoint(x1 - 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - 2 * laneWidth * sin(atan(-1 / m1))) - : FuPoint(x1 + 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + 2 * laneWidth * sin(atan(-1 / m1))); - - pointLeft2 = (m2 < 0) - ? FuPoint(x2 - 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - 2 * laneWidth * sin(atan(-1 / m2))) - : FuPoint(x2 + 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + 2 * laneWidth * sin(atan(-1 / m2))); - - - pointLeft3 = (m3 < 0) - ? FuPoint(x3 - 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - 2 * laneWidth * sin(atan(-1 / m3))) - : FuPoint(x3 + 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + 2 * laneWidth * sin(atan(-1 / m3))); -*/ + shiftPoint(pointLeft1,m1, -2 * laneWidth, usedPoly.at(y1), y1); + shiftPoint(pointLeft2,m2, -2 * laneWidth, usedPoly.at(y2), y2); + shiftPoint(pointLeft3,m3, -2 * laneWidth, usedPoly.at(y3), y3); } } else if (polyDetectedLeft && !polyDetectedCenter) { usedPoly = polyLeft; - m1 = gradient(x1, pointsLeft, usedPoly.getCoefficients()); - m2 = gradient(x2, pointsLeft, usedPoly.getCoefficients()); - m3 = gradient(x3, pointsLeft, usedPoly.getCoefficients()); + m1 = gradient(y1, pointsLeft, usedPoly.getCoefficients()); + m2 = gradient(y2, pointsLeft, usedPoly.getCoefficients()); + m3 = gradient(y3, pointsLeft, usedPoly.getCoefficients()); movedCenter = true; - shiftPoint(pointCenter1,m1, -laneWidth, x1, usedPoly.at(x1)); - shiftPoint(pointCenter2,m2, -laneWidth, x2, usedPoly.at(x2)); - shiftPoint(pointCenter3,m3, -laneWidth, x3, usedPoly.at(x3)); - - -/* - pointCenter1 = (m1 < 0) - ? FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))) - : FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))); + shiftPoint(pointCenter1,m1, laneWidth, usedPoly.at(y1), y1); + shiftPoint(pointCenter2,m2, laneWidth, usedPoly.at(y2), y2); + shiftPoint(pointCenter3,m3, laneWidth, usedPoly.at(y3), y3); - pointCenter2 = (m2 < 0) - ? FuPoint(x2 + laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))) - : FuPoint(x2 - laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))); - - pointCenter3 = (m3 < 0) - ? FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))) - : FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))); -*/ if (!polyDetectedRight) { movedRight = true; - shiftPoint(pointRight1,m1, -laneWidth, x1, usedPoly.at(x1)); - shiftPoint(pointRight2,m2, -laneWidth, x2, usedPoly.at(x2)); - shiftPoint(pointRight3,m3, -laneWidth, x3, usedPoly.at(x3)); - -/* - pointRight1 = (m1 < 0) - ? FuPoint(x1 + 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))) - : FuPoint(x1 - 2 * laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))); - - pointRight2 = (m2 < 0) - ? FuPoint(x2 + 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))) - : FuPoint(x2 - 2 * laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))); - - pointRight3 = (m3 < 0) - ? FuPoint(x3 + 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))) - : FuPoint(x3 - 2 * laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))); -*/ + shiftPoint(pointRight1,m1, 2 * laneWidth, usedPoly.at(y1), y1); + shiftPoint(pointRight2,m2, 2 * laneWidth, usedPoly.at(y2), y2); + shiftPoint(pointRight3,m3, 2 * laneWidth, usedPoly.at(y3), y3); } } else if (polyDetectedCenter) { usedPoly = polyCenter; - m1 = gradient(x1, pointsCenter, usedPoly.getCoefficients()); - m2 = gradient(x2, pointsCenter, usedPoly.getCoefficients()); - m3 = gradient(x3, pointsCenter, usedPoly.getCoefficients()); + m1 = gradient(y1, pointsCenter, usedPoly.getCoefficients()); + m2 = gradient(y2, pointsCenter, usedPoly.getCoefficients()); + m3 = gradient(y3, pointsCenter, usedPoly.getCoefficients()); if (!polyDetectedLeft) { //ROS_ERROR("%f, %f, %f", m1, m2, m3); movedLeft = true; - shiftPoint(pointLeft1,m1, laneWidth, x1, usedPoly.at(x1)); - shiftPoint(pointLeft2,m2, laneWidth, x2, usedPoly.at(x2)); - shiftPoint(pointLeft3,m3, laneWidth, x3, usedPoly.at(x3)); - -/* - pointLeft1 = (m1 < 0) - ? FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))) - : FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))); - - pointLeft2 = (m2 < 0) - ? FuPoint(x2 - laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))) - : FuPoint(x2 + laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))); - - pointLeft3 = (m3 < 0) - ? FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))) - : FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))); -*/ + shiftPoint(pointLeft1,m1, -laneWidth, usedPoly.at(y1), y1); + shiftPoint(pointLeft2,m2, -laneWidth, usedPoly.at(y2), y2); + shiftPoint(pointLeft3,m3, -laneWidth, usedPoly.at(y3), y3); } if (!polyDetectedRight) { movedRight = true; - shiftPoint(pointRight1,m1, -laneWidth, x1, usedPoly.at(x1)); - shiftPoint(pointRight2,m2, -laneWidth, x2, usedPoly.at(x2)); - shiftPoint(pointRight3,m3, -laneWidth, x3, usedPoly.at(x3)); - -/* - pointRight1 = (m1 < 0) - ? FuPoint(x1 + laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) + laneWidth * sin(atan(-1 / m1))) - : FuPoint(x1 - laneWidth * cos(atan(-1 / m1)), usedPoly.at(x1) - laneWidth * sin(atan(-1 / m1))); - - pointRight2 = (m2 < 0) - ? FuPoint(x2 + laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) + laneWidth * sin(atan(-1 / m2))) - : FuPoint(x2 - laneWidth * cos(atan(-1 / m2)), usedPoly.at(x2) - laneWidth * sin(atan(-1 / m2))); - - pointRight3 = (m3 < 0) - ? FuPoint(x3 + laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) + laneWidth * sin(atan(-1 / m3))) - : FuPoint(x3 - laneWidth * cos(atan(-1 / m3)), usedPoly.at(x3) - laneWidth * sin(atan(-1 / m3))); -*/ + shiftPoint(pointRight1,m1, laneWidth, usedPoly.at(y1), y1); + shiftPoint(pointRight2,m2, laneWidth, usedPoly.at(y2), y2); + shiftPoint(pointRight3,m3, laneWidth, usedPoly.at(y3), y3); } } // create the lane polynomial out of the shifted points if (movedLeft) { - movedPolyLeft.addDataXY(pointLeft1); - movedPolyLeft.addDataXY(pointLeft2); - movedPolyLeft.addDataXY(pointLeft3); + movedPolyLeft.addData(pointLeft1); + movedPolyLeft.addData(pointLeft2); + movedPolyLeft.addData(pointLeft3); } if (movedCenter) { - movedPolyCenter.addDataXY(pointCenter1); - movedPolyCenter.addDataXY(pointCenter2); - movedPolyCenter.addDataXY(pointCenter3); + movedPolyCenter.addData(pointCenter1); + movedPolyCenter.addData(pointCenter2); + movedPolyCenter.addData(pointCenter3); } if (movedRight) { - movedPolyRight.addDataXY(pointRight1); - movedPolyRight.addDataXY(pointRight2); - movedPolyRight.addDataXY(pointRight3); + movedPolyRight.addData(pointRight1); + movedPolyRight.addData(pointRight2); + movedPolyRight.addData(pointRight3); } } @@ -2208,17 +2129,9 @@ void cLaneDetectionFu::pubRGBImageMsg(cv::Mat& rgb_mat, image_transport::CameraP } void cLaneDetectionFu::pubAngle() { - //if (!lanePolynomial.hasDetected()) - // return; - - //double oppositeLeg = lanePolynomial.getLanePoly().at(proj_image_h - angleAdjacentLeg); - // Gegenkathete - oppositeLeg - // Ankathete - angleAdjacentLeg - //double result = atan(oppositeLeg / angleAdjacentLeg) * 180 / PI; - int y = proj_image_h - angleAdjacentLeg; double xRightLane; - double m = 0.f; + double m; if (polyDetectedRight) { xRightLane = polyRight.at(y); @@ -2231,47 +2144,18 @@ void cLaneDetectionFu::pubAngle() { m = gradient(y, movedPolyRight); } - ROS_ERROR("gradient: %f, atan: %f, atan2: %f", m, atan(-1 / m), atan2(-1, m)); - - //shiftPoint(movedPointForAngle, m, 22.f, y, xRightLane); - - - if (m < 0) { - movedPointForAngle.setX(y - 22.f * cos(atan(-1 / m))); - movedPointForAngle.setY(xRightLane - 22.f * sin(atan(-1 / m))); - } else { - movedPointForAngle.setX(y + 22.f * cos(atan(-1 / m))); - movedPointForAngle.setY(xRightLane + 22.f * sin(atan(-1 / m))); - } - - - /*if (m > 0) { - movedPointForAngle.setX(y - 22.f * cos(atan(-1 / m))); - movedPointForAngle.setY(xRightLane - 22.f * sin(atan(-1 / m))); - } else{ - movedPointForAngle.setX(y - 22.f * cos(atan(-1 / m))); - movedPointForAngle.setY(xRightLane + 22.f * sin(atan(-1 / m))); - }*/ + shiftPoint(movedPointForAngle, m, -22.f, (int) xRightLane, y); pointForAngle.setX(xRightLane); pointForAngle.setY(y); gradientForAngle = m; - //double xRightLane = polyDetectedRight ? polyRight.at(y) : movedPolyRight.at(y); - //double xCenterLane = polyDetectedCenter ? polyCenter.at(y) : movedPolyCenter.at(y); - - oppositeLeg = proj_image_w_half - movedPointForAngle.getX(); + oppositeLeg = movedPointForAngle.getX() - proj_image_w_half; adjacentLeg = proj_image_h - movedPointForAngle.getY(); - /*if (xCenterLane + oppositeLeg < proj_image_w_half) { - oppositeLeg *= -1; - }*/ - double result = atan(oppositeLeg / adjacentLeg) * 180 / PI; - ROS_ERROR("oppositeLeg: %f, angle: %f", oppositeLeg, result); - /* * filter too rash steering angles / jitter in polynomial data */ diff --git a/src/fu_line_detection/src/tools/NewtonPolynomial.cpp b/src/fu_line_detection/src/tools/NewtonPolynomial.cpp index a5124fb9..eaa699f0 100644 --- a/src/fu_line_detection/src/tools/NewtonPolynomial.cpp +++ b/src/fu_line_detection/src/tools/NewtonPolynomial.cpp @@ -122,7 +122,9 @@ NewtonPolynomial& NewtonPolynomial::addData(std::vector> ps) } /** - * computes polynomial at position x + * Computes polynomial at position x of this polynomial. + * Axes of polynomial are swapped, i.e. the x value of + * the polynomial is the y value of the image. * * this uses horners method for computation. * @param x input for polynomial From e3861ad2a3a47c20951b9bc730c796327640a377 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Tue, 5 Sep 2017 18:20:06 +0200 Subject: [PATCH 41/79] improved stability of moved polynomials, adjusted angle calculation if moved poly is used (but still crashes) --- src/fu_line_detection/src/laneDetection.cpp | 82 +++++++++++---------- src/fu_line_detection/src/laneDetection.h | 4 + 2 files changed, 46 insertions(+), 40 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index e4e29e4d..1bfe4723 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -122,6 +122,8 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) pointsCenter = std::vector>(); pointsRight = std::vector>(); + movedPointsRight = std::vector>(); + lanePoly = NewtonPolynomial(); lanePolynomial = LanePolynomial(); @@ -1046,6 +1048,10 @@ void cLaneDetectionFu::buildLaneMarkingsLists( } } +void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, FuPoint &origin) { + shiftPoint(p, m, offset, origin.getX(), origin.getY()); +} + /** * * @param p @@ -1057,12 +1063,12 @@ void cLaneDetectionFu::buildLaneMarkingsLists( void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, int x, int y) { if (m > 0) { - p.setY(y - offset * cos(atan(-1 / m))); p.setX(x - offset * sin(atan(-1 / m))); + p.setY(y - offset * cos(atan(-1 / m))); return; } - p.setY(y + offset * cos(atan(-1 / m))); p.setX(x + offset * sin(atan(-1 / m))); + p.setY(y + offset * cos(atan(-1 / m))); } void cLaneDetectionFu::generateMovedPolynomials() @@ -1072,9 +1078,7 @@ void cLaneDetectionFu::generateMovedPolynomials() return; } - int y1 = minYPolyRoi+5; - int y2 = minYPolyRoi + ((proj_image_h-minYPolyRoi)/2); - int y3 = proj_image_h-5; + movedPointsRight.clear(); // TODO defaultXRight - defaultXCenter double laneWidth = 45.f; @@ -1099,8 +1103,6 @@ void cLaneDetectionFu::generateMovedPolynomials() double m2 = 0; double m3 = 0; - double dRight = 0; - NewtonPolynomial usedPoly; /* @@ -1115,66 +1117,64 @@ void cLaneDetectionFu::generateMovedPolynomials() */ if (polyDetectedRight && !polyDetectedCenter) { usedPoly = polyRight; - m1 = gradient(y1, pointsRight, usedPoly.getCoefficients()); - m2 = gradient(y2, pointsRight, usedPoly.getCoefficients()); - m3 = gradient(y3, pointsRight, usedPoly.getCoefficients()); + m1 = gradient(pointsRight.at(0).getY(), pointsRight, usedPoly.getCoefficients()); + m2 = gradient(pointsRight.at(1).getY(), pointsRight, usedPoly.getCoefficients()); + m3 = gradient(pointsRight.at(2).getY(), pointsRight, usedPoly.getCoefficients()); movedCenter = true; - shiftPoint(pointCenter1,m1, -laneWidth, usedPoly.at(y1), y1); - shiftPoint(pointCenter2,m2, -laneWidth, usedPoly.at(y2), y2); - shiftPoint(pointCenter3,m3, -laneWidth, usedPoly.at(y3), y3); + shiftPoint(pointCenter1,m1, -laneWidth, pointsRight.at(0)); + shiftPoint(pointCenter2,m2, -laneWidth, pointsRight.at(1)); + shiftPoint(pointCenter3,m3, -laneWidth, pointsRight.at(2)); if (!polyDetectedLeft) { movedLeft = true; - shiftPoint(pointLeft1,m1, -2 * laneWidth, usedPoly.at(y1), y1); - shiftPoint(pointLeft2,m2, -2 * laneWidth, usedPoly.at(y2), y2); - shiftPoint(pointLeft3,m3, -2 * laneWidth, usedPoly.at(y3), y3); + shiftPoint(pointLeft1,m1, -2 * laneWidth, pointsRight.at(0)); + shiftPoint(pointLeft2,m2, -2 * laneWidth, pointsRight.at(1)); + shiftPoint(pointLeft3,m3, -2 * laneWidth, pointsRight.at(2)); } } else if (polyDetectedLeft && !polyDetectedCenter) { usedPoly = polyLeft; - m1 = gradient(y1, pointsLeft, usedPoly.getCoefficients()); - m2 = gradient(y2, pointsLeft, usedPoly.getCoefficients()); - m3 = gradient(y3, pointsLeft, usedPoly.getCoefficients()); + m1 = gradient(pointsLeft.at(0).getY(), pointsLeft, usedPoly.getCoefficients()); + m2 = gradient(pointsLeft.at(1).getY(), pointsLeft, usedPoly.getCoefficients()); + m3 = gradient(pointsLeft.at(2).getY(), pointsLeft, usedPoly.getCoefficients()); movedCenter = true; - shiftPoint(pointCenter1,m1, laneWidth, usedPoly.at(y1), y1); - shiftPoint(pointCenter2,m2, laneWidth, usedPoly.at(y2), y2); - shiftPoint(pointCenter3,m3, laneWidth, usedPoly.at(y3), y3); + shiftPoint(pointCenter1,m1, laneWidth, pointsLeft.at(0)); + shiftPoint(pointCenter2,m2, laneWidth, pointsLeft.at(1)); + shiftPoint(pointCenter3,m3, laneWidth, pointsLeft.at(2)); if (!polyDetectedRight) { movedRight = true; - shiftPoint(pointRight1,m1, 2 * laneWidth, usedPoly.at(y1), y1); - shiftPoint(pointRight2,m2, 2 * laneWidth, usedPoly.at(y2), y2); - shiftPoint(pointRight3,m3, 2 * laneWidth, usedPoly.at(y3), y3); + shiftPoint(pointRight1,m1, 2 * laneWidth, pointsLeft.at(0)); + shiftPoint(pointRight2,m2, 2 * laneWidth, pointsLeft.at(1)); + shiftPoint(pointRight3,m3, 2 * laneWidth, pointsLeft.at(2)); } } else if (polyDetectedCenter) { usedPoly = polyCenter; - m1 = gradient(y1, pointsCenter, usedPoly.getCoefficients()); - m2 = gradient(y2, pointsCenter, usedPoly.getCoefficients()); - m3 = gradient(y3, pointsCenter, usedPoly.getCoefficients()); + m1 = gradient(pointsCenter.at(0).getY(), pointsCenter, usedPoly.getCoefficients()); + m2 = gradient(pointsCenter.at(1).getY(), pointsCenter, usedPoly.getCoefficients()); + m3 = gradient(pointsCenter.at(2).getY(), pointsCenter, usedPoly.getCoefficients()); if (!polyDetectedLeft) { - //ROS_ERROR("%f, %f, %f", m1, m2, m3); - movedLeft = true; - shiftPoint(pointLeft1,m1, -laneWidth, usedPoly.at(y1), y1); - shiftPoint(pointLeft2,m2, -laneWidth, usedPoly.at(y2), y2); - shiftPoint(pointLeft3,m3, -laneWidth, usedPoly.at(y3), y3); + shiftPoint(pointLeft1,m1, -laneWidth, pointsCenter.at(0)); + shiftPoint(pointLeft2,m2, -laneWidth, pointsCenter.at(1)); + shiftPoint(pointLeft3,m3, -laneWidth, pointsCenter.at(2)); } if (!polyDetectedRight) { movedRight = true; - shiftPoint(pointRight1,m1, laneWidth, usedPoly.at(y1), y1); - shiftPoint(pointRight2,m2, laneWidth, usedPoly.at(y2), y2); - shiftPoint(pointRight3,m3, laneWidth, usedPoly.at(y3), y3); + shiftPoint(pointRight1,m1, laneWidth, pointsCenter.at(0)); + shiftPoint(pointRight2,m2, laneWidth, pointsCenter.at(1)); + shiftPoint(pointRight3,m3, laneWidth, pointsCenter.at(2)); } } @@ -1196,6 +1196,11 @@ void cLaneDetectionFu::generateMovedPolynomials() movedPolyRight.addData(pointRight1); movedPolyRight.addData(pointRight2); movedPolyRight.addData(pointRight3); + + + movedPointsRight.push_back(FuPoint(pointRight1.getX(), pointRight1.getY())); + movedPointsRight.push_back(FuPoint(pointRight2.getX(), pointRight2.getY())); + movedPointsRight.push_back(FuPoint(pointRight3.getX(), pointRight3.getY())); } } @@ -2137,11 +2142,8 @@ void cLaneDetectionFu::pubAngle() { xRightLane = polyRight.at(y); m = gradient(y, pointsRight, polyRight.getCoefficients()); } else { - lastAngle = 0; - return; - xRightLane = movedPolyRight.at(y); - m = gradient(y, movedPolyRight); + m = gradient(y, movedPointsRight, movedPolyRight.getCoefficients()); } shiftPoint(movedPointForAngle, m, -22.f, (int) xRightLane, y); diff --git a/src/fu_line_detection/src/laneDetection.h b/src/fu_line_detection/src/laneDetection.h index 0ba8d164..c1d30696 100644 --- a/src/fu_line_detection/src/laneDetection.h +++ b/src/fu_line_detection/src/laneDetection.h @@ -237,6 +237,8 @@ class cLaneDetectionFu NewtonPolynomial movedPolyCenter; NewtonPolynomial movedPolyRight; + vector> movedPointsRight; + int laneMarkingSquaredThreshold; int angleAdjacentLeg; @@ -280,6 +282,8 @@ class cLaneDetectionFu void shiftPoint(FuPoint &p, double m, double offset, int x, int y); + void shiftPoint(FuPoint &p, double m, double offset, FuPoint &origin); + void generateMovedPolynomials(); bool isInRange(FuPoint &lanePoint, FuPoint &p); From 8293bcc44ae5cfe849a1a86d516ca89706889dd0 Mon Sep 17 00:00:00 2001 From: lars Date: Wed, 6 Sep 2017 10:27:14 +0200 Subject: [PATCH 42/79] debug build to hunt down an issue regarding proper lane detection and angle publication --- src/fu_line_detection/src/laneDetection.cpp | 80 ++++++++++++++------- src/fu_line_detection/src/laneDetection.h | 4 +- 2 files changed, 56 insertions(+), 28 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 1bfe4723..e45d7e33 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -6,7 +6,7 @@ using namespace std; //#define PAINT_OUTPUT_CUT_IMAGE //#define PAINT_OUTPUT_IP_MAPPED_IMAGE //#define PAINT_OUTPUT_ROI -//#define PAINT_OUTPUT_LANE_MARKINGS +#define PAINT_OUTPUT_LANE_MARKINGS #define PUBLISH_DEBUG_OUTPUT static const uint32_t MY_ROS_QUEUE_SIZE = 1; @@ -90,8 +90,6 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) ipMapper = IPMapper(cam_w, cam_h_half, f_u, f_v, c_u, c_v, cam_deg, cam_height); - - proj_image_w_half = proj_image_w/2; polyDetectedLeft = false; @@ -105,6 +103,7 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) laneMarkingsLeft = std::vector>(); laneMarkingsCenter = std::vector>(); laneMarkingsRight = std::vector>(); + laneMarkingsNotUsed = std::vector>(); polyLeft = NewtonPolynomial(); polyCenter = NewtonPolynomial(); @@ -122,17 +121,21 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) pointsCenter = std::vector>(); pointsRight = std::vector>(); - movedPointsRight = std::vector>(); - lanePoly = NewtonPolynomial(); lanePolynomial = LanePolynomial(); - movedPolyLeft = NewtonPolynomial(); - movedPolyCenter = NewtonPolynomial(); + movedPolyLeft = NewtonPolynomial(); + movedPolyCenter = NewtonPolynomial(); + movedPolyRight = NewtonPolynomial(); + + movedPointsRight = std::vector>(); + movedPointForAngle = FuPoint(); + pointForAngle = FuPoint(); + - maxDistance = 1; + maxDistance = 1; - lastAngle = 0; + lastAngle = 0; head_time_stamp = ros::Time::now(); @@ -278,11 +281,13 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::waitKey(1); #endif //---------------------- END DEBUG OUTPUT LANE MARKINGS ------------------------------// - +ROS_ERROR("findLanePositions"); findLanePositions(laneMarkings); - +ROS_ERROR("/findLanePositions"); +ROS_ERROR("buildLaneMarkingsLists"); // start actual execution buildLaneMarkingsLists(laneMarkings); +ROS_ERROR("/buildLaneMarkingsLists"); //---------------------- DEBUG OUTPUT GROUPED LANE MARKINGS ---------------------------------// #ifdef PUBLISH_DEBUG_OUTPUT @@ -354,8 +359,13 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) #endif #endif //---------------------- END DEBUG OUTPUT GROUPED LANE MARKINGS ------------------------------// - +ROS_ERROR("ransac"); ransac(); +ROS_ERROR("/ransac"); + +ROS_ERROR("generateMovedPolynomials"); + generateMovedPolynomials(); +ROS_ERROR("/generateMovedPolynomials"); //---------------------- DEBUG OUTPUT RANSAC POLYNOMIALS ---------------------------------// #ifdef PUBLISH_DEBUG_OUTPUT @@ -392,8 +402,9 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) //---------------------- END DEBUG OUTPUT RANSAC POLYNOMIALS ------------------------------// //detectLane(); - +ROS_ERROR("pubAngle"); pubAngle(); +ROS_ERROR("/pubAngle"); #ifdef PAINT_OUTPUT cv::Mat angleImg(120, 120, CV_8UC3, Scalar(255, 255, 255)); @@ -442,7 +453,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Point endNormalPoint = cv::Point(y, x); cv::line(angleImg, startNormalPoint, endNormalPoint, cv::Scalar(0,0,0));*/ } - + int startX = 20; int startY = 20; int endX = 20; @@ -942,12 +953,6 @@ void cLaneDetectionFu::buildLaneMarkingsLists( laneMarkingsRight.clear(); laneMarkingsNotUsed.clear(); - movedPolyLeft.clear(); - movedPolyCenter.clear(); - movedPolyRight.clear(); - - generateMovedPolynomials(); - for (FuPoint laneMarking : laneMarkings) { if (movedPolyLeft.isInitialized()) { @@ -1073,11 +1078,15 @@ void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, i void cLaneDetectionFu::generateMovedPolynomials() { - if ((!polyDetectedLeft && !polyDetectedCenter && !polyDetectedRight) - || (polyDetectedLeft && polyDetectedCenter && polyDetectedRight)) { - return; - } + ROS_ERROR("non detected"); + if (!polyDetectedLeft && !polyDetectedCenter && !polyDetectedRight) return; + ROS_ERROR("/non all detected"); + if (polyDetectedLeft && polyDetectedCenter && polyDetectedRight) return; + ROS_ERROR("all detected"); + movedPolyLeft.clear(); + movedPolyCenter.clear(); + movedPolyRight.clear(); movedPointsRight.clear(); // TODO defaultXRight - defaultXCenter @@ -1116,6 +1125,7 @@ void cLaneDetectionFu::generateMovedPolynomials() * that arctan of some gradient equals its angle to the x-axis in degree. */ if (polyDetectedRight && !polyDetectedCenter) { + ROS_ERROR("right"); usedPoly = polyRight; m1 = gradient(pointsRight.at(0).getY(), pointsRight, usedPoly.getCoefficients()); m2 = gradient(pointsRight.at(1).getY(), pointsRight, usedPoly.getCoefficients()); @@ -1134,8 +1144,10 @@ void cLaneDetectionFu::generateMovedPolynomials() shiftPoint(pointLeft2,m2, -2 * laneWidth, pointsRight.at(1)); shiftPoint(pointLeft3,m3, -2 * laneWidth, pointsRight.at(2)); } + ROS_ERROR("/right"); } else if (polyDetectedLeft && !polyDetectedCenter) { + ROS_ERROR("left"); usedPoly = polyLeft; m1 = gradient(pointsLeft.at(0).getY(), pointsLeft, usedPoly.getCoefficients()); m2 = gradient(pointsLeft.at(1).getY(), pointsLeft, usedPoly.getCoefficients()); @@ -1154,8 +1166,10 @@ void cLaneDetectionFu::generateMovedPolynomials() shiftPoint(pointRight2,m2, 2 * laneWidth, pointsLeft.at(1)); shiftPoint(pointRight3,m3, 2 * laneWidth, pointsLeft.at(2)); } + ROS_ERROR("/left"); } else if (polyDetectedCenter) { + ROS_ERROR("center"); usedPoly = polyCenter; m1 = gradient(pointsCenter.at(0).getY(), pointsCenter, usedPoly.getCoefficients()); m2 = gradient(pointsCenter.at(1).getY(), pointsCenter, usedPoly.getCoefficients()); @@ -1169,38 +1183,46 @@ void cLaneDetectionFu::generateMovedPolynomials() shiftPoint(pointLeft3,m3, -laneWidth, pointsCenter.at(2)); } + ROS_ERROR("polyDetectedRight %d", polyDetectedRight); if (!polyDetectedRight) { + ROS_ERROR("!polyDetectedRight"); movedRight = true; shiftPoint(pointRight1,m1, laneWidth, pointsCenter.at(0)); shiftPoint(pointRight2,m2, laneWidth, pointsCenter.at(1)); shiftPoint(pointRight3,m3, laneWidth, pointsCenter.at(2)); } + ROS_ERROR("/center"); } // create the lane polynomial out of the shifted points if (movedLeft) { + ROS_ERROR("movedLeft"); movedPolyLeft.addData(pointLeft1); movedPolyLeft.addData(pointLeft2); movedPolyLeft.addData(pointLeft3); + ROS_ERROR("/movedLeft"); } if (movedCenter) { + ROS_ERROR("movedCenter"); movedPolyCenter.addData(pointCenter1); movedPolyCenter.addData(pointCenter2); movedPolyCenter.addData(pointCenter3); + ROS_ERROR("/movedCenter"); } if (movedRight) { + ROS_ERROR("movedRight"); movedPolyRight.addData(pointRight1); movedPolyRight.addData(pointRight2); movedPolyRight.addData(pointRight3); - movedPointsRight.push_back(FuPoint(pointRight1.getX(), pointRight1.getY())); movedPointsRight.push_back(FuPoint(pointRight2.getX(), pointRight2.getY())); movedPointsRight.push_back(FuPoint(pointRight3.getX(), pointRight3.getY())); + ROS_ERROR("/movedRight"); } } @@ -1333,6 +1355,7 @@ int cLaneDetectionFu::horizDistance(FuPoint &p1, FuPoint &p2) { */ double cLaneDetectionFu::gradient(double x, vector> &points, vector coeffs) { + ROS_ERROR("gradient %f", ((2 * coeffs[2] * x + coeffs[1])-(coeffs[2] * points[1].getY()) - (coeffs[2] * points[0].getY()))); return (2 * coeffs[2] * x + coeffs[1]) - (coeffs[2] * points[1].getY()) - (coeffs[2] * points[0].getY()); @@ -2138,12 +2161,17 @@ void cLaneDetectionFu::pubAngle() { double xRightLane; double m; + ROS_ERROR("polyDetectedRight %d", polyDetectedRight); + if (polyDetectedRight) { + ROS_ERROR(" polyDetectedRight"); xRightLane = polyRight.at(y); m = gradient(y, pointsRight, polyRight.getCoefficients()); } else { + ROS_ERROR(" !polyDetectedRight"); xRightLane = movedPolyRight.at(y); m = gradient(y, movedPointsRight, movedPolyRight.getCoefficients()); + ROS_ERROR(" gradient %f", m); } shiftPoint(movedPointForAngle, m, -22.f, (int) xRightLane, y); @@ -2155,7 +2183,7 @@ void cLaneDetectionFu::pubAngle() { oppositeLeg = movedPointForAngle.getX() - proj_image_w_half; adjacentLeg = proj_image_h - movedPointForAngle.getY(); - + ROS_ERROR(" oppositeLeg %f adjacentLeg %f", oppositeLeg, adjacentLeg); double result = atan(oppositeLeg / adjacentLeg) * 180 / PI; /* diff --git a/src/fu_line_detection/src/laneDetection.h b/src/fu_line_detection/src/laneDetection.h index c1d30696..aec5f9e0 100644 --- a/src/fu_line_detection/src/laneDetection.h +++ b/src/fu_line_detection/src/laneDetection.h @@ -249,8 +249,8 @@ class cLaneDetectionFu int maxAngleDiff; - FuPoint movedPointForAngle = FuPoint(); - FuPoint pointForAngle = FuPoint(); + FuPoint movedPointForAngle; + FuPoint pointForAngle; double oppositeLeg; double adjacentLeg; double gradientForAngle; From 74a8e334a598ad0c823e82529bbf304a344642d3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Wed, 6 Sep 2017 10:39:10 +0200 Subject: [PATCH 43/79] cancel angle calculation if no right polynomial is detected and no polynomial was moved to right lane --- src/fu_line_detection/src/laneDetection.cpp | 6 ++++++ src/fu_line_detection/src/laneDetection.h | 2 ++ 2 files changed, 8 insertions(+) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index e45d7e33..2a3c24db 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -1224,6 +1224,8 @@ void cLaneDetectionFu::generateMovedPolynomials() movedPointsRight.push_back(FuPoint(pointRight3.getX(), pointRight3.getY())); ROS_ERROR("/movedRight"); } + + isPolyMovedRight = movedRight; } bool cLaneDetectionFu::isInRange(FuPoint &lanePoint, FuPoint &p) { @@ -2157,6 +2159,10 @@ void cLaneDetectionFu::pubRGBImageMsg(cv::Mat& rgb_mat, image_transport::CameraP } void cLaneDetectionFu::pubAngle() { + if (!polyDetectedRight && !isPolyMovedRight) { + return; + } + int y = proj_image_h - angleAdjacentLeg; double xRightLane; double m; diff --git a/src/fu_line_detection/src/laneDetection.h b/src/fu_line_detection/src/laneDetection.h index aec5f9e0..8b91f4c4 100644 --- a/src/fu_line_detection/src/laneDetection.h +++ b/src/fu_line_detection/src/laneDetection.h @@ -237,6 +237,8 @@ class cLaneDetectionFu NewtonPolynomial movedPolyCenter; NewtonPolynomial movedPolyRight; + bool isPolyMovedRight; + vector> movedPointsRight; int laneMarkingSquaredThreshold; From b05ec01dad119244793c1bb0ed387817e4509f96 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Thu, 7 Sep 2017 11:47:14 +0200 Subject: [PATCH 44/79] angle calculation finished --- src/fu_line_detection/src/laneDetection.cpp | 91 ++------------------- 1 file changed, 9 insertions(+), 82 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 2a3c24db..4e14ac05 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -406,84 +406,10 @@ ROS_ERROR("pubAngle"); pubAngle(); ROS_ERROR("/pubAngle"); - #ifdef PAINT_OUTPUT - cv::Mat angleImg(120, 120, CV_8UC3, Scalar(255, 255, 255)); - if (polyDetectedRight) { - - for(int i = minYPolyRoi; i < maxYRoi; i++) - { - cv::Point pointLocRight = cv::Point(polyRight.at(i), i); - cv::circle(angleImg,pointLocRight,0,cv::Scalar(255,0,0),-1); - } - - cv::Point pointLoc = cv::Point(proj_image_w_half, proj_image_h); - cv::circle(angleImg, pointLoc, 2, cv::Scalar(0,0,255), -1); - - //cv::Point anglePointLoc = cv::Point(sin(lastAngle * PI / 180) * angleAdjacentLeg + proj_image_w_half, proj_image_h - angleAdjacentLeg); - //cv::line(angleImg, pointLoc, anglePointLoc, cv::Scalar(0,0,255)); - - cv::Point startNormalPoint = cv::Point(pointForAngle.getX(), pointForAngle.getY()); - cv::circle(angleImg, startNormalPoint, 2, cv::Scalar(100,100,100), -1); - - cv::Point targetPoint = cv::Point(proj_image_w - movedPointForAngle.getX(), movedPointForAngle.getY()); - cv::circle(angleImg, targetPoint, 2, cv::Scalar(0,0,255), -1); - - //cv::Point adjacentLegPoint = cv::Point(proj_image_h - adjacentLeg, proj_image_w_half); - //cv::line(angleImg, pointLoc, adjacentLegPoint, cv::Scalar(255,0,0)); - - //cv::Point oppositeLegPoint = cv::Point(proj_image_h - adjacentLeg, proj_image_w_half + oppositeLeg); - //cv::line(angleImg, adjacentLegPoint, oppositeLegPoint, cv::Scalar(0,255,0)); - - // tangent - double mLocal = -1 / gradientForAngle; - double n = pointForAngle.getY() - mLocal * pointForAngle.getX(); - double x = 0; - double y = mLocal * x + n; - cv::Point startTangent = cv::Point(x, y); - x = 120; - y = mLocal * x + n; - cv::Point endTangent = cv::Point(x, y); - cv::line(angleImg, startTangent, endTangent, cv::Scalar(102,0,204)); - - // normal vector - /*n = pointForAngle.getY() - gradientForAngle * pointForAngle.getX(); - x = 10; - y = gradientForAngle * x + n; - - cv::Point endNormalPoint = cv::Point(y, x); - cv::line(angleImg, startNormalPoint, endNormalPoint, cv::Scalar(0,0,0));*/ - } - - int startX = 20; - int startY = 20; - int endX = 20; - int endY = 30; - - double m = 1.0f; - double offset = -10.f; - - FuPoint shiftStart = FuPoint(); - FuPoint shiftEnd = FuPoint(); - shiftPoint(shiftStart, m, offset, startX, startY); - shiftPoint(shiftEnd, m, offset, endX, endY); - - cv::Point startP = cv::Point(startX, startY); - cv::Point endP = cv::Point(endX, endY); - cv::line(angleImg, startP, endP, cv::Scalar(0,0,0)); - - cv::Point shiftStartP = cv::Point(shiftStart.getX(), shiftStart.getY()); - cv::Point shiftEndP = cv::Point(shiftEnd.getX(), shiftEnd.getY()); - cv::line(angleImg, shiftStartP, shiftEndP, cv::Scalar(0,0,255)); - - cv::namedWindow("pubAngle", WINDOW_NORMAL); - cv::imshow("pubAngle", angleImg); - cv::waitKey(1); - #endif - cv::Mat transformedImagePaintableLaneModel = transformedImage.clone(); cv::cvtColor(transformedImagePaintableLaneModel,transformedImagePaintableLaneModel,CV_GRAY2BGR); - if (polyDetectedRight) { + if (polyDetectedRight || isPolyMovedRight) { int r = lanePolynomial.getLastUsedPosition() == LEFT ? 255 : 0; int g = lanePolynomial.getLastUsedPosition() == CENTER ? 255 : 0; int b = lanePolynomial.getLastUsedPosition() == RIGHT ? 255 : 0; @@ -1219,9 +1145,9 @@ void cLaneDetectionFu::generateMovedPolynomials() movedPolyRight.addData(pointRight2); movedPolyRight.addData(pointRight3); - movedPointsRight.push_back(FuPoint(pointRight1.getX(), pointRight1.getY())); - movedPointsRight.push_back(FuPoint(pointRight2.getX(), pointRight2.getY())); - movedPointsRight.push_back(FuPoint(pointRight3.getX(), pointRight3.getY())); + movedPointsRight.push_back(FuPoint(pointRight1.getY(), pointRight1.getX())); + movedPointsRight.push_back(FuPoint(pointRight2.getY(), pointRight2.getX())); + movedPointsRight.push_back(FuPoint(pointRight3.getY(), pointRight3.getX())); ROS_ERROR("/movedRight"); } @@ -2160,7 +2086,8 @@ void cLaneDetectionFu::pubRGBImageMsg(cv::Mat& rgb_mat, image_transport::CameraP void cLaneDetectionFu::pubAngle() { if (!polyDetectedRight && !isPolyMovedRight) { - return; + ROS_ERROR("!polyDetectedRight && !isPolyMovedRight"); + return; } int y = proj_image_h - angleAdjacentLeg; @@ -2170,14 +2097,14 @@ void cLaneDetectionFu::pubAngle() { ROS_ERROR("polyDetectedRight %d", polyDetectedRight); if (polyDetectedRight) { - ROS_ERROR(" polyDetectedRight"); + ROS_ERROR(" polyDetectedRight"); xRightLane = polyRight.at(y); m = gradient(y, pointsRight, polyRight.getCoefficients()); } else { - ROS_ERROR(" !polyDetectedRight"); + ROS_ERROR(" !polyDetectedRight"); xRightLane = movedPolyRight.at(y); m = gradient(y, movedPointsRight, movedPolyRight.getCoefficients()); - ROS_ERROR(" gradient %f", m); + ROS_ERROR(" gradient %f", m); } shiftPoint(movedPointForAngle, m, -22.f, (int) xRightLane, y); From 3d55cd7c7493ab2a823495e6c3533fef3fee3b20 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Thu, 7 Sep 2017 12:14:02 +0200 Subject: [PATCH 45/79] more debugging --- src/fu_line_detection/src/laneDetection.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 4e14ac05..49a33e42 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -7,6 +7,7 @@ using namespace std; //#define PAINT_OUTPUT_IP_MAPPED_IMAGE //#define PAINT_OUTPUT_ROI #define PAINT_OUTPUT_LANE_MARKINGS +//#define PAINT_OUTPUT_LANE_POLYNOMIALS #define PUBLISH_DEBUG_OUTPUT static const uint32_t MY_ROS_QUEUE_SIZE = 1; @@ -391,13 +392,13 @@ ROS_ERROR("/generateMovedPolynomials"); cv::circle(transformedImagePaintableRansac,pointPoly3,0,cv::Scalar(255,128,0),-1); } - pubRGBImageMsg(transformedImagePaintableRansac, image_publisher_ransac); + pubRGBImageMsg(transformedImagePaintableRansac, image_publisher_ransac); - #ifdef PAINT_OUTPUT - cv::namedWindow("RANSAC results", WINDOW_NORMAL); - cv::imshow("RANSAC results", transformedImagePaintableRansac); - cv::waitKey(1); - #endif + #ifdef PAINT_OUTPUT + cv::namedWindow("RANSAC results", WINDOW_NORMAL); + cv::imshow("RANSAC results", transformedImagePaintableRansac); + cv::waitKey(1); + #endif #endif //---------------------- END DEBUG OUTPUT RANSAC POLYNOMIALS ------------------------------// @@ -478,7 +479,7 @@ ROS_ERROR("/pubAngle"); pubRGBImageMsg(transformedImagePaintableLaneModel, image_publisher); //---------------------- DEBUG OUTPUT LANE POLYNOMIAL ---------------------------------// - #ifdef PAINT_OUTPUT + #ifdef PAINT_OUTPUT_LANE_POLYNOMIALS cv::namedWindow("Lane polynomial", WINDOW_NORMAL); cv::imshow("Lane polynomial", transformedImagePaintableLaneModel); cv::waitKey(1); From 72bc1c97ccdb7bbcb80b09626030b6514efa750d Mon Sep 17 00:00:00 2001 From: lars Date: Thu, 7 Sep 2017 17:28:21 +0200 Subject: [PATCH 46/79] added debug_paintPolynom method to allow for easier painting of debug polynoms; the moved polynoms now are painted in the "GroupedLaneMarkings window" regardless if they were moved or not (if they weren't the lane polynoms are painted instead) --- src/fu_line_detection/src/laneDetection.cpp | 54 ++++++++++----------- src/fu_line_detection/src/laneDetection.h | 1 + 2 files changed, 27 insertions(+), 28 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 49a33e42..21ab68c0 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -319,15 +319,10 @@ ROS_ERROR("/buildLaneMarkingsLists"); cv::circle(transformedImagePaintable,markingLoc,1,cv::Scalar(0,255,255),-1); } - for(int i = minYPolyRoi; i < maxYRoi; i++) - { - cv::Point pointPoly1 = cv::Point(movedPolyLeft.at(i), i); - cv::circle(transformedImagePaintable,pointPoly1,0,cv::Scalar(139,0,139),-1); - cv::Point pointPoly2 = cv::Point(movedPolyCenter.at(i), i); - cv::circle(transformedImagePaintable,pointPoly2,0,cv::Scalar(0,0,0),-1); - cv::Point pointPoly3 = cv::Point(movedPolyRight.at(i), i); - cv::circle(transformedImagePaintable,pointPoly3,0,cv::Scalar(255,128,0),-1); - } + debug_paintPolynom(transformedImagePaintable, cv::Scalar(0,0,255), movedPolyLeft, minYPolyRoi, maxYRoi); + debug_paintPolynom(transformedImagePaintable, cv::Scalar(0,255,0), movedPolyCenter, minYPolyRoi, maxYRoi); + debug_paintPolynom(transformedImagePaintable, cv::Scalar(255,0,0), movedPolyRight, minYPolyRoi, maxYRoi); + cv::Point2d p1l(defaultXLeft,minYPolyRoi); cv::Point2d p2l(defaultXLeft,maxYRoi-1); @@ -372,25 +367,15 @@ ROS_ERROR("/generateMovedPolynomials"); #ifdef PUBLISH_DEBUG_OUTPUT cv::Mat transformedImagePaintableRansac = transformedImage.clone(); cv::cvtColor(transformedImagePaintableRansac,transformedImagePaintableRansac,CV_GRAY2BGR); - for(int i = minYPolyRoi; i < maxYRoi; i++) - { - cv::Point pointLocLeft = cv::Point(polyLeft.at(i), i); - cv::circle(transformedImagePaintableRansac,pointLocLeft,0,cv::Scalar(0,0,255),-1); - cv::Point pointLocCenter = cv::Point(polyCenter.at(i), i); - cv::circle(transformedImagePaintableRansac,pointLocCenter,0,cv::Scalar(0,255,0),-1); - cv::Point pointLocRight = cv::Point(polyRight.at(i), i); - cv::circle(transformedImagePaintableRansac,pointLocRight,0,cv::Scalar(255,0,0),-1); - } - for(int i = minYPolyRoi; i < maxYRoi; i++) - { - cv::Point pointPoly1 = cv::Point(movedPolyLeft.at(i), i); - cv::circle(transformedImagePaintableRansac,pointPoly1,0,cv::Scalar(139,0,139),-1); - cv::Point pointPoly2 = cv::Point(movedPolyCenter.at(i), i); - cv::circle(transformedImagePaintableRansac,pointPoly2,0,cv::Scalar(0,0,0),-1); - cv::Point pointPoly3 = cv::Point(movedPolyRight.at(i), i); - cv::circle(transformedImagePaintableRansac,pointPoly3,0,cv::Scalar(255,128,0),-1); - } + debug_paintPolynom(transformedImagePaintableRansac, cv::Scalar(0,0,255), polyLeft, minYPolyRoi, maxYRoi); + debug_paintPolynom(transformedImagePaintableRansac, cv::Scalar(0,255,0), polyCenter, minYPolyRoi, maxYRoi); + debug_paintPolynom(transformedImagePaintableRansac, cv::Scalar(255,0,0), polyRight, minYPolyRoi, maxYRoi); + + debug_paintPolynom(transformedImagePaintableRansac, cv::Scalar(139,0,139), movedPolyLeft, minYPolyRoi, maxYRoi); + debug_paintPolynom(transformedImagePaintableRansac, cv::Scalar(0,0,0), movedPolyCenter, minYPolyRoi, maxYRoi); + debug_paintPolynom(transformedImagePaintableRansac, cv::Scalar(255,120,0), movedPolyRight, minYPolyRoi, maxYRoi); + pubRGBImageMsg(transformedImagePaintableRansac, image_publisher_ransac); @@ -487,7 +472,14 @@ ROS_ERROR("/pubAngle"); //---------------------- END DEBUG OUTPUT LANE POLYNOMIAL ------------------------------// } - +void cLaneDetectionFu::debug_paintPolynom(cv::Mat &m, cv::Scalar color, NewtonPolynomial &p, int start, int end) +{ + cv::Point point; + for (int i = start; i < end; i++) { + point = cv::Point(p.at(i), i); + cv::circle(m, point, 0, color, -1); + } +} /* EdgeDetector methods */ @@ -1016,6 +1008,12 @@ void cLaneDetectionFu::generateMovedPolynomials() movedPolyRight.clear(); movedPointsRight.clear(); + #ifdef PAINT_OUTPUT + movedPolyCenter = polyCenter; + movedPolyLeft = polyLeft; + movedPolyRight = polyRight; + #endif + // TODO defaultXRight - defaultXCenter double laneWidth = 45.f; diff --git a/src/fu_line_detection/src/laneDetection.h b/src/fu_line_detection/src/laneDetection.h index 8b91f4c4..f95f313e 100644 --- a/src/fu_line_detection/src/laneDetection.h +++ b/src/fu_line_detection/src/laneDetection.h @@ -257,6 +257,7 @@ class cLaneDetectionFu double adjacentLeg; double gradientForAngle; + void debug_paintPolynom(cv::Mat &m, cv::Scalar color, NewtonPolynomial &p, int start, int end); public: From 1188a9b2c27f286ed8871564b28468c4957d2441 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Thu, 7 Sep 2017 20:14:54 +0200 Subject: [PATCH 47/79] - save frames to images - print point debug function --- src/fu_line_detection/src/laneDetection.cpp | 107 ++++++++++---------- src/fu_line_detection/src/laneDetection.h | 4 +- 2 files changed, 58 insertions(+), 53 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 21ab68c0..178f3168 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -28,6 +28,9 @@ sensor_msgs::CameraInfoPtr rgb_camera_info; // try kernel width 5 for now const static int g_kernel1DWidth = 5; + +std::size_t frame = 0; + cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") { @@ -183,9 +186,9 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) bestPolyCenter = std::make_pair(NewtonPolynomial(), 0); bestPolyRight = std::make_pair(NewtonPolynomial(), 0); - supportersLeft.clear(); + /*supportersLeft.clear(); supportersCenter.clear(); - supportersRight.clear(); + supportersRight.clear();*/ //use ROS image_proc or opencv instead of ip mapper? @@ -282,47 +285,27 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::waitKey(1); #endif //---------------------- END DEBUG OUTPUT LANE MARKINGS ------------------------------// -ROS_ERROR("findLanePositions"); + ROS_ERROR("findLanePositions"); findLanePositions(laneMarkings); -ROS_ERROR("/findLanePositions"); -ROS_ERROR("buildLaneMarkingsLists"); + ROS_ERROR("/findLanePositions"); + ROS_ERROR("buildLaneMarkingsLists"); // start actual execution buildLaneMarkingsLists(laneMarkings); -ROS_ERROR("/buildLaneMarkingsLists"); + ROS_ERROR("/buildLaneMarkingsLists"); //---------------------- DEBUG OUTPUT GROUPED LANE MARKINGS ---------------------------------// #ifdef PUBLISH_DEBUG_OUTPUT transformedImagePaintable = transformedImage.clone(); cv::cvtColor(transformedImagePaintable,transformedImagePaintable,CV_GRAY2BGR); - for(int i = 0; i < (int)laneMarkingsLeft.size(); i++) - { - FuPoint marking = laneMarkingsLeft[i]; - cv::Point markingLoc = cv::Point(marking.getX(), marking.getY()); - cv::circle(transformedImagePaintable,markingLoc,1,cv::Scalar(0,0,255),-1); - } - for(int i = 0; i < (int)laneMarkingsCenter.size(); i++) - { - FuPoint marking = laneMarkingsCenter[i]; - cv::Point markingLoc = cv::Point(marking.getX(), marking.getY()); - cv::circle(transformedImagePaintable,markingLoc,1,cv::Scalar(0,255,0),-1); - } - for(int i = 0; i < (int)laneMarkingsRight.size(); i++) - { - FuPoint marking = laneMarkingsRight[i]; - cv::Point markingLoc = cv::Point(marking.getX(), marking.getY()); - cv::circle(transformedImagePaintable,markingLoc,1,cv::Scalar(255,0,0),-1); - } - for(int i = 0; i < (int)laneMarkingsNotUsed.size(); i++) - { - FuPoint marking = laneMarkingsNotUsed[i]; - cv::Point markingLoc = cv::Point(marking.getX(), marking.getY()); - cv::circle(transformedImagePaintable,markingLoc,1,cv::Scalar(0,255,255),-1); - } - debug_paintPolynom(transformedImagePaintable, cv::Scalar(0,0,255), movedPolyLeft, minYPolyRoi, maxYRoi); - debug_paintPolynom(transformedImagePaintable, cv::Scalar(0,255,0), movedPolyCenter, minYPolyRoi, maxYRoi); - debug_paintPolynom(transformedImagePaintable, cv::Scalar(255,0,0), movedPolyRight, minYPolyRoi, maxYRoi); + debugPaintPoints(transformedImagePaintable, cv::Scalar(0,0,255), laneMarkingsLeft); + debugPaintPoints(transformedImagePaintable, cv::Scalar(0,255,0), laneMarkingsCenter); + debugPaintPoints(transformedImagePaintable, cv::Scalar(255,0,0), laneMarkingsRight); + debugPaintPoints(transformedImagePaintable, cv::Scalar(0,255,255), laneMarkingsNotUsed); + debugPaintPolynom(transformedImagePaintable, cv::Scalar(0, 255, 255), movedPolyLeft, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintable, cv::Scalar(255, 255, 0), movedPolyCenter, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintable, cv::Scalar(255, 0, 255), movedPolyRight, minYPolyRoi, maxYRoi); cv::Point2d p1l(defaultXLeft,minYPolyRoi); cv::Point2d p2l(defaultXLeft,maxYRoi-1); @@ -345,36 +328,50 @@ ROS_ERROR("/buildLaneMarkingsLists"); cv::line(transformedImagePaintable,p3,p4,cv::Scalar(0,200,0)); cv::line(transformedImagePaintable,p4,p1,cv::Scalar(0,200,0));*/ + debugPaintPoints(transformedImagePaintable, cv::Scalar(0,0,255), supportersLeft); + debugPaintPoints(transformedImagePaintable, cv::Scalar(0,255,0), supportersCenter); + debugPaintPoints(transformedImagePaintable, cv::Scalar(255,0,0), supportersRight); + pubRGBImageMsg(transformedImagePaintable, image_publisher_lane_markings); - #ifdef PAINT_OUTPUT - cv::namedWindow("Grouped Lane Markings", WINDOW_NORMAL); - cv::imshow("Grouped Lane Markings", transformedImagePaintable); - cv::waitKey(1); - #endif + #ifdef PAINT_OUTPUT + cv::namedWindow("Grouped Lane Markings", WINDOW_NORMAL); + cv::imshow("Grouped Lane Markings", transformedImagePaintable); + cv::waitKey(1); + + stringstream img; + img << "./frames/" << frame++ << ".jpg"; + cv::imwrite(img.str(), transformedImagePaintable); + + #endif #endif //---------------------- END DEBUG OUTPUT GROUPED LANE MARKINGS ------------------------------// -ROS_ERROR("ransac"); + + supportersLeft.clear(); + supportersCenter.clear(); + supportersRight.clear(); + + ROS_ERROR("ransac"); ransac(); -ROS_ERROR("/ransac"); + ROS_ERROR("/ransac"); -ROS_ERROR("generateMovedPolynomials"); + ROS_ERROR("generateMovedPolynomials"); generateMovedPolynomials(); -ROS_ERROR("/generateMovedPolynomials"); + ROS_ERROR("/generateMovedPolynomials"); //---------------------- DEBUG OUTPUT RANSAC POLYNOMIALS ---------------------------------// #ifdef PUBLISH_DEBUG_OUTPUT cv::Mat transformedImagePaintableRansac = transformedImage.clone(); cv::cvtColor(transformedImagePaintableRansac,transformedImagePaintableRansac,CV_GRAY2BGR); - debug_paintPolynom(transformedImagePaintableRansac, cv::Scalar(0,0,255), polyLeft, minYPolyRoi, maxYRoi); - debug_paintPolynom(transformedImagePaintableRansac, cv::Scalar(0,255,0), polyCenter, minYPolyRoi, maxYRoi); - debug_paintPolynom(transformedImagePaintableRansac, cv::Scalar(255,0,0), polyRight, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(0, 0, 255), polyLeft, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(0, 255, 0), polyCenter, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(255, 0, 0), polyRight, minYPolyRoi, maxYRoi); - debug_paintPolynom(transformedImagePaintableRansac, cv::Scalar(139,0,139), movedPolyLeft, minYPolyRoi, maxYRoi); - debug_paintPolynom(transformedImagePaintableRansac, cv::Scalar(0,0,0), movedPolyCenter, minYPolyRoi, maxYRoi); - debug_paintPolynom(transformedImagePaintableRansac, cv::Scalar(255,120,0), movedPolyRight, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(139, 0, 139), movedPolyLeft, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(0, 0, 0), movedPolyCenter, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(255, 120, 0), movedPolyRight, minYPolyRoi, maxYRoi); pubRGBImageMsg(transformedImagePaintableRansac, image_publisher_ransac); @@ -388,9 +385,9 @@ ROS_ERROR("/generateMovedPolynomials"); //---------------------- END DEBUG OUTPUT RANSAC POLYNOMIALS ------------------------------// //detectLane(); -ROS_ERROR("pubAngle"); + ROS_ERROR("pubAngle"); pubAngle(); -ROS_ERROR("/pubAngle"); + ROS_ERROR("/pubAngle"); cv::Mat transformedImagePaintableLaneModel = transformedImage.clone(); cv::cvtColor(transformedImagePaintableLaneModel,transformedImagePaintableLaneModel,CV_GRAY2BGR); @@ -472,8 +469,7 @@ ROS_ERROR("/pubAngle"); //---------------------- END DEBUG OUTPUT LANE POLYNOMIAL ------------------------------// } -void cLaneDetectionFu::debug_paintPolynom(cv::Mat &m, cv::Scalar color, NewtonPolynomial &p, int start, int end) -{ +void cLaneDetectionFu::debugPaintPolynom(cv::Mat &m, cv::Scalar color, NewtonPolynomial &p, int start, int end) { cv::Point point; for (int i = start; i < end; i++) { point = cv::Point(p.at(i), i); @@ -481,6 +477,13 @@ void cLaneDetectionFu::debug_paintPolynom(cv::Mat &m, cv::Scalar color, NewtonPo } } +void cLaneDetectionFu::debugPaintPoints(cv::Mat &m, cv::Scalar color, vector> &points) { + for(FuPoint point : points) { + cv::Point pointLoc = cv::Point(point.getX(), point.getY()); + cv::circle(m, pointLoc, 1, color, -1); + } +} + /* EdgeDetector methods */ /** diff --git a/src/fu_line_detection/src/laneDetection.h b/src/fu_line_detection/src/laneDetection.h index f95f313e..119f21ab 100644 --- a/src/fu_line_detection/src/laneDetection.h +++ b/src/fu_line_detection/src/laneDetection.h @@ -257,7 +257,9 @@ class cLaneDetectionFu double adjacentLeg; double gradientForAngle; - void debug_paintPolynom(cv::Mat &m, cv::Scalar color, NewtonPolynomial &p, int start, int end); + void debugPaintPolynom(cv::Mat &m, cv::Scalar color, NewtonPolynomial &p, int start, int end); + + void debugPaintPoints(cv::Mat &m, cv::Scalar color, std::vector> &points); public: From 51cbaa3df3ed5f1773d0c7aa203818ee0f2cd073 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Fri, 8 Sep 2017 13:44:50 +0200 Subject: [PATCH 48/79] - improved params value choice - added publish debug images --- src/fu_line_detection/cfg/LaneDetection.cfg | 2 +- .../launch/line_detection_fu.launch | 6 +- src/fu_line_detection/src/laneDetection.cpp | 145 ++++++++++++------ src/fu_line_detection/src/laneDetection.h | 8 +- 4 files changed, 108 insertions(+), 53 deletions(-) diff --git a/src/fu_line_detection/cfg/LaneDetection.cfg b/src/fu_line_detection/cfg/LaneDetection.cfg index d9c594de..55915ecd 100755 --- a/src/fu_line_detection/cfg/LaneDetection.cfg +++ b/src/fu_line_detection/cfg/LaneDetection.cfg @@ -5,7 +5,7 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("interestDistancePoly", int_t, 0, "Accepted horizontal distance between a potential point and a detected polynomial", 5, 0, 100) +gen.add("interestDistancePoly", int_t, 0, "Accepted horizontal distance between a potential point and a detected polynomial", 20, 0, 100) gen.add("interestDistanceDefault", int_t, 0, "Accepted horizontal distance between a potential point and a default line", 20, 0, 100) gen.add("iterationsRansac", int_t, 0, "Iterations during ransac", 30, 0, 100) gen.add("maxYRoi", int_t, 0, "Maximum y value for points on a line", 159, 0, 160) diff --git a/src/fu_line_detection/launch/line_detection_fu.launch b/src/fu_line_detection/launch/line_detection_fu.launch index 7f48d213..cc799eec 100644 --- a/src/fu_line_detection/launch/line_detection_fu.launch +++ b/src/fu_line_detection/launch/line_detection_fu.launch @@ -22,9 +22,9 @@ - - - + + + diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 178f3168..74b2a1ef 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -1,3 +1,4 @@ +#include #include "laneDetection.h" using namespace std; @@ -9,6 +10,7 @@ using namespace std; #define PAINT_OUTPUT_LANE_MARKINGS //#define PAINT_OUTPUT_LANE_POLYNOMIALS #define PUBLISH_DEBUG_OUTPUT +#define PUBLISH_DEBUG_IMAGES static const uint32_t MY_ROS_QUEUE_SIZE = 1; @@ -171,6 +173,14 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) //the outer vector represents rows on image, inner vector is vector of line segments of one row, usualy just one line segment //we should generate this only once in the beginning! or even just have it pregenerated for our cam scanlines = getScanlines(); + + #ifdef PUBLISH_DEBUG_IMAGES + mkdir("groupedLaneMarkings", S_IRWXU); + mkdir("ransac", S_IRWXU); + + system("exec rm -r ./groupedLaneMarkings/*"); + system("exec rm -r ./ransac/*"); + #endif } cLaneDetectionFu::~cLaneDetectionFu() @@ -303,9 +313,9 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) debugPaintPoints(transformedImagePaintable, cv::Scalar(255,0,0), laneMarkingsRight); debugPaintPoints(transformedImagePaintable, cv::Scalar(0,255,255), laneMarkingsNotUsed); - debugPaintPolynom(transformedImagePaintable, cv::Scalar(0, 255, 255), movedPolyLeft, minYPolyRoi, maxYRoi); - debugPaintPolynom(transformedImagePaintable, cv::Scalar(255, 255, 0), movedPolyCenter, minYPolyRoi, maxYRoi); - debugPaintPolynom(transformedImagePaintable, cv::Scalar(255, 0, 255), movedPolyRight, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintable, cv::Scalar(0, 255, 255), polyDetectedLeft ? polyLeft : movedPolyLeft, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintable, cv::Scalar(255, 255, 0), polyDetectedCenter ? polyCenter : movedPolyCenter, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintable, cv::Scalar(255, 0, 255), polyDetectedRight ? polyRight : movedPolyRight, minYPolyRoi, maxYRoi); cv::Point2d p1l(defaultXLeft,minYPolyRoi); cv::Point2d p2l(defaultXLeft,maxYRoi-1); @@ -340,10 +350,9 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::imshow("Grouped Lane Markings", transformedImagePaintable); cv::waitKey(1); - stringstream img; - img << "./frames/" << frame++ << ".jpg"; - cv::imwrite(img.str(), transformedImagePaintable); - + #ifdef PUBLISH_DEBUG_IMAGES + debugWriteImg(transformedImagePaintable, "groupedLaneMarkings"); + #endif #endif #endif //---------------------- END DEBUG OUTPUT GROUPED LANE MARKINGS ------------------------------// @@ -380,6 +389,10 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::namedWindow("RANSAC results", WINDOW_NORMAL); cv::imshow("RANSAC results", transformedImagePaintableRansac); cv::waitKey(1); + + #ifdef PUBLISH_DEBUG_IMAGES + debugWriteImg(transformedImagePaintableRansac, "ransac"); + #endif #endif #endif //---------------------- END DEBUG OUTPUT RANSAC POLYNOMIALS ------------------------------// @@ -467,6 +480,8 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::waitKey(1); #endif //---------------------- END DEBUG OUTPUT LANE POLYNOMIAL ------------------------------// + + frame++; } void cLaneDetectionFu::debugPaintPolynom(cv::Mat &m, cv::Scalar color, NewtonPolynomial &p, int start, int end) { @@ -484,6 +499,12 @@ void cLaneDetectionFu::debugPaintPoints(cv::Mat &m, cv::Scalar color, vector &p, double m, double offset, i void cLaneDetectionFu::generateMovedPolynomials() { - ROS_ERROR("non detected"); - if (!polyDetectedLeft && !polyDetectedCenter && !polyDetectedRight) return; - ROS_ERROR("/non all detected"); - if (polyDetectedLeft && polyDetectedCenter && polyDetectedRight) return; - ROS_ERROR("all detected"); - movedPolyLeft.clear(); movedPolyCenter.clear(); movedPolyRight.clear(); movedPointsRight.clear(); - #ifdef PAINT_OUTPUT - movedPolyCenter = polyCenter; - movedPolyLeft = polyLeft; - movedPolyRight = polyRight; - #endif + isPolyMovedLeft = false; + isPolyMovedCenter = false; + isPolyMovedRight = false; + + ROS_ERROR("non detected"); + if (!polyDetectedLeft && !polyDetectedCenter && !polyDetectedRight) return; + ROS_ERROR("/non all detected"); + if (polyDetectedLeft && polyDetectedCenter && polyDetectedRight) return; + ROS_ERROR("all detected"); // TODO defaultXRight - defaultXCenter double laneWidth = 45.f; @@ -1032,10 +1051,6 @@ void cLaneDetectionFu::generateMovedPolynomials() FuPoint pointRight2 = FuPoint(); FuPoint pointRight3 = FuPoint(); - bool movedLeft = false; - bool movedCenter = false; - bool movedRight = false; - double m1 = 0; double m2 = 0; double m3 = 0; @@ -1059,14 +1074,14 @@ void cLaneDetectionFu::generateMovedPolynomials() m2 = gradient(pointsRight.at(1).getY(), pointsRight, usedPoly.getCoefficients()); m3 = gradient(pointsRight.at(2).getY(), pointsRight, usedPoly.getCoefficients()); - movedCenter = true; + isPolyMovedCenter = true; shiftPoint(pointCenter1,m1, -laneWidth, pointsRight.at(0)); shiftPoint(pointCenter2,m2, -laneWidth, pointsRight.at(1)); shiftPoint(pointCenter3,m3, -laneWidth, pointsRight.at(2)); if (!polyDetectedLeft) { - movedLeft = true; + isPolyMovedLeft = true; shiftPoint(pointLeft1,m1, -2 * laneWidth, pointsRight.at(0)); shiftPoint(pointLeft2,m2, -2 * laneWidth, pointsRight.at(1)); @@ -1081,14 +1096,14 @@ void cLaneDetectionFu::generateMovedPolynomials() m2 = gradient(pointsLeft.at(1).getY(), pointsLeft, usedPoly.getCoefficients()); m3 = gradient(pointsLeft.at(2).getY(), pointsLeft, usedPoly.getCoefficients()); - movedCenter = true; + isPolyMovedCenter = true; shiftPoint(pointCenter1,m1, laneWidth, pointsLeft.at(0)); shiftPoint(pointCenter2,m2, laneWidth, pointsLeft.at(1)); shiftPoint(pointCenter3,m3, laneWidth, pointsLeft.at(2)); if (!polyDetectedRight) { - movedRight = true; + isPolyMovedRight = true; shiftPoint(pointRight1,m1, 2 * laneWidth, pointsLeft.at(0)); shiftPoint(pointRight2,m2, 2 * laneWidth, pointsLeft.at(1)); @@ -1104,7 +1119,7 @@ void cLaneDetectionFu::generateMovedPolynomials() m3 = gradient(pointsCenter.at(2).getY(), pointsCenter, usedPoly.getCoefficients()); if (!polyDetectedLeft) { - movedLeft = true; + isPolyMovedLeft = true; shiftPoint(pointLeft1,m1, -laneWidth, pointsCenter.at(0)); shiftPoint(pointLeft2,m2, -laneWidth, pointsCenter.at(1)); @@ -1114,7 +1129,7 @@ void cLaneDetectionFu::generateMovedPolynomials() ROS_ERROR("polyDetectedRight %d", polyDetectedRight); if (!polyDetectedRight) { ROS_ERROR("!polyDetectedRight"); - movedRight = true; + isPolyMovedRight = true; shiftPoint(pointRight1,m1, laneWidth, pointsCenter.at(0)); shiftPoint(pointRight2,m2, laneWidth, pointsCenter.at(1)); @@ -1125,7 +1140,7 @@ void cLaneDetectionFu::generateMovedPolynomials() // create the lane polynomial out of the shifted points - if (movedLeft) { + if (isPolyMovedLeft) { ROS_ERROR("movedLeft"); movedPolyLeft.addData(pointLeft1); movedPolyLeft.addData(pointLeft2); @@ -1133,7 +1148,7 @@ void cLaneDetectionFu::generateMovedPolynomials() ROS_ERROR("/movedLeft"); } - if (movedCenter) { + if (isPolyMovedCenter) { ROS_ERROR("movedCenter"); movedPolyCenter.addData(pointCenter1); movedPolyCenter.addData(pointCenter2); @@ -1141,7 +1156,7 @@ void cLaneDetectionFu::generateMovedPolynomials() ROS_ERROR("/movedCenter"); } - if (movedRight) { + if (isPolyMovedRight) { ROS_ERROR("movedRight"); movedPolyRight.addData(pointRight1); movedPolyRight.addData(pointRight2); @@ -1152,8 +1167,6 @@ void cLaneDetectionFu::generateMovedPolynomials() movedPointsRight.push_back(FuPoint(pointRight3.getY(), pointRight3.getX())); ROS_ERROR("/movedRight"); } - - isPolyMovedRight = movedRight; } bool cLaneDetectionFu::isInRange(FuPoint &lanePoint, FuPoint &p) { @@ -2012,23 +2025,31 @@ bool cLaneDetectionFu::polyValid(ePosition position, NewtonPolynomial poly, NewtonPolynomial prevPoly) { if (prevPoly.getDegree() != -1) { - FuPoint p4 = FuPoint(poly.at(polyY1), polyY1); - FuPoint p5 = FuPoint(prevPoly.at(polyY1), polyY1); - - if (horizDistance(p4, p5) > 5) {//0.05 * meters) { - //ROS_INFO("Poly was to far away from previous poly at y = 25"); - return false; + return isSimilar(poly, prevPoly); + } + if (position == RIGHT) { + if (polyDetectedRight) { + return isSimilar(poly, polyRight); } - - FuPoint p6 = FuPoint(poly.at(polyY2), polyY2); - FuPoint p7 = FuPoint(prevPoly.at(polyY2), polyY2); - - if (horizDistance(p6, p7) > 5) {//0.05 * meters) { - //ROS_INFO("Poly was to far away from previous poly at y = 30"); - return false; + if (isPolyMovedRight) { + return isSimilar(poly, movedPolyRight); + } + } + if (position == CENTER) { + if (polyDetectedCenter) { + return isSimilar(poly, polyCenter); + } + if (isPolyMovedCenter) { + return isSimilar(poly, movedPolyCenter); + } + } + if (position == LEFT) { + if (polyDetectedLeft) { + return isSimilar(poly, polyLeft); + } + if (isPolyMovedLeft) { + return isSimilar(poly, movedPolyLeft); } - - return true; } /*FuPoint p1 = FuPoint(poly.at(polyY1), polyY1); //y = 75 @@ -2057,7 +2078,35 @@ bool cLaneDetectionFu::polyValid(ePosition position, NewtonPolynomial poly, return true; } +bool cLaneDetectionFu::isSimilar(const NewtonPolynomial &poly1, const NewtonPolynomial &poly2) { + //ROS_ERROR("y1: %d, y2: %d, y3: %d", polyY1, polyY2, polyY3); + FuPoint p1 = FuPoint(poly1.at(polyY1), polyY1); + FuPoint p2 = FuPoint(poly2.at(polyY1), polyY1); + + if (horizDistance(p1, p2) > 5) {//0.05 * meters) { + //ROS_INFO("Poly was to far away from previous poly at y = 25"); + return false; + } + + FuPoint p3 = FuPoint(poly1.at(polyY2), polyY2); + FuPoint p4 = FuPoint(poly2.at(polyY2), polyY2); + + if (horizDistance(p3, p4) > 5) {//0.05 * meters) { + //ROS_INFO("Poly was to far away from previous poly at y = 30"); + return false; + } + + FuPoint p5 = FuPoint(poly1.at(polyY3), polyY3); + FuPoint p6 = FuPoint(poly2.at(polyY3), polyY3); + + if (horizDistance(p5, p6) > 5) {//0.05 * meters) { + //ROS_INFO("Poly was to far away from previous poly at y = 30"); + return false; + } + + return true; +} void cLaneDetectionFu::pubRGBImageMsg(cv::Mat& rgb_mat, image_transport::CameraPublisher publisher) { diff --git a/src/fu_line_detection/src/laneDetection.h b/src/fu_line_detection/src/laneDetection.h index 119f21ab..e5d6e80b 100644 --- a/src/fu_line_detection/src/laneDetection.h +++ b/src/fu_line_detection/src/laneDetection.h @@ -237,7 +237,9 @@ class cLaneDetectionFu NewtonPolynomial movedPolyCenter; NewtonPolynomial movedPolyRight; - bool isPolyMovedRight; + bool isPolyMovedRight = false; + bool isPolyMovedCenter = false; + bool isPolyMovedLeft = false; vector> movedPointsRight; @@ -261,6 +263,8 @@ class cLaneDetectionFu void debugPaintPoints(cv::Mat &m, cv::Scalar color, std::vector> &points); + void debugWriteImg(cv::Mat &image, string folder); + public: cLaneDetectionFu(ros::NodeHandle nh); @@ -311,6 +315,8 @@ class cLaneDetectionFu bool polyValid(ePosition, NewtonPolynomial, NewtonPolynomial); + bool isSimilar(const NewtonPolynomial &poly1, const NewtonPolynomial &poly2); + int horizDistance(FuPoint &p1, FuPoint &p2); void createLanePoly(ePosition position); From b671b8613f1ee55d419f95e20dc3239c36b26998 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Fri, 8 Sep 2017 14:33:22 +0200 Subject: [PATCH 49/79] param "interestDistancePoly" now also used in isSimilar function to check if ransac poly is valid --- src/fu_line_detection/cfg/LaneDetection.cfg | 2 +- src/fu_line_detection/src/laneDetection.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/fu_line_detection/cfg/LaneDetection.cfg b/src/fu_line_detection/cfg/LaneDetection.cfg index 55915ecd..5328b730 100755 --- a/src/fu_line_detection/cfg/LaneDetection.cfg +++ b/src/fu_line_detection/cfg/LaneDetection.cfg @@ -5,7 +5,7 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("interestDistancePoly", int_t, 0, "Accepted horizontal distance between a potential point and a detected polynomial", 20, 0, 100) +gen.add("interestDistancePoly", int_t, 0, "Accepted horizontal distance between a potential point and a detected polynomial", 10, 0, 100) gen.add("interestDistanceDefault", int_t, 0, "Accepted horizontal distance between a potential point and a default line", 20, 0, 100) gen.add("iterationsRansac", int_t, 0, "Iterations during ransac", 30, 0, 100) gen.add("maxYRoi", int_t, 0, "Maximum y value for points on a line", 159, 0, 160) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 74b2a1ef..a6aa11c4 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -2084,7 +2084,7 @@ bool cLaneDetectionFu::isSimilar(const NewtonPolynomial &poly1, const NewtonPoly FuPoint p1 = FuPoint(poly1.at(polyY1), polyY1); FuPoint p2 = FuPoint(poly2.at(polyY1), polyY1); - if (horizDistance(p1, p2) > 5) {//0.05 * meters) { + if (horizDistance(p1, p2) > interestDistancePoly) {//0.05 * meters) { //ROS_INFO("Poly was to far away from previous poly at y = 25"); return false; } @@ -2092,7 +2092,7 @@ bool cLaneDetectionFu::isSimilar(const NewtonPolynomial &poly1, const NewtonPoly FuPoint p3 = FuPoint(poly1.at(polyY2), polyY2); FuPoint p4 = FuPoint(poly2.at(polyY2), polyY2); - if (horizDistance(p3, p4) > 5) {//0.05 * meters) { + if (horizDistance(p3, p4) > interestDistancePoly) {//0.05 * meters) { //ROS_INFO("Poly was to far away from previous poly at y = 30"); return false; } @@ -2100,7 +2100,7 @@ bool cLaneDetectionFu::isSimilar(const NewtonPolynomial &poly1, const NewtonPoly FuPoint p5 = FuPoint(poly1.at(polyY3), polyY3); FuPoint p6 = FuPoint(poly2.at(polyY3), polyY3); - if (horizDistance(p5, p6) > 5) {//0.05 * meters) { + if (horizDistance(p5, p6) > interestDistancePoly) {//0.05 * meters) { //ROS_INFO("Poly was to far away from previous poly at y = 30"); return false; } From 787435c505267d987e8c9f88d41bf822b9db684d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Fri, 8 Sep 2017 14:44:12 +0200 Subject: [PATCH 50/79] fixed shift point function when gradient is zero --- src/fu_line_detection/src/laneDetection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index a6aa11c4..fe540dde 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -1010,7 +1010,7 @@ void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, F */ void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, int x, int y) { - if (m > 0) { + if (m >= 0) { p.setX(x - offset * sin(atan(-1 / m))); p.setY(y - offset * cos(atan(-1 / m))); return; From b36680792afded0a3bff665b9c0d06b9f2a8f14b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Sat, 9 Sep 2017 18:08:56 +0200 Subject: [PATCH 51/79] - refined build lane markings loop - lane with is calculated dynamically --- src/fu_line_detection/src/laneDetection.cpp | 84 ++++++++++----------- src/fu_line_detection/src/laneDetection.h | 2 + 2 files changed, 42 insertions(+), 44 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index fe540dde..48b938f8 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -8,9 +8,9 @@ using namespace std; //#define PAINT_OUTPUT_IP_MAPPED_IMAGE //#define PAINT_OUTPUT_ROI #define PAINT_OUTPUT_LANE_MARKINGS -//#define PAINT_OUTPUT_LANE_POLYNOMIALS +#define PAINT_OUTPUT_LANE_POLYNOMIALS #define PUBLISH_DEBUG_OUTPUT -#define PUBLISH_DEBUG_IMAGES +//#define PUBLISH_DEBUG_IMAGES static const uint32_t MY_ROS_QUEUE_SIZE = 1; @@ -828,7 +828,8 @@ void cLaneDetectionFu::findLanePositions(vector> &laneMarkings) { } if (defaultXCenter > 0 && defaultXRight > 0) { - defaultXLeft = defaultXCenter - (defaultXRight - defaultXCenter); + laneWidth = defaultXRight - defaultXCenter; + defaultXLeft = defaultXCenter - (int) laneWidth; } ROS_ERROR("left: %d, center: %d, right: %d", defaultXLeft, defaultXCenter, defaultXRight); @@ -898,6 +899,31 @@ void cLaneDetectionFu::buildLaneMarkingsLists( for (FuPoint laneMarking : laneMarkings) { + // check if lane marking point is near to found lane poly of ransac + + if (polyDetectedRight) { + if (isInPolyRoi(polyRight, laneMarking)) { + laneMarkingsRight.push_back(laneMarking); + continue; + } + } + + if (polyDetectedCenter) { + if (isInPolyRoi(polyCenter, laneMarking)) { + laneMarkingsCenter.push_back(laneMarking); + continue; + } + } + + if (polyDetectedLeft) { + if (isInPolyRoi(polyLeft, laneMarking)) { + laneMarkingsLeft.push_back(laneMarking); + continue; + } + } + + // check if lane marking point is near to moved poly of ransac + if (movedPolyLeft.isInitialized()) { if (isInPolyRoi(movedPolyLeft, laneMarking)) { laneMarkingsLeft.push_back(laneMarking); @@ -919,6 +945,15 @@ void cLaneDetectionFu::buildLaneMarkingsLists( } } + // if ransac found a polynomial in last frame skip default lane comparison + if (polyDetectedLeft || polyDetectedCenter || polyDetectedRight) { + continue; + } + + // no poly available from last frame, check if lane marking point is near to + // default lane or near to already classified point (this way points are also + // classified properly if car starts in a turn) + if (laneMarkingsRight.size() > 0) { if (isInRange(laneMarkingsRight.at(laneMarkingsRight.size() - 1), laneMarking)) { laneMarkingsRight.push_back(laneMarking); @@ -940,27 +975,6 @@ void cLaneDetectionFu::buildLaneMarkingsLists( } } - if (polyDetectedRight) { - if (isInPolyRoi(polyRight, laneMarking)) { - laneMarkingsRight.push_back(laneMarking); - continue; - } - } - - if (polyDetectedCenter) { - if (isInPolyRoi(polyCenter, laneMarking)) { - laneMarkingsCenter.push_back(laneMarking); - continue; - } - } - - if (polyDetectedLeft) { - if (isInPolyRoi(polyLeft, laneMarking)) { - laneMarkingsLeft.push_back(laneMarking); - continue; - } - } - if (isInDefaultRoi(RIGHT, laneMarking)) { laneMarkingsRight.push_back(laneMarking); continue; @@ -976,22 +990,6 @@ void cLaneDetectionFu::buildLaneMarkingsLists( continue; } - /*if (laneMarkingsRight.size() == 0) { - - } else { - - - double x1 = laneMarking.getX(); - double x2 = laneMarkingsRight.at(laneMarkingsRight.size() - 1).getX(); - double y1 = laneMarking.getY(); - double y2 = laneMarkingsRight.at(laneMarkingsRight.size() - 1).getY(); - - double x = std::abs(x1 - x2); - double y = std::abs(y1 - y2); - - ROS_ERROR("x1: %f, x2: %f, y1: %f, y2: %f, x: %f, y: %f", x1, x2, y1, y2, x, y); - }*/ - laneMarkingsNotUsed.push_back(laneMarking); } } @@ -1036,9 +1034,6 @@ void cLaneDetectionFu::generateMovedPolynomials() if (polyDetectedLeft && polyDetectedCenter && polyDetectedRight) return; ROS_ERROR("all detected"); - // TODO defaultXRight - defaultXCenter - double laneWidth = 45.f; - FuPoint pointLeft1 = FuPoint(); FuPoint pointLeft2 = FuPoint(); FuPoint pointLeft3 = FuPoint(); @@ -2158,7 +2153,8 @@ void cLaneDetectionFu::pubAngle() { ROS_ERROR(" gradient %f", m); } - shiftPoint(movedPointForAngle, m, -22.f, (int) xRightLane, y); + double offset = -1 * laneWidth / 2; + shiftPoint(movedPointForAngle, m, offset, (int) xRightLane, y); pointForAngle.setX(xRightLane); pointForAngle.setY(y); diff --git a/src/fu_line_detection/src/laneDetection.h b/src/fu_line_detection/src/laneDetection.h index e5d6e80b..65be997f 100644 --- a/src/fu_line_detection/src/laneDetection.h +++ b/src/fu_line_detection/src/laneDetection.h @@ -259,6 +259,8 @@ class cLaneDetectionFu double adjacentLeg; double gradientForAngle; + double laneWidth = 45.f; + void debugPaintPolynom(cv::Mat &m, cv::Scalar color, NewtonPolynomial &p, int start, int end); void debugPaintPoints(cv::Mat &m, cv::Scalar color, std::vector> &points); From 3b69e32f87c2a607947aeae62aa43200f7b97eaa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Sat, 9 Sep 2017 19:59:36 +0200 Subject: [PATCH 52/79] - optimized params - improved find lane position function --- .../launch/line_detection_fu.launch | 4 +- src/fu_line_detection/src/laneDetection.cpp | 66 +++++++------------ 2 files changed, 27 insertions(+), 43 deletions(-) diff --git a/src/fu_line_detection/launch/line_detection_fu.launch b/src/fu_line_detection/launch/line_detection_fu.launch index cc799eec..090739e8 100644 --- a/src/fu_line_detection/launch/line_detection_fu.launch +++ b/src/fu_line_detection/launch/line_detection_fu.launch @@ -16,8 +16,8 @@ - - + + diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index 48b938f8..d39a08e9 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -295,13 +295,9 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::waitKey(1); #endif //---------------------- END DEBUG OUTPUT LANE MARKINGS ------------------------------// - ROS_ERROR("findLanePositions"); findLanePositions(laneMarkings); - ROS_ERROR("/findLanePositions"); - ROS_ERROR("buildLaneMarkingsLists"); // start actual execution buildLaneMarkingsLists(laneMarkings); - ROS_ERROR("/buildLaneMarkingsLists"); //---------------------- DEBUG OUTPUT GROUPED LANE MARKINGS ---------------------------------// #ifdef PUBLISH_DEBUG_OUTPUT @@ -361,13 +357,9 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) supportersCenter.clear(); supportersRight.clear(); - ROS_ERROR("ransac"); ransac(); - ROS_ERROR("/ransac"); - ROS_ERROR("generateMovedPolynomials"); generateMovedPolynomials(); - ROS_ERROR("/generateMovedPolynomials"); //---------------------- DEBUG OUTPUT RANSAC POLYNOMIALS ---------------------------------// #ifdef PUBLISH_DEBUG_OUTPUT @@ -398,9 +390,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) //---------------------- END DEBUG OUTPUT RANSAC POLYNOMIALS ------------------------------// //detectLane(); - ROS_ERROR("pubAngle"); pubAngle(); - ROS_ERROR("/pubAngle"); cv::Mat transformedImagePaintableLaneModel = transformedImage.clone(); cv::cvtColor(transformedImagePaintableLaneModel,transformedImagePaintableLaneModel,CV_GRAY2BGR); @@ -442,7 +432,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Point pointLoc = cv::Point(proj_image_w_half, proj_image_h); cv::circle(transformedImagePaintableLaneModel, pointLoc, 2, cv::Scalar(0,0,255), -1); - cv::Point anglePointLoc = cv::Point(sin(lastAngle * PI / 180) * angleAdjacentLeg + proj_image_w_half, proj_image_h - angleAdjacentLeg); + cv::Point anglePointLoc = cv::Point(sin(lastAngle * PI / 180) * angleAdjacentLeg + proj_image_w_half, proj_image_h - (cos(lastAngle * PI / 180) * angleAdjacentLeg)); cv::line(transformedImagePaintableLaneModel, pointLoc, anglePointLoc, cv::Scalar(255,255,255)); cv::Point startNormalPoint = cv::Point(pointForAngle.getX(), pointForAngle.getY()); @@ -451,11 +441,11 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Point targetPoint = cv::Point(movedPointForAngle.getX(), movedPointForAngle.getY()); cv::circle(transformedImagePaintableLaneModel, targetPoint, 2, cv::Scalar(0,0,255), -1); - cv::Point adjacentLegPoint = cv::Point(proj_image_w_half, proj_image_h - adjacentLeg); + /*cv::Point adjacentLegPoint = cv::Point(proj_image_w_half, proj_image_h - adjacentLeg); cv::line(transformedImagePaintableLaneModel, pointLoc, adjacentLegPoint, cv::Scalar(255,0,0)); cv::Point oppositeLegPoint = cv::Point(proj_image_w_half + oppositeLeg, proj_image_h - adjacentLeg); - cv::line(transformedImagePaintableLaneModel, adjacentLegPoint, oppositeLegPoint, cv::Scalar(0,255,0)); + cv::line(transformedImagePaintableLaneModel, adjacentLegPoint, oppositeLegPoint, cv::Scalar(0,255,0));*/ double m = -gradientForAngle; @@ -774,9 +764,6 @@ void cLaneDetectionFu::findLanePositions(vector> &laneMarkings) { if (laneMarking->getY() > maxYRoi) { continue; } - if (laneMarking->getY() < proj_image_h - (proj_image_h - maxYRoi) - 10) { - break; - } bool isSupporter = false; if (laneMarking->getX() < proj_image_w_half + proj_image_horizontal_offset) { @@ -784,6 +771,11 @@ void cLaneDetectionFu::findLanePositions(vector> &laneMarkings) { if (isInRange(*centerStart.at(i), *laneMarking)) { isSupporter = true; centerSupporter.at(i)++; + + if (centerSupporter.at(i) > 5) { + goto defaultLineCalculation; + } + break; } } @@ -797,6 +789,11 @@ void cLaneDetectionFu::findLanePositions(vector> &laneMarkings) { if (isInRange(*rightStart.at(i), *laneMarking)) { isSupporter = true; rightSupporter.at(i)++; + + if (rightSupporter.at(i) > 5) { + goto defaultLineCalculation; + } + break; } } @@ -808,6 +805,18 @@ void cLaneDetectionFu::findLanePositions(vector> &laneMarkings) { } } +defaultLineCalculation: + + int i = 0; + for (FuPoint* p : centerStart) { + ROS_ERROR("center: %d, %d, %d", p->getX(), p->getY(), centerSupporter.at(i++)); + } + + i = 0; + for (FuPoint* p : rightStart) { + ROS_ERROR("right: %d, %d, %d", p->getX(), p->getY(), rightSupporter.at(i++)); + } + // use x-value of lane marking point with most (and at least 3) supporters if (centerStart.size() > 0) { vector::iterator maxCenterElement = max_element(centerSupporter.begin(), centerSupporter.end()); @@ -1028,11 +1037,8 @@ void cLaneDetectionFu::generateMovedPolynomials() isPolyMovedCenter = false; isPolyMovedRight = false; - ROS_ERROR("non detected"); if (!polyDetectedLeft && !polyDetectedCenter && !polyDetectedRight) return; - ROS_ERROR("/non all detected"); if (polyDetectedLeft && polyDetectedCenter && polyDetectedRight) return; - ROS_ERROR("all detected"); FuPoint pointLeft1 = FuPoint(); FuPoint pointLeft2 = FuPoint(); @@ -1063,7 +1069,6 @@ void cLaneDetectionFu::generateMovedPolynomials() * that arctan of some gradient equals its angle to the x-axis in degree. */ if (polyDetectedRight && !polyDetectedCenter) { - ROS_ERROR("right"); usedPoly = polyRight; m1 = gradient(pointsRight.at(0).getY(), pointsRight, usedPoly.getCoefficients()); m2 = gradient(pointsRight.at(1).getY(), pointsRight, usedPoly.getCoefficients()); @@ -1082,10 +1087,8 @@ void cLaneDetectionFu::generateMovedPolynomials() shiftPoint(pointLeft2,m2, -2 * laneWidth, pointsRight.at(1)); shiftPoint(pointLeft3,m3, -2 * laneWidth, pointsRight.at(2)); } - ROS_ERROR("/right"); } else if (polyDetectedLeft && !polyDetectedCenter) { - ROS_ERROR("left"); usedPoly = polyLeft; m1 = gradient(pointsLeft.at(0).getY(), pointsLeft, usedPoly.getCoefficients()); m2 = gradient(pointsLeft.at(1).getY(), pointsLeft, usedPoly.getCoefficients()); @@ -1104,10 +1107,8 @@ void cLaneDetectionFu::generateMovedPolynomials() shiftPoint(pointRight2,m2, 2 * laneWidth, pointsLeft.at(1)); shiftPoint(pointRight3,m3, 2 * laneWidth, pointsLeft.at(2)); } - ROS_ERROR("/left"); } else if (polyDetectedCenter) { - ROS_ERROR("center"); usedPoly = polyCenter; m1 = gradient(pointsCenter.at(0).getY(), pointsCenter, usedPoly.getCoefficients()); m2 = gradient(pointsCenter.at(1).getY(), pointsCenter, usedPoly.getCoefficients()); @@ -1121,38 +1122,30 @@ void cLaneDetectionFu::generateMovedPolynomials() shiftPoint(pointLeft3,m3, -laneWidth, pointsCenter.at(2)); } - ROS_ERROR("polyDetectedRight %d", polyDetectedRight); if (!polyDetectedRight) { - ROS_ERROR("!polyDetectedRight"); isPolyMovedRight = true; shiftPoint(pointRight1,m1, laneWidth, pointsCenter.at(0)); shiftPoint(pointRight2,m2, laneWidth, pointsCenter.at(1)); shiftPoint(pointRight3,m3, laneWidth, pointsCenter.at(2)); } - ROS_ERROR("/center"); } // create the lane polynomial out of the shifted points if (isPolyMovedLeft) { - ROS_ERROR("movedLeft"); movedPolyLeft.addData(pointLeft1); movedPolyLeft.addData(pointLeft2); movedPolyLeft.addData(pointLeft3); - ROS_ERROR("/movedLeft"); } if (isPolyMovedCenter) { - ROS_ERROR("movedCenter"); movedPolyCenter.addData(pointCenter1); movedPolyCenter.addData(pointCenter2); movedPolyCenter.addData(pointCenter3); - ROS_ERROR("/movedCenter"); } if (isPolyMovedRight) { - ROS_ERROR("movedRight"); movedPolyRight.addData(pointRight1); movedPolyRight.addData(pointRight2); movedPolyRight.addData(pointRight3); @@ -1160,7 +1153,6 @@ void cLaneDetectionFu::generateMovedPolynomials() movedPointsRight.push_back(FuPoint(pointRight1.getY(), pointRight1.getX())); movedPointsRight.push_back(FuPoint(pointRight2.getY(), pointRight2.getX())); movedPointsRight.push_back(FuPoint(pointRight3.getY(), pointRight3.getX())); - ROS_ERROR("/movedRight"); } } @@ -1293,7 +1285,6 @@ int cLaneDetectionFu::horizDistance(FuPoint &p1, FuPoint &p2) { */ double cLaneDetectionFu::gradient(double x, vector> &points, vector coeffs) { - ROS_ERROR("gradient %f", ((2 * coeffs[2] * x + coeffs[1])-(coeffs[2] * points[1].getY()) - (coeffs[2] * points[0].getY()))); return (2 * coeffs[2] * x + coeffs[1]) - (coeffs[2] * points[1].getY()) - (coeffs[2] * points[0].getY()); @@ -2132,7 +2123,6 @@ void cLaneDetectionFu::pubRGBImageMsg(cv::Mat& rgb_mat, image_transport::CameraP void cLaneDetectionFu::pubAngle() { if (!polyDetectedRight && !isPolyMovedRight) { - ROS_ERROR("!polyDetectedRight && !isPolyMovedRight"); return; } @@ -2140,17 +2130,12 @@ void cLaneDetectionFu::pubAngle() { double xRightLane; double m; - ROS_ERROR("polyDetectedRight %d", polyDetectedRight); - if (polyDetectedRight) { - ROS_ERROR(" polyDetectedRight"); xRightLane = polyRight.at(y); m = gradient(y, pointsRight, polyRight.getCoefficients()); } else { - ROS_ERROR(" !polyDetectedRight"); xRightLane = movedPolyRight.at(y); m = gradient(y, movedPointsRight, movedPolyRight.getCoefficients()); - ROS_ERROR(" gradient %f", m); } double offset = -1 * laneWidth / 2; @@ -2163,7 +2148,6 @@ void cLaneDetectionFu::pubAngle() { oppositeLeg = movedPointForAngle.getX() - proj_image_w_half; adjacentLeg = proj_image_h - movedPointForAngle.getY(); - ROS_ERROR(" oppositeLeg %f adjacentLeg %f", oppositeLeg, adjacentLeg); double result = atan(oppositeLeg / adjacentLeg) * 180 / PI; /* From a04abb65208d30793ce401a42288b1e6201ead47 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Sat, 9 Sep 2017 20:35:26 +0200 Subject: [PATCH 53/79] added frame time calculation --- src/fu_line_detection/src/laneDetection.cpp | 29 +++++++++++++-------- 1 file changed, 18 insertions(+), 11 deletions(-) diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/fu_line_detection/src/laneDetection.cpp index d39a08e9..394806b9 100644 --- a/src/fu_line_detection/src/laneDetection.cpp +++ b/src/fu_line_detection/src/laneDetection.cpp @@ -191,6 +191,10 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) { //ROS_ERROR_STREAM("defaultXLeft: "<> &laneMarkings) { defaultLineCalculation: - int i = 0; - for (FuPoint* p : centerStart) { - ROS_ERROR("center: %d, %d, %d", p->getX(), p->getY(), centerSupporter.at(i++)); - } - - i = 0; - for (FuPoint* p : rightStart) { - ROS_ERROR("right: %d, %d, %d", p->getX(), p->getY(), rightSupporter.at(i++)); - } - // use x-value of lane marking point with most (and at least 3) supporters if (centerStart.size() > 0) { vector::iterator maxCenterElement = max_element(centerSupporter.begin(), centerSupporter.end()); @@ -841,7 +848,7 @@ void cLaneDetectionFu::findLanePositions(vector> &laneMarkings) { defaultXLeft = defaultXCenter - (int) laneWidth; } - ROS_ERROR("left: %d, center: %d, right: %d", defaultXLeft, defaultXCenter, defaultXRight); + //ROS_ERROR("left: %d, center: %d, right: %d", defaultXLeft, defaultXCenter, defaultXRight); } /** From 8e60fe3c5fee02003d6b1e67ca06be7173827dab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Thu, 14 Sep 2017 12:42:17 +0200 Subject: [PATCH 54/79] renamed folder to reos node name --- .../CMakeLists.txt | 0 .../README.md | 0 .../callgraph.graphml | 0 .../callgraph.pdf | Bin .../cfg/LaneDetection.cfg | 0 .../include/EMPTY | 0 .../launch/line_detection_fu.launch | 0 .../package.xml | 0 .../src/laneDetection.cpp | 0 .../src/laneDetection.h | 0 .../src/tools/Common.h | 0 .../src/tools/Edges.h | 0 .../src/tools/FuPoint.h | 0 .../src/tools/FuVector.h | 0 .../src/tools/IPMapper.cpp | 0 .../src/tools/IPMapper.h | 0 .../src/tools/LanePolynomial.cpp | 0 .../src/tools/LanePolynomial.h | 0 .../src/tools/LineSegment.h | 0 .../src/tools/NewtonPolynomial.cpp | 0 .../src/tools/NewtonPolynomial.h | 0 .../src/tools/attributes.h | 0 .../src/tools/enums.h | 0 .../src/tools/has_member.h | 0 .../src/tools/position.h | 0 .../src/tools/units.h | 0 26 files changed, 0 insertions(+), 0 deletions(-) rename src/{fu_line_detection => line_detection_fu}/CMakeLists.txt (100%) rename src/{fu_line_detection => line_detection_fu}/README.md (100%) rename src/{fu_line_detection => line_detection_fu}/callgraph.graphml (100%) rename src/{fu_line_detection => line_detection_fu}/callgraph.pdf (100%) rename src/{fu_line_detection => line_detection_fu}/cfg/LaneDetection.cfg (100%) rename src/{fu_line_detection => line_detection_fu}/include/EMPTY (100%) rename src/{fu_line_detection => line_detection_fu}/launch/line_detection_fu.launch (100%) rename src/{fu_line_detection => line_detection_fu}/package.xml (100%) rename src/{fu_line_detection => line_detection_fu}/src/laneDetection.cpp (100%) rename src/{fu_line_detection => line_detection_fu}/src/laneDetection.h (100%) rename src/{fu_line_detection => line_detection_fu}/src/tools/Common.h (100%) rename src/{fu_line_detection => line_detection_fu}/src/tools/Edges.h (100%) rename src/{fu_line_detection => line_detection_fu}/src/tools/FuPoint.h (100%) rename src/{fu_line_detection => line_detection_fu}/src/tools/FuVector.h (100%) rename src/{fu_line_detection => line_detection_fu}/src/tools/IPMapper.cpp (100%) rename src/{fu_line_detection => line_detection_fu}/src/tools/IPMapper.h (100%) rename src/{fu_line_detection => line_detection_fu}/src/tools/LanePolynomial.cpp (100%) rename src/{fu_line_detection => line_detection_fu}/src/tools/LanePolynomial.h (100%) rename src/{fu_line_detection => line_detection_fu}/src/tools/LineSegment.h (100%) rename src/{fu_line_detection => line_detection_fu}/src/tools/NewtonPolynomial.cpp (100%) rename src/{fu_line_detection => line_detection_fu}/src/tools/NewtonPolynomial.h (100%) rename src/{fu_line_detection => line_detection_fu}/src/tools/attributes.h (100%) rename src/{fu_line_detection => line_detection_fu}/src/tools/enums.h (100%) rename src/{fu_line_detection => line_detection_fu}/src/tools/has_member.h (100%) rename src/{fu_line_detection => line_detection_fu}/src/tools/position.h (100%) rename src/{fu_line_detection => line_detection_fu}/src/tools/units.h (100%) diff --git a/src/fu_line_detection/CMakeLists.txt b/src/line_detection_fu/CMakeLists.txt similarity index 100% rename from src/fu_line_detection/CMakeLists.txt rename to src/line_detection_fu/CMakeLists.txt diff --git a/src/fu_line_detection/README.md b/src/line_detection_fu/README.md similarity index 100% rename from src/fu_line_detection/README.md rename to src/line_detection_fu/README.md diff --git a/src/fu_line_detection/callgraph.graphml b/src/line_detection_fu/callgraph.graphml similarity index 100% rename from src/fu_line_detection/callgraph.graphml rename to src/line_detection_fu/callgraph.graphml diff --git a/src/fu_line_detection/callgraph.pdf b/src/line_detection_fu/callgraph.pdf similarity index 100% rename from src/fu_line_detection/callgraph.pdf rename to src/line_detection_fu/callgraph.pdf diff --git a/src/fu_line_detection/cfg/LaneDetection.cfg b/src/line_detection_fu/cfg/LaneDetection.cfg similarity index 100% rename from src/fu_line_detection/cfg/LaneDetection.cfg rename to src/line_detection_fu/cfg/LaneDetection.cfg diff --git a/src/fu_line_detection/include/EMPTY b/src/line_detection_fu/include/EMPTY similarity index 100% rename from src/fu_line_detection/include/EMPTY rename to src/line_detection_fu/include/EMPTY diff --git a/src/fu_line_detection/launch/line_detection_fu.launch b/src/line_detection_fu/launch/line_detection_fu.launch similarity index 100% rename from src/fu_line_detection/launch/line_detection_fu.launch rename to src/line_detection_fu/launch/line_detection_fu.launch diff --git a/src/fu_line_detection/package.xml b/src/line_detection_fu/package.xml similarity index 100% rename from src/fu_line_detection/package.xml rename to src/line_detection_fu/package.xml diff --git a/src/fu_line_detection/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp similarity index 100% rename from src/fu_line_detection/src/laneDetection.cpp rename to src/line_detection_fu/src/laneDetection.cpp diff --git a/src/fu_line_detection/src/laneDetection.h b/src/line_detection_fu/src/laneDetection.h similarity index 100% rename from src/fu_line_detection/src/laneDetection.h rename to src/line_detection_fu/src/laneDetection.h diff --git a/src/fu_line_detection/src/tools/Common.h b/src/line_detection_fu/src/tools/Common.h similarity index 100% rename from src/fu_line_detection/src/tools/Common.h rename to src/line_detection_fu/src/tools/Common.h diff --git a/src/fu_line_detection/src/tools/Edges.h b/src/line_detection_fu/src/tools/Edges.h similarity index 100% rename from src/fu_line_detection/src/tools/Edges.h rename to src/line_detection_fu/src/tools/Edges.h diff --git a/src/fu_line_detection/src/tools/FuPoint.h b/src/line_detection_fu/src/tools/FuPoint.h similarity index 100% rename from src/fu_line_detection/src/tools/FuPoint.h rename to src/line_detection_fu/src/tools/FuPoint.h diff --git a/src/fu_line_detection/src/tools/FuVector.h b/src/line_detection_fu/src/tools/FuVector.h similarity index 100% rename from src/fu_line_detection/src/tools/FuVector.h rename to src/line_detection_fu/src/tools/FuVector.h diff --git a/src/fu_line_detection/src/tools/IPMapper.cpp b/src/line_detection_fu/src/tools/IPMapper.cpp similarity index 100% rename from src/fu_line_detection/src/tools/IPMapper.cpp rename to src/line_detection_fu/src/tools/IPMapper.cpp diff --git a/src/fu_line_detection/src/tools/IPMapper.h b/src/line_detection_fu/src/tools/IPMapper.h similarity index 100% rename from src/fu_line_detection/src/tools/IPMapper.h rename to src/line_detection_fu/src/tools/IPMapper.h diff --git a/src/fu_line_detection/src/tools/LanePolynomial.cpp b/src/line_detection_fu/src/tools/LanePolynomial.cpp similarity index 100% rename from src/fu_line_detection/src/tools/LanePolynomial.cpp rename to src/line_detection_fu/src/tools/LanePolynomial.cpp diff --git a/src/fu_line_detection/src/tools/LanePolynomial.h b/src/line_detection_fu/src/tools/LanePolynomial.h similarity index 100% rename from src/fu_line_detection/src/tools/LanePolynomial.h rename to src/line_detection_fu/src/tools/LanePolynomial.h diff --git a/src/fu_line_detection/src/tools/LineSegment.h b/src/line_detection_fu/src/tools/LineSegment.h similarity index 100% rename from src/fu_line_detection/src/tools/LineSegment.h rename to src/line_detection_fu/src/tools/LineSegment.h diff --git a/src/fu_line_detection/src/tools/NewtonPolynomial.cpp b/src/line_detection_fu/src/tools/NewtonPolynomial.cpp similarity index 100% rename from src/fu_line_detection/src/tools/NewtonPolynomial.cpp rename to src/line_detection_fu/src/tools/NewtonPolynomial.cpp diff --git a/src/fu_line_detection/src/tools/NewtonPolynomial.h b/src/line_detection_fu/src/tools/NewtonPolynomial.h similarity index 100% rename from src/fu_line_detection/src/tools/NewtonPolynomial.h rename to src/line_detection_fu/src/tools/NewtonPolynomial.h diff --git a/src/fu_line_detection/src/tools/attributes.h b/src/line_detection_fu/src/tools/attributes.h similarity index 100% rename from src/fu_line_detection/src/tools/attributes.h rename to src/line_detection_fu/src/tools/attributes.h diff --git a/src/fu_line_detection/src/tools/enums.h b/src/line_detection_fu/src/tools/enums.h similarity index 100% rename from src/fu_line_detection/src/tools/enums.h rename to src/line_detection_fu/src/tools/enums.h diff --git a/src/fu_line_detection/src/tools/has_member.h b/src/line_detection_fu/src/tools/has_member.h similarity index 100% rename from src/fu_line_detection/src/tools/has_member.h rename to src/line_detection_fu/src/tools/has_member.h diff --git a/src/fu_line_detection/src/tools/position.h b/src/line_detection_fu/src/tools/position.h similarity index 100% rename from src/fu_line_detection/src/tools/position.h rename to src/line_detection_fu/src/tools/position.h diff --git a/src/fu_line_detection/src/tools/units.h b/src/line_detection_fu/src/tools/units.h similarity index 100% rename from src/fu_line_detection/src/tools/units.h rename to src/line_detection_fu/src/tools/units.h From c1b7fb8a5c62dfb74f7e56f89020028a52c44cb8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Thu, 14 Sep 2017 13:12:04 +0200 Subject: [PATCH 55/79] removed debug comments and unused methods --- src/line_detection_fu/src/laneDetection.cpp | 580 ++------------------ src/line_detection_fu/src/laneDetection.h | 9 +- 2 files changed, 33 insertions(+), 556 deletions(-) diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index 394806b9..c91d8b17 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -147,7 +147,6 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) read_images_ = nh.subscribe(nh_.resolveName(camera_name), MY_ROS_QUEUE_SIZE, &cLaneDetectionFu::ProcessInput,this); - //publish_curvature = nh.advertise("/lane_model/curvature", MY_ROS_QUEUE_SIZE); publish_angle = nh.advertise("/lane_model/angle", MY_ROS_QUEUE_SIZE); image_transport::ImageTransport image_transport(nh); @@ -189,28 +188,17 @@ cLaneDetectionFu::~cLaneDetectionFu() void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) { - //ROS_ERROR_STREAM("defaultXLeft: "<image.clone(); - Mat cut_image = image(cv::Rect(0,cam_h * 0.25f,cam_w,cam_h * 0.75f)); #ifdef PAINT_OUTPUT_CUT_IMAGE @@ -218,17 +206,8 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::waitKey(1); #endif - gettimeofday(&tp, NULL); - long int ms1 = tp.tv_sec * 1000 + tp.tv_usec / 1000; - - //Mat cut_image = image(cv::Rect(0,cam_h/2,cam_w,cam_h/2)); Mat remapped_image = ipMapper.remap(cut_image); - gettimeofday(&tp, NULL); - long int ms2 = tp.tv_sec * 1000 + tp.tv_usec / 1000; - - ROS_ERROR("remap time: %d", (ms2 - ms1)); - #ifdef PAINT_OUTPUT_IP_MAPPED_IMAGE cv::imshow("IPmapped image", remapped_image); cv::waitKey(1); @@ -236,15 +215,9 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) cv::Mat transformedImage = remapped_image(cv::Rect((cam_w/2)-proj_image_w_half+proj_image_horizontal_offset, proj_y_start,proj_image_w,proj_image_h)).clone(); -// cv::Mat transformedImage = remapped_image(cv::Rect((cam_w/2)-proj_image_w_half+proj_image_horizontal_offset, -// proj_y_start,proj_image_w,proj_image_h)).clone(); cv::flip(transformedImage, transformedImage, 0); - //cv::imshow("Cut IPmapped image", transformedImage); - //cv::waitKey(1); - - //scanlines -> edges (in each scanline we find maximum and minimum of kernel fn ~= where the edge is) //this is where we use input image! vector> edges = cLaneDetectionFu::scanImage(transformedImage); @@ -401,7 +374,6 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) #endif //---------------------- END DEBUG OUTPUT RANSAC POLYNOMIALS ------------------------------// - //detectLane(); pubAngle(); cv::Mat transformedImagePaintableLaneModel = transformedImage.clone(); @@ -484,11 +456,6 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) //---------------------- END DEBUG OUTPUT LANE POLYNOMIAL ------------------------------// frame++; - - gettimeofday(&tp, NULL); - long int end = tp.tv_sec * 1000 + tp.tv_usec / 1000; - - ROS_ERROR("Frame time: %d", (end -start)); } void cLaneDetectionFu::debugPaintPolynom(cv::Mat &m, cv::Scalar color, NewtonPolynomial &p, int start, int end) { @@ -847,8 +814,6 @@ void cLaneDetectionFu::findLanePositions(vector> &laneMarkings) { laneWidth = defaultXRight - defaultXCenter; defaultXLeft = defaultXCenter - (int) laneWidth; } - - //ROS_ERROR("left: %d, center: %d, right: %d", defaultXLeft, defaultXCenter, defaultXRight); } /** @@ -861,53 +826,7 @@ void cLaneDetectionFu::findLanePositions(vector> &laneMarkings) { * * @param laneMarkings a vector containing all detected lane markings */ -/*void cLaneDetectionFu::buildLaneMarkingsLists( - const std::vector> &laneMarkings) { - laneMarkingsLeft.clear(); - laneMarkingsCenter.clear(); - laneMarkingsRight.clear(); - - for (FuPoint laneMarking : laneMarkings) { - if (polyDetectedLeft) { - if (isInPolyRoi(polyLeft, laneMarking)) { - laneMarkingsLeft.push_back(laneMarking); - continue; - } - } - - if (polyDetectedCenter) { - if (isInPolyRoi(polyCenter, laneMarking)) { - laneMarkingsCenter.push_back(laneMarking); - continue; - } - } - - if (polyDetectedRight) { - if (isInPolyRoi(polyRight, laneMarking)) { - laneMarkingsRight.push_back(laneMarking); - continue; - } - } - - if (isInDefaultRoi(LEFT, laneMarking)) { - laneMarkingsLeft.push_back(laneMarking); - continue; - } - - if (isInDefaultRoi(CENTER, laneMarking)) { - laneMarkingsCenter.push_back(laneMarking); - continue; - } - - if (isInDefaultRoi(RIGHT, laneMarking)) { - laneMarkingsRight.push_back(laneMarking); - continue; - } - } -}*/ - -void cLaneDetectionFu::buildLaneMarkingsLists( - const std::vector> &laneMarkings) { +void cLaneDetectionFu::buildLaneMarkingsLists( const std::vector> &laneMarkings) { laneMarkingsLeft.clear(); laneMarkingsCenter.clear(); laneMarkingsRight.clear(); @@ -1022,8 +941,17 @@ void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, F * @param y * @param x */ -void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, int x, int y) -{ +void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, int x, int y) { + /* + * Depending on the sign of the gradient of the poly at the different + * x-values and depending on which position we are, we have to add or + * subtract the expected distance to the respective lane polynomial, to get + * the wanted points. + * + * The calculation is done for the x- and y-components of the points + * separately using the trigonometric ratios of right triangles and the fact + * that arctan of some gradient equals its angle to the x-axis in degree. + */ if (m >= 0) { p.setX(x - offset * sin(atan(-1 / m))); p.setY(y - offset * cos(atan(-1 / m))); @@ -1033,8 +961,7 @@ void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, i p.setY(y + offset * cos(atan(-1 / m))); } -void cLaneDetectionFu::generateMovedPolynomials() -{ +void cLaneDetectionFu::generateMovedPolynomials() { movedPolyLeft.clear(); movedPolyCenter.clear(); movedPolyRight.clear(); @@ -1044,8 +971,10 @@ void cLaneDetectionFu::generateMovedPolynomials() isPolyMovedCenter = false; isPolyMovedRight = false; - if (!polyDetectedLeft && !polyDetectedCenter && !polyDetectedRight) return; - if (polyDetectedLeft && polyDetectedCenter && polyDetectedRight) return; + if ((!polyDetectedLeft && !polyDetectedCenter && !polyDetectedRight) + || (polyDetectedLeft && polyDetectedCenter && polyDetectedRight)) { + return; + } FuPoint pointLeft1 = FuPoint(); FuPoint pointLeft2 = FuPoint(); @@ -1232,12 +1161,9 @@ int cLaneDetectionFu::horizDistanceToPolynomial(NewtonPolynomial& poly, FuPoint< bool cLaneDetectionFu::isInDefaultRoi(ePosition position, FuPoint &p) { if (p.getY() < minYDefaultRoi || p.getY() > maxYRoi) { return false; - } - else if (horizDistanceToDefaultLine(position, p) - <= interestDistanceDefault) { + } else if (horizDistanceToDefaultLine(position, p) <= interestDistanceDefault) { return true; - } - else { + } else { return false; } } @@ -1252,11 +1178,9 @@ bool cLaneDetectionFu::isInDefaultRoi(ePosition position, FuPoint &p) { bool cLaneDetectionFu::isInPolyRoi(NewtonPolynomial &poly, FuPoint &p) { if (p.getY() < minYPolyRoi || p.getY() > maxYRoi) { return false; - } - else if (horizDistanceToPolynomial(poly, p) <= interestDistancePoly) { + } else if (horizDistanceToPolynomial(poly, p) <= interestDistancePoly) { return true; - } - else { + } else { return false; } } @@ -1290,21 +1214,12 @@ int cLaneDetectionFu::horizDistance(FuPoint &p1, FuPoint &p2) { * @param coeffs The coefficients under usage of the newton basis * @return The gradient of the polynomial at x */ -double cLaneDetectionFu::gradient(double x, vector> &points, - vector coeffs) { +double cLaneDetectionFu::gradient(double x, vector> &points, vector coeffs) { return (2 * coeffs[2] * x + coeffs[1]) - (coeffs[2] * points[1].getY()) - (coeffs[2] * points[0].getY()); } -double cLaneDetectionFu::gradient(double x, NewtonPolynomial polynomial) { - vector coeffs = polynomial.getCoefficients(); - - - return 2 * coeffs[2] * x + coeffs[1] - coeffs[2] * polynomial.at(10); - - coeffs[2] * polynomial.at(50); -} - /** * Calculates the x value of the point where the normal of the tangent of a * polynomial at a given point p intersects with a second polynomial. @@ -1422,393 +1337,6 @@ ePosition cLaneDetectionFu::maxProportion() { return maxPos; } -/** - * Create the lane polynomial starting from the detected polynomial of the - * given position. A lane polynomial is formed by shifting points with - * different x-values of the used polynomial along the normals of the polynomial - * at this points to the distance, where the respective lane polynomial is - * expected to lie. - * - * @param position The position of the detected polynomial used as reference - */ -void cLaneDetectionFu::createLanePoly(ePosition position) { - lanePoly.clear(); - - //double laneWidth = (defaultXRight - defaultXCenter) / 2; //45.f; - double x1 = minYPolyRoi + 5; - double x2 = minYPolyRoi + ((proj_image_h-minYPolyRoi) / 2); - double x3 = proj_image_h - 5; - - FuPoint point1 = FuPoint(); - FuPoint point2 = FuPoint(); - FuPoint point3 = FuPoint(); - - NewtonPolynomial usedPoly; - - double m1 = 0; - double m2 = 0; - double m3 = 0; - - if (position == LEFT) { - usedPoly = polyLeft; - m1 = gradient(x1, pointsLeft, usedPoly.getCoefficients()); - m2 = gradient(x2, pointsLeft, usedPoly.getCoefficients()); - m3 = gradient(x3, pointsLeft, usedPoly.getCoefficients()); - - shiftPoint(point1,m1, defaultXLeft, x1, usedPoly.at(x1)); - shiftPoint(point2,m2, defaultXLeft, x2, usedPoly.at(x2)); - shiftPoint(point3,m3, defaultXLeft, x3, usedPoly.at(x3)); - } - else if (position == CENTER) { - usedPoly = polyCenter; - m1 = gradient(x1, pointsCenter, usedPoly.getCoefficients()); - m2 = gradient(x2, pointsCenter, usedPoly.getCoefficients()); - m3 = gradient(x3, pointsCenter, usedPoly.getCoefficients()); - - shiftPoint(point1,m1, defaultXCenter, x1, usedPoly.at(x1)); - shiftPoint(point2,m2, defaultXCenter, x2, usedPoly.at(x2)); - shiftPoint(point3,m3, defaultXCenter, x3, usedPoly.at(x3)); - } - else if (position == RIGHT) { - usedPoly = polyRight; - m1 = gradient(x1, pointsRight, usedPoly.getCoefficients()); - m2 = gradient(x2, pointsRight, usedPoly.getCoefficients()); - m3 = gradient(x3, pointsRight, usedPoly.getCoefficients()); - - shiftPoint(point1,m1, defaultXRight, x1, usedPoly.at(x1)); - shiftPoint(point2,m2, defaultXRight, x2, usedPoly.at(x2)); - shiftPoint(point3,m3, defaultXRight, x3, usedPoly.at(x3)); - } - - // create the lane polynomial out of the shifted points - lanePoly.addDataXY(point1); - lanePoly.addDataXY(point2); - lanePoly.addDataXY(point3); - - lanePolynomial.setLanePoly(lanePoly); - lanePolynomial.setDetected(); - lanePolynomial.setLastUsedPosition(position); -/* - lanePoly.clear(); - - double coef = 1.2; - - double x1 = minYPolyRoi+5; - double x2 = minYPolyRoi + ((proj_image_h-minYPolyRoi)/2); - double x3 = proj_image_h-5; - - FuPoint pointRight1; - FuPoint pointRight2; - FuPoint pointRight3; - - double dRight = 0; - - NewtonPolynomial usedPoly; - - double y1; - double y2; - double y3; - - if (position == LEFT) { - usedPoly = polyLeft; - dRight = 10; - } - else if (position == CENTER) { - usedPoly = polyCenter; - dRight = defaultXCenter-5; - } - else if (position == RIGHT) { - usedPoly = polyRight; - dRight = defaultXRight+5; - } - - - pointRight1 = FuPoint(x1, usedPoly.at(x1) - dRight); - pointRight2 = FuPoint(x2, usedPoly.at(x2) - dRight); - pointRight3 = FuPoint(x3, usedPoly.at(x3) - dRight); - - // create the lane polynomial out of the shifted points - lanePoly.addDataXY(pointRight1); - lanePoly.addDataXY(pointRight2); - lanePoly.addDataXY(pointRight3); - - lanePolynomial.setLanePoly(lanePoly); - lanePolynomial.setDetected(); - lanePolynomial.setLastUsedPosition(position); -*/ -} - -//original method, should be better, but doesn't work correctly in our case when RIGHT polynomial is used -/*void LaneDetector::createLanePoly(ePosition position) { - lanePoly.clear(); - - double x1 = 0.05; - double x2 = 0.4; - double x3 = 1.0; - - FuPoint pointRight1; - FuPoint pointRight2; - FuPoint pointRight3; - - double m1 = 0; - double m2 = 0; - double m3 = 0; - - double dRight = 0; - - NewtonPolynomial usedPoly; - - /* - * Depending on the sign of the gradient of the poly at the different - * x-values and depending on which position we are, we have to add or - * subtract the expected distance to the respective lane polynomial, to get - * the wanted points. - * - * The calculation is done for the x- and y-components of the points - * separately using the trigonometric ratios of right triangles and the fact - * that arctan of some gradient equals its angle to the x-axis in degree. - */ - /*if (position == LEFT) { - usedPoly = polyLeft; - m1 = gradient(x1, pointsLeft, usedPoly.getCoefficients()); - m2 = gradient(x2, pointsLeft, usedPoly.getCoefficients()); - m3 = gradient(x3, pointsLeft, usedPoly.getCoefficients()); - - dRight = defaultXLeft; - - if (m1 > 0) { - pointRight1 = FuPoint(x1 + dRight * cos(atan(-1 / m1)), - usedPoly.at(x1) + dRight * sin(atan(-1 / m1))); - } - else { - pointRight1 = FuPoint(x1 - dRight * cos(atan(-1 / m1)), - usedPoly.at(x1) - dRight * sin(atan(-1 / m1))); - } - - if (m2 > 0) { - pointRight2 = FuPoint(x2 + dRight * cos(atan(-1 / m2)), - usedPoly.at(x2) + dRight * sin(atan(-1 / m2))); - } - else { - pointRight2 = FuPoint(x2 - dRight * cos(atan(-1 / m2)), - usedPoly.at(x2) - dRight * sin(atan(-1 / m2))); - } - - if (m3 > 0) { - pointRight3 = FuPoint(x3 + dRight * cos(atan(-1 / m3)), - usedPoly.at(x3) + dRight * sin(atan(-1 / m3))); - } - else { - pointRight3 = FuPoint(x3 - dRight * cos(atan(-1 / m3)), - usedPoly.at(x3) - dRight * sin(atan(-1 / m3))); - } - } - else if (position == CENTER) { - usedPoly = polyCenter; - m1 = gradient(x1, pointsCenter, usedPoly.getCoefficients()); - m2 = gradient(x2, pointsCenter, usedPoly.getCoefficients()); - m3 = gradient(x3, pointsCenter, usedPoly.getCoefficients()); - - dRight = defaultXCenter; - - if (m1 > 0) { - pointRight1 = FuPoint(x1 + dRight * cos(atan(-1 / m1)), - usedPoly.at(x1) + dRight * sin(atan(-1 / m1))); - } - else { - pointRight1 = FuPoint(x1 - dRight * cos(atan(-1 / m1)), - usedPoly.at(x1) - dRight * sin(atan(-1 / m1))); - } - - if (m2 > 0) { - pointRight2 = FuPoint(x2 + dRight * cos(atan(-1 / m2)), - usedPoly.at(x2) + dRight * sin(atan(-1 / m2))); - } - else { - pointRight2 = FuPoint(x2 - dRight * cos(atan(-1 / m2)), - usedPoly.at(x2) - dRight * sin(atan(-1 / m2))); - } - - if (m3 > 0) { - pointRight3 = FuPoint(x3 + dRight * cos(atan(-1 / m3)), - usedPoly.at(x3) + dRight * sin(atan(-1 / m3))); - } - else { - pointRight3 = FuPoint(x3 - dRight * cos(atan(-1 / m3)), - usedPoly.at(x3) - dRight * sin(atan(-1 / m3))); - } - } - else if (position == RIGHT) { - usedPoly = polyRight; - m1 = gradient(x1, pointsRight, usedPoly.getCoefficients()); - m2 = gradient(x2, pointsRight, usedPoly.getCoefficients()); - m3 = gradient(x3, pointsRight, usedPoly.getCoefficients()); - - dRight = defaultXCenter; - - if (m1 > 0) { - pointRight1 = FuPoint(x1 - dRight * cos(atan(-1 / m1)), - usedPoly.at(x1) - dRight * sin(atan(-1 / m1))); - } - else { - pointRight1 = FuPoint(x1 + dRight * cos(atan(-1 / m1)), - usedPoly.at(x1) + dRight * sin(atan(-1 / m1))); - } - - if (m2 > 0) { - pointRight2 = FuPoint(x2 - dRight * cos(atan(-1 / m2)), - usedPoly.at(x2) - dRight * sin(atan(-1 / m2))); - } - else { - pointRight2 = FuPoint(x2 + dRight * cos(atan(-1 / m2)), - usedPoly.at(x2) + dRight * sin(atan(-1 / m2))); - } - - if (m3 > 0) { - pointRight3 = FuPoint(x3 - dRight * cos(atan(-1 / m3)), - usedPoly.at(x3) - dRight * sin(atan(-1 / m3))); - } - else { - pointRight3 = FuPoint(x3 + dRight * cos(atan(-1 / m3)), - usedPoly.at(x3) + dRight * sin(atan(-1 / m3))); - } - } - - // create the lane polynomial out of the shifted points - lanePoly.addDataXY(pointRight1); - lanePoly.addDataXY(pointRight2); - lanePoly.addDataXY(pointRight3); - - lanePolynomial.setLanePoly(lanePoly); - lanePolynomial.setDetected(); - lanePolynomial.setLastUsedPosition(position); -}*/ - -/** - * Decide, which of the detected polynomials (if there are any) should be used - * as reference for creating the lane polynomials. - * - * @param startX The x-value, starting from which we compare the detected polys - */ -void cLaneDetectionFu::detectLane() { - int startX = detectLaneStartX; - - if (polyDetectedLeft && !polyDetectedCenter && !polyDetectedRight) { - createLanePoly(LEFT); - } - else if (!polyDetectedLeft && polyDetectedCenter && !polyDetectedRight) { - createLanePoly(CENTER); - } - else if (!polyDetectedLeft && !polyDetectedCenter && polyDetectedRight) { - createLanePoly(RIGHT); - } - else if (polyDetectedLeft && polyDetectedCenter && !polyDetectedRight) { - double gradLeft = gradient(startX, pointsLeft, - polyLeft.getCoefficients()); - - double gradCenter = nextGradient(startX, polyLeft, pointsLeft, - pointsCenter, polyLeft.getCoefficients(), - polyCenter.getCoefficients(), gradLeft); - - if (gradientsSimilar(gradLeft, gradCenter)) { - createLanePoly(CENTER); - } - else { - if (bestPolyLeft.second >= bestPolyCenter.second) { - createLanePoly(LEFT); - } - else { - createLanePoly(CENTER); - } - } - } - else if (!polyDetectedLeft && polyDetectedCenter && polyDetectedRight) { - double gradCenter = gradient(startX, pointsCenter, - polyCenter.getCoefficients()); - - double gradRight = nextGradient(startX, polyCenter, pointsCenter, - pointsRight, polyCenter.getCoefficients(), - polyRight.getCoefficients(), gradCenter); - - if (gradientsSimilar(gradCenter, gradRight)) { - createLanePoly(RIGHT); - } - else { - if (bestPolyCenter.second >= bestPolyRight.second) { - createLanePoly(CENTER); - } - else { - createLanePoly(RIGHT); - } - } - } - else if (polyDetectedLeft && !polyDetectedCenter && polyDetectedRight) { - double gradLeft = gradient(startX, pointsLeft, - polyLeft.getCoefficients()); - - double gradRight = nextGradient(startX, polyLeft, pointsLeft, - pointsRight, polyLeft.getCoefficients(), - polyRight.getCoefficients(), gradLeft); - - if (gradientsSimilar(gradLeft, gradRight)) { - createLanePoly(RIGHT); - } - else { - if (bestPolyLeft.second >= bestPolyRight.second) { - createLanePoly(LEFT); - } - else { - createLanePoly(RIGHT); - } - } - } - else if (polyDetectedLeft && polyDetectedCenter && polyDetectedRight) { - double gradRight = gradient(startX, pointsRight, - polyRight.getCoefficients()); - - double gradCenter2 = gradient(startX, pointsCenter, - polyCenter.getCoefficients()); - - double gradCenter1 = nextGradient(startX, polyRight, pointsRight, - pointsCenter, polyRight.getCoefficients(), - polyCenter.getCoefficients(), gradRight); - - //double gradLeft1 = nextGradient(startX, polyRight, pointsRight, - // pointsLeft, polyRight.getCoefficients(), - // polyLeft.getCoefficients(), gradRight); - - double gradLeft2 = nextGradient(startX, polyCenter, pointsCenter, - pointsLeft, polyCenter.getCoefficients(), - polyLeft.getCoefficients(), gradCenter2); - - if (gradientsSimilar(gradRight, gradCenter1)) { - // ?! - //if (gradientsSimilar(gradCenter1, gradLeft1)) { - createLanePoly(RIGHT); - //} - //else { - // createLanePoly(RIGHT); - //} - } - else { - if (gradientsSimilar(gradCenter2, gradLeft2)) { - createLanePoly(CENTER); - } - else { - //ROS_ERROR("Creating lane according to max proportion."); - ePosition maxPos = maxProportion(); - - createLanePoly(maxPos); - } - } - } - else if (!polyDetectedLeft && !polyDetectedCenter && !polyDetectedRight) { - lanePoly.clear(); - - lanePolynomial.setNotDetected(); - } -} - /** * Starts the RANSAC algorithm for detecting each of the three lane marking * polynomials. @@ -1854,17 +1382,6 @@ bool cLaneDetectionFu::ransacInternal(ePosition position, return false; } - //ROS_ERROR("length: %d", sortedMarkings.size()); - - /* - * threshold value for minimal distance between lowest and highest point - * to prevent the left and center markings being chosen over the right markings - */ - /*if (sortedMarkings.at(sortedMarkings.size() - 1).getY() - sortedMarkings.at(0).getY() < 30) { - ROS_ERROR("Minimal distance"); - return false; - }*/ - std::vector> tmpSupporters = std::vector>(); // vectors for points selected from the bottom, mid and top of the sorted @@ -2014,8 +1531,7 @@ bool cLaneDetectionFu::ransacInternal(ePosition position, * @param prevPoly The previous polynomial detected at this position * @return True, if the polynomial counts as valid */ -bool cLaneDetectionFu::polyValid(ePosition position, NewtonPolynomial poly, - NewtonPolynomial prevPoly) { +bool cLaneDetectionFu::polyValid(ePosition position, NewtonPolynomial poly, NewtonPolynomial prevPoly) { if (prevPoly.getDegree() != -1) { return isSimilar(poly, prevPoly); @@ -2045,64 +1561,35 @@ bool cLaneDetectionFu::polyValid(ePosition position, NewtonPolynomial poly, } } - /*FuPoint p1 = FuPoint(poly.at(polyY1), polyY1); //y = 75 - - if (horizDistanceToDefaultLine(position, p1) > 10) { - //ROS_INFO("Poly was to far away from default line at y = 25"); - return false; - } - - FuPoint p2 = FuPoint(poly.at(polyY2), polyY2); //y = 60 - - if (horizDistanceToDefaultLine(position, p2) > 20) { - //ROS_INFO("Poly was to far away from default line at y = 25"); - return false; - } - - FuPoint p3 = FuPoint(poly.at(polyY3), polyY3); //y = 40 - - if (horizDistanceToDefaultLine(position, p3) > 40) { - //ROS_INFO("Poly was to far away from default line at y = 30"); - return false; - }*/ - - //ROS_INFO("Poly is valid"); - return true; } bool cLaneDetectionFu::isSimilar(const NewtonPolynomial &poly1, const NewtonPolynomial &poly2) { - //ROS_ERROR("y1: %d, y2: %d, y3: %d", polyY1, polyY2, polyY3); - FuPoint p1 = FuPoint(poly1.at(polyY1), polyY1); FuPoint p2 = FuPoint(poly2.at(polyY1), polyY1); - if (horizDistance(p1, p2) > interestDistancePoly) {//0.05 * meters) { - //ROS_INFO("Poly was to far away from previous poly at y = 25"); + if (horizDistance(p1, p2) > interestDistancePoly) { //0.05 * meters return false; } FuPoint p3 = FuPoint(poly1.at(polyY2), polyY2); FuPoint p4 = FuPoint(poly2.at(polyY2), polyY2); - if (horizDistance(p3, p4) > interestDistancePoly) {//0.05 * meters) { - //ROS_INFO("Poly was to far away from previous poly at y = 30"); + if (horizDistance(p3, p4) > interestDistancePoly) { //0.05 * meters return false; } FuPoint p5 = FuPoint(poly1.at(polyY3), polyY3); FuPoint p6 = FuPoint(poly2.at(polyY3), polyY3); - if (horizDistance(p5, p6) > interestDistancePoly) {//0.05 * meters) { - //ROS_INFO("Poly was to far away from previous poly at y = 30"); + if (horizDistance(p5, p6) > interestDistancePoly) { //0.05 * meters return false; } return true; } -void cLaneDetectionFu::pubRGBImageMsg(cv::Mat& rgb_mat, image_transport::CameraPublisher publisher) -{ +void cLaneDetectionFu::pubRGBImageMsg(cv::Mat& rgb_mat, image_transport::CameraPublisher publisher) { sensor_msgs::ImagePtr rgb_img(new sensor_msgs::Image); rgb_img->header.seq = head_sequence_id; @@ -2160,12 +1647,12 @@ void cLaneDetectionFu::pubAngle() { /* * filter too rash steering angles / jitter in polynomial data */ - /*if (std::abs(result - lastAngle) > maxAngleDiff) { + if (std::abs(result - lastAngle) > maxAngleDiff) { if (result - lastAngle > 0) result = lastAngle + maxAngleDiff; else result = lastAngle - maxAngleDiff; - }*/ + } lastAngle = result; @@ -2176,8 +1663,7 @@ void cLaneDetectionFu::pubAngle() { publish_angle.publish(angleMsg); } -void cLaneDetectionFu::pubGradientAngle() -{ +void cLaneDetectionFu::pubGradientAngle() { int position = proj_image_h - angleAdjacentLeg; double val1 = lanePolynomial.getLanePoly().at(position); @@ -2222,8 +1708,7 @@ void cLaneDetectionFu::config_callback(line_detection_fu::LaneDetectionConfig &c scanlines = getScanlines(); } -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { ros::init(argc, argv, "cLaneDetectionFu"); ros::NodeHandle nh; @@ -2234,8 +1719,7 @@ int main(int argc, char **argv) f = boost::bind(&cLaneDetectionFu::config_callback, &node, _1, _2); server.setCallback(f); - while(ros::ok()) - { + while(ros::ok()) { ros::spinOnce(); } return 0; diff --git a/src/line_detection_fu/src/laneDetection.h b/src/line_detection_fu/src/laneDetection.h index 65be997f..bac80ed9 100644 --- a/src/line_detection_fu/src/laneDetection.h +++ b/src/line_detection_fu/src/laneDetection.h @@ -321,14 +321,7 @@ class cLaneDetectionFu int horizDistance(FuPoint &p1, FuPoint &p2); - void createLanePoly(ePosition position); - - void detectLane(); - - double gradient(double, std::vector>&, - std::vector); - - double gradient(double x, NewtonPolynomial polynomial); + double gradient(double, std::vector>&, std::vector); double intersection(FuPoint&, double&, std::vector>&, std::vector&); From fc9ac9cebbaf6d6ae0477a46eb84e5b3a2f47521 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Thu, 14 Sep 2017 13:40:06 +0200 Subject: [PATCH 56/79] started to move debug commands to its own class --- src/line_detection_fu/src/laneDetection.cpp | 49 +++---------------- src/line_detection_fu/src/laneDetection.h | 3 ++ .../src/tools/DebugUtils.cpp | 41 ++++++++++++++++ src/line_detection_fu/src/tools/DebugUtils.h | 18 +++++++ 4 files changed, 69 insertions(+), 42 deletions(-) create mode 100644 src/line_detection_fu/src/tools/DebugUtils.cpp create mode 100644 src/line_detection_fu/src/tools/DebugUtils.h diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index c91d8b17..99eaf520 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -6,7 +6,7 @@ using namespace std; #define PAINT_OUTPUT //#define PAINT_OUTPUT_CUT_IMAGE //#define PAINT_OUTPUT_IP_MAPPED_IMAGE -//#define PAINT_OUTPUT_ROI +#define PAINT_OUTPUT_ROI #define PAINT_OUTPUT_LANE_MARKINGS #define PAINT_OUTPUT_LANE_POLYNOMIALS #define PUBLISH_DEBUG_OUTPUT @@ -33,9 +33,7 @@ const static int g_kernel1DWidth = 5; std::size_t frame = 0; -cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) - : nh_(nh), priv_nh_("~") -{ +cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") { std::string node_name = ros::this_node::getName(); ROS_ERROR("Node name: %s",node_name.c_str()); @@ -138,6 +136,7 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) movedPointForAngle = FuPoint(); pointForAngle = FuPoint(); + debugUtils = DebugUtils(); maxDistance = 1; @@ -160,8 +159,7 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) - if (!rgb_camera_info) - { + if (!rgb_camera_info) { rgb_camera_info.reset(new sensor_msgs::CameraInfo()); rgb_camera_info->width = proj_image_w; rgb_camera_info->height = proj_image_h+50; @@ -182,12 +180,10 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) #endif } -cLaneDetectionFu::~cLaneDetectionFu() -{ +cLaneDetectionFu::~cLaneDetectionFu() { } -void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) -{ +void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) { // clear some stuff from the last cycle bestPolyLeft = std::make_pair(NewtonPolynomial(), 0); bestPolyCenter = std::make_pair(NewtonPolynomial(), 0); @@ -226,38 +222,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) //---------------------- DEBUG OUTPUT EDGES ---------------------------------// #ifdef PAINT_OUTPUT_ROI - transformedImagePaintable = transformedImage.clone(); - cv::cvtColor(transformedImagePaintable,transformedImagePaintable,CV_GRAY2BGR); - for(int i = 0; i < (int)edges.size(); i++) - { - for(int j=0; j < edges[i].size(); j++) { - FuPoint edge = edges[i][j].getImgPos(); - cv::Point edgeLoc = cv::Point(edge.getX(), edge.getY()); - cv::circle(transformedImagePaintable,edgeLoc,1,cv::Scalar(0,0,edges[i][j].getValue()),-1); - } - } - - /*cv::Point2d p1(proj_image_w_half-(roi_bottom_w/2),maxYRoi-1); - cv::Point2d p2(proj_image_w_half+(roi_bottom_w/2),maxYRoi-1); - cv::Point2d p3(proj_image_w_half+(roi_top_w/2),minYPolyRoi); - cv::Point2d p4(proj_image_w_half-(roi_top_w/2),minYPolyRoi); - cv::line(transformedImagePaintable,p1,p2,cv::Scalar(0,200,0)); - cv::line(transformedImagePaintable,p2,p3,cv::Scalar(0,200,0)); - cv::line(transformedImagePaintable,p3,p4,cv::Scalar(0,200,0)); - cv::line(transformedImagePaintable,p4,p1,cv::Scalar(0,200,0));*/ - - /*for(int i = 0; i < (int)scanlines.size(); i++) - { - LineSegment scanline = scanlines[i][0]; - cv::Point scanlineStart = cv::Point(scanline.getStart().getX(), scanline.getStart().getY()); - cv::Point scanlineEnd = cv::Point(scanline.getEnd().getX(), scanline.getEnd().getY()); - cv::line(transformedImagePaintable,scanlineStart,scanlineEnd,cv::Scalar(255,0,0)); - }*/ - - - cv::namedWindow("ROI, scanlines and edges", WINDOW_NORMAL); - cv::imshow("ROI, scanlines and edges", transformedImagePaintable); - cv::waitKey(1); + debugUtils.paintOutputRoi(transformedImage, edges); #endif //---------------------- END DEBUG OUTPUT EDGES ------------------------------// diff --git a/src/line_detection_fu/src/laneDetection.h b/src/line_detection_fu/src/laneDetection.h index bac80ed9..b172e8ba 100644 --- a/src/line_detection_fu/src/laneDetection.h +++ b/src/line_detection_fu/src/laneDetection.h @@ -56,6 +56,7 @@ THIS SOFTWARE IS PROVIDED BY AUDI AG AND CONTRIBUTORS �AS IS� AND ANY EXPRES #include "tools/LanePolynomial.h" #include "tools/enums.h" #include "tools/IPMapper.h" +#include "tools/DebugUtils.h" #include #include @@ -243,6 +244,8 @@ class cLaneDetectionFu vector> movedPointsRight; + DebugUtils debugUtils; + int laneMarkingSquaredThreshold; int angleAdjacentLeg; diff --git a/src/line_detection_fu/src/tools/DebugUtils.cpp b/src/line_detection_fu/src/tools/DebugUtils.cpp new file mode 100644 index 00000000..b04514d7 --- /dev/null +++ b/src/line_detection_fu/src/tools/DebugUtils.cpp @@ -0,0 +1,41 @@ +#include "DebugUtils.h" + +using namespace std; + +DebugUtils() { +} + +void paintOutputRoi(cv::Mat img, vector> edges) { + cv::Mat transformedImagePaintable = img.clone(); + cv::cvtColor(transformedImagePaintable,transformedImagePaintable,CV_GRAY2BGR); + for(int i = 0; i < (int)edges.size(); i++) + { + for(int j=0; j < edges[i].size(); j++) { + FuPoint edge = edges[i][j].getImgPos(); + cv::Point edgeLoc = cv::Point(edge.getX(), edge.getY()); + cv::circle(transformedImagePaintable,edgeLoc,1,cv::Scalar(0,0,edges[i][j].getValue()),-1); + } + } + + /*cv::Point2d p1(proj_image_w_half-(roi_bottom_w/2),maxYRoi-1); + cv::Point2d p2(proj_image_w_half+(roi_bottom_w/2),maxYRoi-1); + cv::Point2d p3(proj_image_w_half+(roi_top_w/2),minYPolyRoi); + cv::Point2d p4(proj_image_w_half-(roi_top_w/2),minYPolyRoi); + cv::line(transformedImagePaintable,p1,p2,cv::Scalar(0,200,0)); + cv::line(transformedImagePaintable,p2,p3,cv::Scalar(0,200,0)); + cv::line(transformedImagePaintable,p3,p4,cv::Scalar(0,200,0)); + cv::line(transformedImagePaintable,p4,p1,cv::Scalar(0,200,0));*/ + + /*for(int i = 0; i < (int)scanlines.size(); i++) + { + LineSegment scanline = scanlines[i][0]; + cv::Point scanlineStart = cv::Point(scanline.getStart().getX(), scanline.getStart().getY()); + cv::Point scanlineEnd = cv::Point(scanline.getEnd().getX(), scanline.getEnd().getY()); + cv::line(transformedImagePaintable,scanlineStart,scanlineEnd,cv::Scalar(255,0,0)); + }*/ + + + cv::namedWindow("ROI, scanlines and edges", WINDOW_NORMAL); + cv::imshow("ROI, scanlines and edges", transformedImagePaintable); + cv::waitKey(1); +} \ No newline at end of file diff --git a/src/line_detection_fu/src/tools/DebugUtils.h b/src/line_detection_fu/src/tools/DebugUtils.h new file mode 100644 index 00000000..52598f2e --- /dev/null +++ b/src/line_detection_fu/src/tools/DebugUtils.h @@ -0,0 +1,18 @@ +#include + +#include +#include +#include + +#include "Edges.h" + +#include + +using namespace std; + +class DebugUtils { +public: + DebugUtils(); + + void paintOutputRoi(cv::Mat img, vector> edges); +}; \ No newline at end of file From 8f5a80acf0d3165c3008d9491e54c50a66a5fedf Mon Sep 17 00:00:00 2001 From: lars Date: Thu, 14 Sep 2017 14:51:56 +0200 Subject: [PATCH 57/79] fixed a compilation issue --- src/line_detection_fu/CMakeLists.txt | 3 ++- src/line_detection_fu/src/tools/DebugUtils.cpp | 8 ++++---- src/line_detection_fu/src/tools/DebugUtils.h | 5 ++--- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/line_detection_fu/CMakeLists.txt b/src/line_detection_fu/CMakeLists.txt index 0d52bfe5..b4e21869 100644 --- a/src/line_detection_fu/CMakeLists.txt +++ b/src/line_detection_fu/CMakeLists.txt @@ -81,6 +81,7 @@ add_library(LaneDetectorFu src/tools/NewtonPolynomial.cpp src/tools/LanePolynomial.cpp src/tools/IPMapper.cpp + src/tools/DebugUtils.cpp #src/LaneDetector.cpp #src/StrongClassifier.cpp #src/WeakClassifier.cpp @@ -157,4 +158,4 @@ set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}") # make sure configure headers are built before any node using them -add_dependencies(line_detection_fu_node ${PROJECT_NAME}_gencfg) \ No newline at end of file +add_dependencies(line_detection_fu_node ${PROJECT_NAME}_gencfg) diff --git a/src/line_detection_fu/src/tools/DebugUtils.cpp b/src/line_detection_fu/src/tools/DebugUtils.cpp index b04514d7..1c53b579 100644 --- a/src/line_detection_fu/src/tools/DebugUtils.cpp +++ b/src/line_detection_fu/src/tools/DebugUtils.cpp @@ -1,11 +1,11 @@ #include "DebugUtils.h" using namespace std; +using namespace cv; -DebugUtils() { -} +DebugUtils::DebugUtils() {} -void paintOutputRoi(cv::Mat img, vector> edges) { +void DebugUtils::paintOutputRoi(cv::Mat img, vector> edges) { cv::Mat transformedImagePaintable = img.clone(); cv::cvtColor(transformedImagePaintable,transformedImagePaintable,CV_GRAY2BGR); for(int i = 0; i < (int)edges.size(); i++) @@ -38,4 +38,4 @@ void paintOutputRoi(cv::Mat img, vector> edges) { cv::namedWindow("ROI, scanlines and edges", WINDOW_NORMAL); cv::imshow("ROI, scanlines and edges", transformedImagePaintable); cv::waitKey(1); -} \ No newline at end of file +} diff --git a/src/line_detection_fu/src/tools/DebugUtils.h b/src/line_detection_fu/src/tools/DebugUtils.h index 52598f2e..24ddd0bc 100644 --- a/src/line_detection_fu/src/tools/DebugUtils.h +++ b/src/line_detection_fu/src/tools/DebugUtils.h @@ -8,11 +8,10 @@ #include -using namespace std; class DebugUtils { public: DebugUtils(); - void paintOutputRoi(cv::Mat img, vector> edges); -}; \ No newline at end of file + void paintOutputRoi(cv::Mat img, std::vector> edges); +}; From 66d95a93fa7ba062e6ee0a784a292abee8331d14 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Thu, 14 Sep 2017 20:51:52 +0200 Subject: [PATCH 58/79] continued DebugUtils --- .../launch/line_detection_fu.launch | 4 +- src/line_detection_fu/src/laneDetection.cpp | 343 +----------------- src/line_detection_fu/src/laneDetection.h | 24 +- .../src/tools/DebugUtils.cpp | 309 +++++++++++++++- src/line_detection_fu/src/tools/DebugUtils.h | 72 +++- 5 files changed, 392 insertions(+), 360 deletions(-) diff --git a/src/line_detection_fu/launch/line_detection_fu.launch b/src/line_detection_fu/launch/line_detection_fu.launch index 090739e8..875b8774 100644 --- a/src/line_detection_fu/launch/line_detection_fu.launch +++ b/src/line_detection_fu/launch/line_detection_fu.launch @@ -7,8 +7,8 @@ - - + + diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index 99eaf520..37b5f18b 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -1,38 +1,10 @@ -#include #include "laneDetection.h" using namespace std; -#define PAINT_OUTPUT -//#define PAINT_OUTPUT_CUT_IMAGE -//#define PAINT_OUTPUT_IP_MAPPED_IMAGE -#define PAINT_OUTPUT_ROI -#define PAINT_OUTPUT_LANE_MARKINGS -#define PAINT_OUTPUT_LANE_POLYNOMIALS -#define PUBLISH_DEBUG_OUTPUT -//#define PUBLISH_DEBUG_IMAGES - -static const uint32_t MY_ROS_QUEUE_SIZE = 1; - -#define PI 3.14159265 - -image_transport::CameraPublisher image_publisher; -image_transport::CameraPublisher image_publisher_ransac; -image_transport::CameraPublisher image_publisher_lane_markings; - - -//msgs head -unsigned int head_sequence_id = 0; -ros::Time head_time_stamp; -std::string rgb_frame_id = "_rgb_optical_frame"; -sensor_msgs::CameraInfoPtr rgb_camera_info; - // try kernel width 5 for now const static int g_kernel1DWidth = 5; - -std::size_t frame = 0; - cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") { std::string node_name = ros::this_node::getName(); @@ -46,8 +18,8 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") priv_nh_.param(node_name+"/proj_image_h", proj_image_h, 40); priv_nh_.param(node_name+"/proj_image_w", proj_image_w, 80); priv_nh_.param(node_name+"/proj_image_horizontal_offset", proj_image_horizontal_offset, 0); - priv_nh_.param(node_name+"/roi_top_w", roi_top_w, 62); - priv_nh_.param(node_name+"/roi_bottom_w", roi_bottom_w, 30); + priv_nh_.param(node_name+"/roiTopW", roi_top_w, 62); + priv_nh_.param(node_name+"/roiBottomW", roi_bottom_w, 30); priv_nh_.param(node_name+"/maxYRoi", maxYRoi, 5); priv_nh_.param(node_name+"/minYDefaultRoi", minYDefaultRoi, 39); @@ -125,9 +97,6 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") pointsCenter = std::vector>(); pointsRight = std::vector>(); - lanePoly = NewtonPolynomial(); - lanePolynomial = LanePolynomial(); - movedPolyLeft = NewtonPolynomial(); movedPolyCenter = NewtonPolynomial(); movedPolyRight = NewtonPolynomial(); @@ -136,48 +105,21 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") movedPointForAngle = FuPoint(); pointForAngle = FuPoint(); - debugUtils = DebugUtils(); - maxDistance = 1; lastAngle = 0; - - head_time_stamp = ros::Time::now(); read_images_ = nh.subscribe(nh_.resolveName(camera_name), MY_ROS_QUEUE_SIZE, &cLaneDetectionFu::ProcessInput,this); publish_angle = nh.advertise("/lane_model/angle", MY_ROS_QUEUE_SIZE); - image_transport::ImageTransport image_transport(nh); - - image_publisher = image_transport.advertiseCamera("/lane_model/lane_model_image", MY_ROS_QUEUE_SIZE); - - #ifdef PUBLISH_DEBUG_OUTPUT - image_publisher_ransac = image_transport.advertiseCamera("/lane_model/ransac", MY_ROS_QUEUE_SIZE); - image_publisher_lane_markings = image_transport.advertiseCamera("/lane_model/lane_markings", MY_ROS_QUEUE_SIZE); - #endif - - - - if (!rgb_camera_info) { - rgb_camera_info.reset(new sensor_msgs::CameraInfo()); - rgb_camera_info->width = proj_image_w; - rgb_camera_info->height = proj_image_h+50; - } - //from camera properties and ROI etc we get scanlines (=line segments, úseÄky) //these line segments are lines in image on whose we look for edges //the outer vector represents rows on image, inner vector is vector of line segments of one row, usualy just one line segment //we should generate this only once in the beginning! or even just have it pregenerated for our cam scanlines = getScanlines(); - #ifdef PUBLISH_DEBUG_IMAGES - mkdir("groupedLaneMarkings", S_IRWXU); - mkdir("ransac", S_IRWXU); - - system("exec rm -r ./groupedLaneMarkings/*"); - system("exec rm -r ./ransac/*"); - #endif + debugUtils = DebugUtils(nh, minYPolyRoi, maxYRoi, roi_bottom_w, roi_top_w, proj_image_w, proj_image_h); } cLaneDetectionFu::~cLaneDetectionFu() { @@ -189,6 +131,10 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) { bestPolyCenter = std::make_pair(NewtonPolynomial(), 0); bestPolyRight = std::make_pair(NewtonPolynomial(), 0); + supportersLeft.clear(); + supportersCenter.clear(); + supportersRight.clear(); + //use ROS image_proc or opencv instead of ip mapper? cv_bridge::CvImagePtr cv_ptr; @@ -197,18 +143,8 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) { cv::Mat image = cv_ptr->image.clone(); Mat cut_image = image(cv::Rect(0,cam_h * 0.25f,cam_w,cam_h * 0.75f)); - #ifdef PAINT_OUTPUT_CUT_IMAGE - cv::imshow("Cut image", cut_image); - cv::waitKey(1); - #endif - Mat remapped_image = ipMapper.remap(cut_image); - #ifdef PAINT_OUTPUT_IP_MAPPED_IMAGE - cv::imshow("IPmapped image", remapped_image); - cv::waitKey(1); - #endif - cv::Mat transformedImage = remapped_image(cv::Rect((cam_w/2)-proj_image_w_half+proj_image_horizontal_offset, proj_y_start,proj_image_w,proj_image_h)).clone(); @@ -219,229 +155,31 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) { vector> edges = cLaneDetectionFu::scanImage(transformedImage); cv::Mat transformedImagePaintable; - - //---------------------- DEBUG OUTPUT EDGES ---------------------------------// - #ifdef PAINT_OUTPUT_ROI - debugUtils.paintOutputRoi(transformedImage, edges); - #endif - //---------------------- END DEBUG OUTPUT EDGES ------------------------------// + debugUtils.drawEdgeWindow(transformedImage, edges); //edges -> lane markings vector> laneMarkings = cLaneDetectionFu::extractLaneMarkings(edges); + debugUtils.drawLaneMarkingsWindow(transformedImage, laneMarkings); - //---------------------- DEBUG OUTPUT LANE MARKINGS ---------------------------------// - #ifdef PAINT_OUTPUT_LANE_MARKINGS - transformedImagePaintable = transformedImage.clone(); - cv::cvtColor(transformedImagePaintable,transformedImagePaintable,CV_GRAY2BGR); - for(int i = 0; i < (int)laneMarkings.size(); i++) - { - FuPoint marking = laneMarkings[i]; - cv::Point markingLoc = cv::Point(marking.getX(), marking.getY()); - cv::circle(transformedImagePaintable,markingLoc,1,cv::Scalar(0,255,0),-1); - } - - cv::namedWindow("Lane Markings", WINDOW_NORMAL); - cv::imshow("Lane Markings", transformedImagePaintable); - cv::waitKey(1); - #endif - //---------------------- END DEBUG OUTPUT LANE MARKINGS ------------------------------// findLanePositions(laneMarkings); + // start actual execution buildLaneMarkingsLists(laneMarkings); - - //---------------------- DEBUG OUTPUT GROUPED LANE MARKINGS ---------------------------------// - #ifdef PUBLISH_DEBUG_OUTPUT - transformedImagePaintable = transformedImage.clone(); - cv::cvtColor(transformedImagePaintable,transformedImagePaintable,CV_GRAY2BGR); - - debugPaintPoints(transformedImagePaintable, cv::Scalar(0,0,255), laneMarkingsLeft); - debugPaintPoints(transformedImagePaintable, cv::Scalar(0,255,0), laneMarkingsCenter); - debugPaintPoints(transformedImagePaintable, cv::Scalar(255,0,0), laneMarkingsRight); - debugPaintPoints(transformedImagePaintable, cv::Scalar(0,255,255), laneMarkingsNotUsed); - - debugPaintPolynom(transformedImagePaintable, cv::Scalar(0, 255, 255), polyDetectedLeft ? polyLeft : movedPolyLeft, minYPolyRoi, maxYRoi); - debugPaintPolynom(transformedImagePaintable, cv::Scalar(255, 255, 0), polyDetectedCenter ? polyCenter : movedPolyCenter, minYPolyRoi, maxYRoi); - debugPaintPolynom(transformedImagePaintable, cv::Scalar(255, 0, 255), polyDetectedRight ? polyRight : movedPolyRight, minYPolyRoi, maxYRoi); - - cv::Point2d p1l(defaultXLeft,minYPolyRoi); - cv::Point2d p2l(defaultXLeft,maxYRoi-1); - cv::line(transformedImagePaintable,p1l,p2l,cv::Scalar(0,0,255)); - - cv::Point2d p1c(defaultXCenter,minYPolyRoi); - cv::Point2d p2c(defaultXCenter,maxYRoi-1); - cv::line(transformedImagePaintable,p1c,p2c,cv::Scalar(0,255,0)); - - cv::Point2d p1r(defaultXRight,minYPolyRoi); - cv::Point2d p2r(defaultXRight,maxYRoi-1); - cv::line(transformedImagePaintable,p1r,p2r,cv::Scalar(255,0,0)); - - /*cv::Point2d p1(proj_image_w_half-(roi_bottom_w/2),maxYRoi-1); - cv::Point2d p2(proj_image_w_half+(roi_bottom_w/2),maxYRoi-1); - cv::Point2d p3(proj_image_w_half+(roi_top_w/2),minYPolyRoi); - cv::Point2d p4(proj_image_w_half-(roi_top_w/2),minYPolyRoi); - cv::line(transformedImagePaintable,p1,p2,cv::Scalar(0,200,0)); - cv::line(transformedImagePaintable,p2,p3,cv::Scalar(0,200,0)); - cv::line(transformedImagePaintable,p3,p4,cv::Scalar(0,200,0)); - cv::line(transformedImagePaintable,p4,p1,cv::Scalar(0,200,0));*/ - - debugPaintPoints(transformedImagePaintable, cv::Scalar(0,0,255), supportersLeft); - debugPaintPoints(transformedImagePaintable, cv::Scalar(0,255,0), supportersCenter); - debugPaintPoints(transformedImagePaintable, cv::Scalar(255,0,0), supportersRight); - - pubRGBImageMsg(transformedImagePaintable, image_publisher_lane_markings); - - - #ifdef PAINT_OUTPUT - cv::namedWindow("Grouped Lane Markings", WINDOW_NORMAL); - cv::imshow("Grouped Lane Markings", transformedImagePaintable); - cv::waitKey(1); - - #ifdef PUBLISH_DEBUG_IMAGES - debugWriteImg(transformedImagePaintable, "groupedLaneMarkings"); - #endif - #endif - #endif - //---------------------- END DEBUG OUTPUT GROUPED LANE MARKINGS ------------------------------// - - supportersLeft.clear(); - supportersCenter.clear(); - supportersRight.clear(); + debugUtils.drawGroupedLaneMarkingsWindow(transformedImage, defaultXLeft, defaultXCenter, defaultXRight, + polyDetectedLeft ? polyLeft : movedPolyLeft, + polyDetectedCenter ? polyCenter : movedPolyCenter, + polyDetectedRight ? polyRight : movedPolyRight, + laneMarkingsLeft, laneMarkingsCenter, laneMarkingsRight, laneMarkingsNotUsed); ransac(); - generateMovedPolynomials(); - //---------------------- DEBUG OUTPUT RANSAC POLYNOMIALS ---------------------------------// - #ifdef PUBLISH_DEBUG_OUTPUT - cv::Mat transformedImagePaintableRansac = transformedImage.clone(); - cv::cvtColor(transformedImagePaintableRansac,transformedImagePaintableRansac,CV_GRAY2BGR); - - debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(0, 0, 255), polyLeft, minYPolyRoi, maxYRoi); - debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(0, 255, 0), polyCenter, minYPolyRoi, maxYRoi); - debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(255, 0, 0), polyRight, minYPolyRoi, maxYRoi); - - debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(139, 0, 139), movedPolyLeft, minYPolyRoi, maxYRoi); - debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(0, 0, 0), movedPolyCenter, minYPolyRoi, maxYRoi); - debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(255, 120, 0), movedPolyRight, minYPolyRoi, maxYRoi); - - - pubRGBImageMsg(transformedImagePaintableRansac, image_publisher_ransac); - - #ifdef PAINT_OUTPUT - cv::namedWindow("RANSAC results", WINDOW_NORMAL); - cv::imshow("RANSAC results", transformedImagePaintableRansac); - cv::waitKey(1); - - #ifdef PUBLISH_DEBUG_IMAGES - debugWriteImg(transformedImagePaintableRansac, "ransac"); - #endif - #endif - #endif - //---------------------- END DEBUG OUTPUT RANSAC POLYNOMIALS ------------------------------// + debugUtils.drawRansacWindow(transformedImage, polyLeft, polyCenter, polyRight, + movedPolyLeft, movedPolyCenter, movedPolyRight); pubAngle(); - - cv::Mat transformedImagePaintableLaneModel = transformedImage.clone(); - cv::cvtColor(transformedImagePaintableLaneModel,transformedImagePaintableLaneModel,CV_GRAY2BGR); - - if (polyDetectedRight || isPolyMovedRight) { - int r = lanePolynomial.getLastUsedPosition() == LEFT ? 255 : 0; - int g = lanePolynomial.getLastUsedPosition() == CENTER ? 255 : 0; - int b = lanePolynomial.getLastUsedPosition() == RIGHT ? 255 : 0; - - - /*for(int i = minYPolyRoi; i < maxYRoi; i++) { - cv::Point pointLoc = cv::Point(lanePolynomial.getLanePoly().at(i)+proj_image_w_half, i); - cv::circle(transformedImagePaintableLaneModel, pointLoc, 0, cv::Scalar(b,g,r), -1); - } - - std::vector> supps; - switch (lanePolynomial.getLastUsedPosition()) { - case LEFT: - supps = supportersLeft; - break; - case CENTER: - supps = supportersCenter; - break; - case RIGHT: - supps = supportersRight; - break; - default: - ROS_ERROR("No last used position"); - supps = supportersRight; - break; - }; - - for(int i = 0; i < (int)supps.size(); i++) { - FuPoint supp = supps[i]; - cv::Point suppLoc = cv::Point(supp.getX(), supp.getY()); - cv::circle(transformedImagePaintableLaneModel, suppLoc, 1, cv::Scalar(b,g,r), -1); - }*/ - - cv::Point pointLoc = cv::Point(proj_image_w_half, proj_image_h); - cv::circle(transformedImagePaintableLaneModel, pointLoc, 2, cv::Scalar(0,0,255), -1); - - cv::Point anglePointLoc = cv::Point(sin(lastAngle * PI / 180) * angleAdjacentLeg + proj_image_w_half, proj_image_h - (cos(lastAngle * PI / 180) * angleAdjacentLeg)); - cv::line(transformedImagePaintableLaneModel, pointLoc, anglePointLoc, cv::Scalar(255,255,255)); - - cv::Point startNormalPoint = cv::Point(pointForAngle.getX(), pointForAngle.getY()); - cv::circle(transformedImagePaintableLaneModel, startNormalPoint, 2, cv::Scalar(100,100,100), -1); - - cv::Point targetPoint = cv::Point(movedPointForAngle.getX(), movedPointForAngle.getY()); - cv::circle(transformedImagePaintableLaneModel, targetPoint, 2, cv::Scalar(0,0,255), -1); - - /*cv::Point adjacentLegPoint = cv::Point(proj_image_w_half, proj_image_h - adjacentLeg); - cv::line(transformedImagePaintableLaneModel, pointLoc, adjacentLegPoint, cv::Scalar(255,0,0)); - - cv::Point oppositeLegPoint = cv::Point(proj_image_w_half + oppositeLeg, proj_image_h - adjacentLeg); - cv::line(transformedImagePaintableLaneModel, adjacentLegPoint, oppositeLegPoint, cv::Scalar(0,255,0));*/ - - double m = -gradientForAngle; - - double n = pointForAngle.getY() - m * pointForAngle.getX(); - double x = 10; - double y = m * x + n; - - cv::Point endNormalPoint = cv::Point(x, y); - cv::line(transformedImagePaintableLaneModel, startNormalPoint, endNormalPoint, cv::Scalar(0,0,0)); - - } else { - cv::Point pointLoc = cv::Point(5, 5); - cv::circle(transformedImagePaintableLaneModel, pointLoc, 3, cv::Scalar(0,0,255), 0); - } - - pubRGBImageMsg(transformedImagePaintableLaneModel, image_publisher); - - //---------------------- DEBUG OUTPUT LANE POLYNOMIAL ---------------------------------// - #ifdef PAINT_OUTPUT_LANE_POLYNOMIALS - cv::namedWindow("Lane polynomial", WINDOW_NORMAL); - cv::imshow("Lane polynomial", transformedImagePaintableLaneModel); - cv::waitKey(1); - #endif - //---------------------- END DEBUG OUTPUT LANE POLYNOMIAL ------------------------------// - - frame++; -} - -void cLaneDetectionFu::debugPaintPolynom(cv::Mat &m, cv::Scalar color, NewtonPolynomial &p, int start, int end) { - cv::Point point; - for (int i = start; i < end; i++) { - point = cv::Point(p.at(i), i); - cv::circle(m, point, 0, color, -1); - } -} - -void cLaneDetectionFu::debugPaintPoints(cv::Mat &m, cv::Scalar color, vector> &points) { - for(FuPoint point : points) { - cv::Point pointLoc = cv::Point(point.getX(), point.getY()); - cv::circle(m, pointLoc, 1, color, -1); - } -} - -void cLaneDetectionFu::debugWriteImg(cv::Mat &image, string folder) { - stringstream img; - img << "./" << folder << "/" << frame << ".jpg"; - cv::imwrite(img.str(), image); + debugUtils.drawAngleWindow(transformedImage, polyDetectedRight, isPolyMovedRight, lastAngle, gradientForAngle, + angleAdjacentLeg, pointForAngle, movedPointForAngle); } /* EdgeDetector methods */ @@ -1554,32 +1292,6 @@ bool cLaneDetectionFu::isSimilar(const NewtonPolynomial &poly1, const NewtonPoly return true; } -void cLaneDetectionFu::pubRGBImageMsg(cv::Mat& rgb_mat, image_transport::CameraPublisher publisher) { - sensor_msgs::ImagePtr rgb_img(new sensor_msgs::Image); - - rgb_img->header.seq = head_sequence_id; - rgb_img->header.stamp = head_time_stamp; - rgb_img->header.frame_id = rgb_frame_id; - - rgb_img->width = rgb_mat.cols; - rgb_img->height = rgb_mat.rows; - - rgb_img->encoding = sensor_msgs::image_encodings::BGR8; - rgb_img->is_bigendian = 0; - - int step = sizeof(unsigned char) * 3 * rgb_img->width; - int size = step * rgb_img->height; - rgb_img->step = step; - rgb_img->data.resize(size); - memcpy(&(rgb_img->data[0]), rgb_mat.data, size); - - rgb_camera_info->header.frame_id = rgb_frame_id; - rgb_camera_info->header.stamp = head_time_stamp; - rgb_camera_info->header.seq = head_sequence_id; - - publisher.publish(rgb_img, rgb_camera_info); -} - void cLaneDetectionFu::pubAngle() { if (!polyDetectedRight && !isPolyMovedRight) { return; @@ -1628,23 +1340,6 @@ void cLaneDetectionFu::pubAngle() { publish_angle.publish(angleMsg); } -void cLaneDetectionFu::pubGradientAngle() { - int position = proj_image_h - angleAdjacentLeg; - - double val1 = lanePolynomial.getLanePoly().at(position); - double val2 = lanePolynomial.getLanePoly().at(position+1); - - double result = atan (val1-val2) * 180 / PI; - - lastAngle = result; - - std_msgs::Float32 angleMsg; - - angleMsg.data = result; - - publish_angle.publish(angleMsg); -} - void cLaneDetectionFu::config_callback(line_detection_fu::LaneDetectionConfig &config, uint32_t level) { ROS_ERROR("Reconfigure Request"); diff --git a/src/line_detection_fu/src/laneDetection.h b/src/line_detection_fu/src/laneDetection.h index b172e8ba..6dc8b62c 100644 --- a/src/line_detection_fu/src/laneDetection.h +++ b/src/line_detection_fu/src/laneDetection.h @@ -35,7 +35,6 @@ THIS SOFTWARE IS PROVIDED BY AUDI AG AND CONTRIBUTORS �AS IS� AND ANY EXPRES #include #include #include -#include #include #include #include @@ -64,9 +63,9 @@ THIS SOFTWARE IS PROVIDED BY AUDI AG AND CONTRIBUTORS �AS IS� AND ANY EXPRES using namespace std; +class DebugUtils; -class cLaneDetectionFu -{ +class cLaneDetectionFu { private: // the node handle @@ -227,13 +226,6 @@ class cLaneDetectionFu NewtonPolynomial prevPolyCenter; NewtonPolynomial prevPolyRight; - /** - * The polynomial representing the center of the right lane - */ - NewtonPolynomial lanePoly; - - LanePolynomial lanePolynomial; - NewtonPolynomial movedPolyLeft; NewtonPolynomial movedPolyCenter; NewtonPolynomial movedPolyRight; @@ -264,26 +256,20 @@ class cLaneDetectionFu double laneWidth = 45.f; - void debugPaintPolynom(cv::Mat &m, cv::Scalar color, NewtonPolynomial &p, int start, int end); - - void debugPaintPoints(cv::Mat &m, cv::Scalar color, std::vector> &points); + public: - void debugWriteImg(cv::Mat &image, string folder); + static const uint32_t MY_ROS_QUEUE_SIZE = 1; - public: + static constexpr double PI = 3.14159265; cLaneDetectionFu(ros::NodeHandle nh); virtual ~cLaneDetectionFu(); void ProcessInput(const sensor_msgs::Image::ConstPtr& msg); - - void pubRGBImageMsg(cv::Mat& rgb_mat, image_transport::CameraPublisher publisher); void pubAngle(); - void pubGradientAngle(); - std::vector> > getScanlines(); std::vector > scanImage(cv::Mat image); diff --git a/src/line_detection_fu/src/tools/DebugUtils.cpp b/src/line_detection_fu/src/tools/DebugUtils.cpp index 1c53b579..f2ac2880 100644 --- a/src/line_detection_fu/src/tools/DebugUtils.cpp +++ b/src/line_detection_fu/src/tools/DebugUtils.cpp @@ -3,24 +3,58 @@ using namespace std; using namespace cv; -DebugUtils::DebugUtils() {} +DebugUtils::DebugUtils(ros::NodeHandle &nh, int minYPolyRoi, int maxYRoi, int roiBottomW, int roiTopW, int projImageW, int projImageH) { + this->minYPolyRoi = minYPolyRoi; + this->maxYRoi = maxYRoi; + this->roiBottomW = roiBottomW; + this->roiTopW = roiTopW; + this->projImageHalfW = projImageW / 2; + this->projImageH = projImageH; -void DebugUtils::paintOutputRoi(cv::Mat img, vector> edges) { - cv::Mat transformedImagePaintable = img.clone(); - cv::cvtColor(transformedImagePaintable,transformedImagePaintable,CV_GRAY2BGR); - for(int i = 0; i < (int)edges.size(); i++) - { - for(int j=0; j < edges[i].size(); j++) { + head_time_stamp = ros::Time::now(); + + if (PUBLISH_IMAGES) { + image_transport::ImageTransport image_transport(nh); + + image_publisher = image_transport.advertiseCamera("/lane_model/lane_model_image", cLaneDetectionFu::MY_ROS_QUEUE_SIZE); + image_publisher_ransac = image_transport.advertiseCamera("/lane_model/ransac", cLaneDetectionFu::MY_ROS_QUEUE_SIZE); + image_publisher_lane_markings = image_transport.advertiseCamera("/lane_model/lane_markings", cLaneDetectionFu::MY_ROS_QUEUE_SIZE); + + if (!rgb_camera_info) { + rgb_camera_info.reset(new sensor_msgs::CameraInfo()); + rgb_camera_info->width = projImageW; + rgb_camera_info->height = projImageH + 50; + } + } + + if (SAVE_FRAME_IMAGES) { + mkdir("groupedLaneMarkings", S_IRWXU); + mkdir("ransac", S_IRWXU); + + system("exec rm -r ./groupedLaneMarkings/*"); + system("exec rm -r ./ransac/*"); + } +} + +void DebugUtils::drawEdgeWindow(Mat &img, vector> edges) { + if (!SHOW_EDGE_WINDOW) { + return; + } + + Mat transformedImagePaintable = img.clone(); + cvtColor(transformedImagePaintable, transformedImagePaintable, CV_GRAY2BGR); + for (int i = 0; i < (int) edges.size(); i++) { + for (int j = 0; j < edges[i].size(); j++) { FuPoint edge = edges[i][j].getImgPos(); - cv::Point edgeLoc = cv::Point(edge.getX(), edge.getY()); - cv::circle(transformedImagePaintable,edgeLoc,1,cv::Scalar(0,0,edges[i][j].getValue()),-1); + Point edgeLoc = Point(edge.getX(), edge.getY()); + circle(transformedImagePaintable, edgeLoc, 1, Scalar(0, 0, edges[i][j].getValue()), -1); } } - /*cv::Point2d p1(proj_image_w_half-(roi_bottom_w/2),maxYRoi-1); - cv::Point2d p2(proj_image_w_half+(roi_bottom_w/2),maxYRoi-1); - cv::Point2d p3(proj_image_w_half+(roi_top_w/2),minYPolyRoi); - cv::Point2d p4(proj_image_w_half-(roi_top_w/2),minYPolyRoi); + /*cv::Point2d p1(projImageHalfW-(roiBottomW/2),maxYRoi-1); + cv::Point2d p2(projImageHalfW+(roiBottomW/2),maxYRoi-1); + cv::Point2d p3(projImageHalfW+(roiTopW/2),minYPolyRoi); + cv::Point2d p4(projImageHalfW-(roiTopW/2),minYPolyRoi); cv::line(transformedImagePaintable,p1,p2,cv::Scalar(0,200,0)); cv::line(transformedImagePaintable,p2,p3,cv::Scalar(0,200,0)); cv::line(transformedImagePaintable,p3,p4,cv::Scalar(0,200,0)); @@ -39,3 +73,252 @@ void DebugUtils::paintOutputRoi(cv::Mat img, vector> edges) { cv::imshow("ROI, scanlines and edges", transformedImagePaintable); cv::waitKey(1); } + +void DebugUtils::drawLaneMarkingsWindow(Mat &img, vector> &laneMarkings) { + if (!SHOW_LANE_MARKINGS_WINDOW) { + return; + } + + Mat transformedImagePaintable = img.clone(); + cv::cvtColor(transformedImagePaintable, transformedImagePaintable, CV_GRAY2BGR); + for (int i = 0; i < (int) laneMarkings.size(); i++) { + FuPoint marking = laneMarkings[i]; + cv::Point markingLoc = cv::Point(marking.getX(), marking.getY()); + cv::circle(transformedImagePaintable, markingLoc, 1, cv::Scalar(0, 255, 0), -1); + } + + cv::namedWindow("Lane Markings", WINDOW_NORMAL); + cv::imshow("Lane Markings", transformedImagePaintable); + cv::waitKey(1); +} + +void DebugUtils::drawGroupedLaneMarkingsWindow(Mat &img, int defaultXLeft, int defaultXCenter, int defaultXRight, + NewtonPolynomial &polyLeft, NewtonPolynomial &polyCenter, + NewtonPolynomial &polyRight, + std::vector> &laneMarkingsLeft, + std::vector> &laneMarkingsCenter, + std::vector> &laneMarkingsRight, + std::vector> &laneMarkingsNotUsed) { + if (!PUBLISH_IMAGES && !SHOW_GROUPED_LANE_MARKINGS_WINDOW && !SAVE_FRAME_IMAGES) { + return; + } + + Mat transformedImagePaintable = img.clone(); + cv::cvtColor(transformedImagePaintable,transformedImagePaintable,CV_GRAY2BGR); + + debugPaintPoints(transformedImagePaintable, Scalar(0,0,255), laneMarkingsLeft); + debugPaintPoints(transformedImagePaintable, Scalar(0,255,0), laneMarkingsCenter); + debugPaintPoints(transformedImagePaintable, Scalar(255,0,0), laneMarkingsRight); + debugPaintPoints(transformedImagePaintable, Scalar(0,255,255), laneMarkingsNotUsed); + + debugPaintPolynom(transformedImagePaintable, cv::Scalar(0, 255, 255), polyLeft, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintable, cv::Scalar(255, 255, 0), polyCenter, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintable, cv::Scalar(255, 0, 255), polyRight, minYPolyRoi, maxYRoi); + + cv::Point2d p1l(defaultXLeft,minYPolyRoi); + cv::Point2d p2l(defaultXLeft,maxYRoi-1); + cv::line(transformedImagePaintable,p1l,p2l,cv::Scalar(0,0,255)); + + cv::Point2d p1c(defaultXCenter,minYPolyRoi); + cv::Point2d p2c(defaultXCenter,maxYRoi-1); + cv::line(transformedImagePaintable,p1c,p2c,cv::Scalar(0,255,0)); + + cv::Point2d p1r(defaultXRight,minYPolyRoi); + cv::Point2d p2r(defaultXRight,maxYRoi-1); + cv::line(transformedImagePaintable,p1r,p2r,cv::Scalar(255,0,0)); + + cv::Point2d p1(projImageHalfW-(roiBottomW/2),maxYRoi-1); + cv::Point2d p2(projImageHalfW+(roiBottomW/2),maxYRoi-1); + cv::Point2d p3(projImageHalfW+(roiTopW/2),minYPolyRoi); + cv::Point2d p4(projImageHalfW-(roiTopW/2),minYPolyRoi); + cv::line(transformedImagePaintable,p1,p2,cv::Scalar(0,200,0)); + cv::line(transformedImagePaintable,p2,p3,cv::Scalar(0,200,0)); + cv::line(transformedImagePaintable,p3,p4,cv::Scalar(0,200,0)); + cv::line(transformedImagePaintable,p4,p1,cv::Scalar(0,200,0)); + + if (PUBLISH_IMAGES) { + pubRGBImageMsg(transformedImagePaintable, image_publisher_lane_markings); + } + + + if (SHOW_GROUPED_LANE_MARKINGS_WINDOW) { + cv::namedWindow("Grouped Lane Markings", WINDOW_NORMAL); + cv::imshow("Grouped Lane Markings", transformedImagePaintable); + cv::waitKey(1); + } + + if (SAVE_FRAME_IMAGES) { + debugWriteImg(transformedImagePaintable, "groupedLaneMarkings"); + } +} + +void DebugUtils::drawRansacWindow(cv::Mat &img, NewtonPolynomial &polyLeft, NewtonPolynomial &polyCenter, + NewtonPolynomial &polyRight, NewtonPolynomial &movedPolyLeft, + NewtonPolynomial &movedPolyCenter, NewtonPolynomial &movedPolyRight) { + + if (!PUBLISH_IMAGES && !SHOW_RANSAC_WINDOW && !SAVE_FRAME_IMAGES) { + return; + } + + cv::Mat transformedImagePaintableRansac = img.clone(); + cv::cvtColor(transformedImagePaintableRansac,transformedImagePaintableRansac,CV_GRAY2BGR); + + debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(0, 0, 255), polyLeft, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(0, 255, 0), polyCenter, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(255, 0, 0), polyRight, minYPolyRoi, maxYRoi); + + debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(139, 0, 139), movedPolyLeft, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(0, 0, 0), movedPolyCenter, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(255, 120, 0), movedPolyRight, minYPolyRoi, maxYRoi); + + + if (PUBLISH_IMAGES) { + pubRGBImageMsg(transformedImagePaintableRansac, image_publisher_ransac); + } + + if (SHOW_RANSAC_WINDOW) { + cv::namedWindow("RANSAC results", WINDOW_NORMAL); + cv::imshow("RANSAC results", transformedImagePaintableRansac); + cv::waitKey(1); + } + + if (SAVE_FRAME_IMAGES) { + debugWriteImg(transformedImagePaintableRansac, "ransac"); + } +} + +void DebugUtils::drawAngleWindow(Mat &img, bool polyDetectedRight, bool isPolyMovedRight, double lastAngle, + double gradientForAngle, int angleAdjacentLeg, FuPoint &pointForAngle, + FuPoint &movedPointForAngle) { + frame++; + + if (!PUBLISH_IMAGES && !SHOW_ANGLE_WINDOW) { + return; + } + + Mat transformedImagePaintableLaneModel = img.clone(); + cvtColor(transformedImagePaintableLaneModel,transformedImagePaintableLaneModel,CV_GRAY2BGR); + + if (polyDetectedRight || isPolyMovedRight) { + /*int r = lanePolynomial.getLastUsedPosition() == LEFT ? 255 : 0; + int g = lanePolynomial.getLastUsedPosition() == CENTER ? 255 : 0; + int b = lanePolynomial.getLastUsedPosition() == RIGHT ? 255 : 0; + + + for(int i = minYPolyRoi; i < maxYRoi; i++) { + cv::Point pointLoc = cv::Point(lanePolynomial.getLanePoly().at(i)+projImageHalfW, i); + cv::circle(transformedImagePaintableLaneModel, pointLoc, 0, cv::Scalar(b,g,r), -1); + } + + std::vector> supps; + switch (lanePolynomial.getLastUsedPosition()) { + case LEFT: + supps = supportersLeft; + break; + case CENTER: + supps = supportersCenter; + break; + case RIGHT: + supps = supportersRight; + break; + default: + ROS_ERROR("No last used position"); + supps = supportersRight; + break; + }; + + for(int i = 0; i < (int)supps.size(); i++) { + FuPoint supp = supps[i]; + cv::Point suppLoc = cv::Point(supp.getX(), supp.getY()); + cv::circle(transformedImagePaintableLaneModel, suppLoc, 1, cv::Scalar(b,g,r), -1); + }*/ + + cv::Point pointLoc = cv::Point(projImageHalfW, projImageH); + cv::circle(transformedImagePaintableLaneModel, pointLoc, 2, cv::Scalar(0,0,255), -1); + + cv::Point anglePointLoc = cv::Point(sin(lastAngle * cLaneDetectionFu::PI / 180) * angleAdjacentLeg + projImageHalfW, projImageH - (cos(lastAngle * cLaneDetectionFu::PI / 180) * angleAdjacentLeg)); + cv::line(transformedImagePaintableLaneModel, pointLoc, anglePointLoc, cv::Scalar(255,255,255)); + + cv::Point startNormalPoint = cv::Point(pointForAngle.getX(), pointForAngle.getY()); + cv::circle(transformedImagePaintableLaneModel, startNormalPoint, 2, cv::Scalar(100,100,100), -1); + + cv::Point targetPoint = cv::Point(movedPointForAngle.getX(), movedPointForAngle.getY()); + cv::circle(transformedImagePaintableLaneModel, targetPoint, 2, cv::Scalar(0,0,255), -1); + + /*cv::Point adjacentLegPoint = cv::Point(projImageHalfW, proj_image_h - adjacentLeg); + cv::line(transformedImagePaintableLaneModel, pointLoc, adjacentLegPoint, cv::Scalar(255,0,0)); + + cv::Point oppositeLegPoint = cv::Point(projImageHalfW + oppositeLeg, proj_image_h - adjacentLeg); + cv::line(transformedImagePaintableLaneModel, adjacentLegPoint, oppositeLegPoint, cv::Scalar(0,255,0));*/ + + double m = -gradientForAngle; + + double n = pointForAngle.getY() - m * pointForAngle.getX(); + double x = 10; + double y = m * x + n; + + cv::Point endNormalPoint = cv::Point(x, y); + cv::line(transformedImagePaintableLaneModel, startNormalPoint, endNormalPoint, cv::Scalar(0,0,0)); + + } else { + cv::Point pointLoc = cv::Point(5, 5); + cv::circle(transformedImagePaintableLaneModel, pointLoc, 3, cv::Scalar(0,0,255), 0); + } + + if (PUBLISH_IMAGES) { + pubRGBImageMsg(transformedImagePaintableLaneModel, image_publisher); + } + + if (SHOW_ANGLE_WINDOW) { + cv::namedWindow("Lane polynomial", WINDOW_NORMAL); + cv::imshow("Lane polynomial", transformedImagePaintableLaneModel); + cv::waitKey(1); + } +} + +void DebugUtils::pubRGBImageMsg(cv::Mat& rgb_mat, image_transport::CameraPublisher publisher) { + sensor_msgs::ImagePtr rgb_img(new sensor_msgs::Image); + + rgb_img->header.seq = head_sequence_id; + rgb_img->header.stamp = head_time_stamp; + rgb_img->header.frame_id = rgb_frame_id; + + rgb_img->width = rgb_mat.cols; + rgb_img->height = rgb_mat.rows; + + rgb_img->encoding = sensor_msgs::image_encodings::BGR8; + rgb_img->is_bigendian = 0; + + int step = sizeof(unsigned char) * 3 * rgb_img->width; + int size = step * rgb_img->height; + rgb_img->step = step; + rgb_img->data.resize(size); + memcpy(&(rgb_img->data[0]), rgb_mat.data, size); + + rgb_camera_info->header.frame_id = rgb_frame_id; + rgb_camera_info->header.stamp = head_time_stamp; + rgb_camera_info->header.seq = head_sequence_id; + + publisher.publish(rgb_img, rgb_camera_info); +} + +void DebugUtils::debugPaintPolynom(cv::Mat &m, cv::Scalar color, NewtonPolynomial &p, int start, int end) { + cv::Point point; + for (int i = start; i < end; i++) { + point = cv::Point(p.at(i), i); + cv::circle(m, point, 0, color, -1); + } +} + +void DebugUtils::debugPaintPoints(cv::Mat &m, cv::Scalar color, vector> &points) { + for(FuPoint point : points) { + cv::Point pointLoc = cv::Point(point.getX(), point.getY()); + cv::circle(m, pointLoc, 1, color, -1); + } +} + +void DebugUtils::debugWriteImg(cv::Mat &image, string folder) { + stringstream img; + img << "./" << folder << "/" << frame << ".jpg"; + cv::imwrite(img.str(), image); +} \ No newline at end of file diff --git a/src/line_detection_fu/src/tools/DebugUtils.h b/src/line_detection_fu/src/tools/DebugUtils.h index 24ddd0bc..af2f14c5 100644 --- a/src/line_detection_fu/src/tools/DebugUtils.h +++ b/src/line_detection_fu/src/tools/DebugUtils.h @@ -1,17 +1,85 @@ +#ifndef _DebugUtils_FILTER_HEADER_ +#define _DebugUtils_FILTER_HEADER_ + #include +#include +#include + #include #include #include +#include "../laneDetection.h" #include "Edges.h" +#include "NewtonPolynomial.h" +#include "FuPoint.h" #include +#include class DebugUtils { public: - DebugUtils(); + DebugUtils(ros::NodeHandle &nh, int minYPolyRoi, int maxYRoi, int roiBottomW, int roiTopW, + int projImageW, int projImageH); + + void drawEdgeWindow(cv::Mat &img, std::vector> edges); + + void drawLaneMarkingsWindow(cv::Mat &img, std::vector> &laneMarkings); + + void drawGroupedLaneMarkingsWindow(cv::Mat &img, int defaultXLeft, int defaultXCenter, int defaultXRight, + NewtonPolynomial &polyLeft, NewtonPolynomial &polyCenter, NewtonPolynomial &polyRight, + std::vector> &laneMarkingsLeft, + std::vector> &laneMarkingsCenter, + std::vector> &laneMarkingsRight, + std::vector> &laneMarkingsNotUsed); + + void drawRansacWindow(cv::Mat &img, NewtonPolynomial &polyLeft, NewtonPolynomial &polyCenter, NewtonPolynomial &polyRight, + NewtonPolynomial &movedPolyLeft, NewtonPolynomial &movedPolyCenter, NewtonPolynomial &movedPolyRight); + + void drawAngleWindow(cv::Mat &img, bool polyDetectedRight, bool isPolyMovedRight, double lastAngle, + double gradientForAngle, int angleAdjacentLeg, FuPoint &pointForAngle, + FuPoint &movedPointForAngle); + +private: + const bool PUBLISH_IMAGES = true; + const bool SAVE_FRAME_IMAGES = true; + + bool SHOW_EDGE_WINDOW = true; + bool SHOW_LANE_MARKINGS_WINDOW = true; + bool SHOW_GROUPED_LANE_MARKINGS_WINDOW = true; + bool SHOW_RANSAC_WINDOW = true; + bool SHOW_ANGLE_WINDOW = true; - void paintOutputRoi(cv::Mat img, std::vector> edges); + image_transport::CameraPublisher image_publisher; + image_transport::CameraPublisher image_publisher_ransac; + image_transport::CameraPublisher image_publisher_lane_markings; + + // params for pubRGBImageMsg + unsigned int head_sequence_id = 0; + ros::Time head_time_stamp; + std::string rgb_frame_id = "_rgb_optical_frame"; + sensor_msgs::CameraInfoPtr rgb_camera_info; + + // params from laneDetectionNode + int minYPolyRoi; + int maxYRoi; + int roiBottomW; + int roiTopW; + int projImageHalfW; + int projImageH; + + // frame number for saving image files + std::size_t frame = 0; + + void pubRGBImageMsg(cv::Mat& rgb_mat, image_transport::CameraPublisher publisher); + + void debugPaintPolynom(cv::Mat &m, cv::Scalar color, NewtonPolynomial &p, int start, int end); + + void debugPaintPoints(cv::Mat &m, cv::Scalar color, std::vector> &points); + + void debugWriteImg(cv::Mat &image, std::string folder); }; + +#endif \ No newline at end of file From 9cf6df19dfeb2f4336ef709bc211ad66d50c5905 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Sat, 16 Sep 2017 15:18:53 +0200 Subject: [PATCH 59/79] moved debug methods in laneDetectin class again, improved paramter naming, updated paramter values in config and laneDetection class, restructured laneDetection class --- src/line_detection_fu/CMakeLists.txt | 1 - src/line_detection_fu/cfg/LaneDetection.cfg | 41 +- .../launch/line_detection_fu.launch | 57 +- src/line_detection_fu/src/laneDetection.cpp | 1485 ++++++++++------- src/line_detection_fu/src/laneDetection.h | 449 ++--- .../src/tools/DebugUtils.cpp | 324 ---- src/line_detection_fu/src/tools/DebugUtils.h | 85 - 7 files changed, 1191 insertions(+), 1251 deletions(-) delete mode 100644 src/line_detection_fu/src/tools/DebugUtils.cpp delete mode 100644 src/line_detection_fu/src/tools/DebugUtils.h diff --git a/src/line_detection_fu/CMakeLists.txt b/src/line_detection_fu/CMakeLists.txt index b4e21869..04e4ab54 100644 --- a/src/line_detection_fu/CMakeLists.txt +++ b/src/line_detection_fu/CMakeLists.txt @@ -81,7 +81,6 @@ add_library(LaneDetectorFu src/tools/NewtonPolynomial.cpp src/tools/LanePolynomial.cpp src/tools/IPMapper.cpp - src/tools/DebugUtils.cpp #src/LaneDetector.cpp #src/StrongClassifier.cpp #src/WeakClassifier.cpp diff --git a/src/line_detection_fu/cfg/LaneDetection.cfg b/src/line_detection_fu/cfg/LaneDetection.cfg index 5328b730..11e0b9c6 100755 --- a/src/line_detection_fu/cfg/LaneDetection.cfg +++ b/src/line_detection_fu/cfg/LaneDetection.cfg @@ -5,27 +5,26 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() -gen.add("interestDistancePoly", int_t, 0, "Accepted horizontal distance between a potential point and a detected polynomial", 10, 0, 100) -gen.add("interestDistanceDefault", int_t, 0, "Accepted horizontal distance between a potential point and a default line", 20, 0, 100) -gen.add("iterationsRansac", int_t, 0, "Iterations during ransac", 30, 0, 100) -gen.add("maxYRoi", int_t, 0, "Maximum y value for points on a line", 159, 0, 160) -gen.add("minYDefaultRoi", int_t, 0, "Minimum y value for points to check distance to default lane positions (defaultXLeft etc) ", 110, 0, 160) -gen.add("minYPolyRoi", int_t, 0, "Minimum y value for points on lane marking if a polynomial was detected in a previous frame", 50, 0, 160) -gen.add("polyY1", int_t, 0, "Y of first point of generated polynomial during ransac iteration. Horizontal distance of this point is compared to default lane and to previous found polynomial.", 155, 0, 160) -gen.add("polyY2", int_t, 0, "Y of second point. See polyY1", 145, 0, 160) -gen.add("polyY3", int_t, 0, "Y of third point. See polyY1", 130, 0, 160) -gen.add("detectLaneStartX",int_t, 0, "X value used for gradient calculation in detectLane", 155, 0, 160) -gen.add("maxAngleDiff", int_t, 0, "Optional smoothing when angle diff larger than this", 999, 1, 999) -gen.add("proj_y_start", int_t, 0, "Y position of processed window inside of IPmapped img", 50, 0, 80) -gen.add("roi_top_w", int_t, 0, "Width of top of roi", 70, 0, 160) -gen.add("roi_bottom_w", int_t, 0, "Width of bottom of roi", 70, 0, 160) -gen.add("proportionThreshould", double_t, 0, "Minimum proportion of supporter points for possible polynomial to accept polynomial as valid during ransac iteration", .6, 0, 1) -gen.add("m_gradientThreshold", int_t, 0, "Edge Detection: Minimum value of sum after applying kernel", 10, 0, 100) -gen.add("m_nonMaxWidth", int_t, 0, "Edge Detection: Minimum width between 2 maxima points on a scanline", 10, 0, 100) -gen.add("laneMarkingSquaredThreshold", int_t, 0, "Lane Extraction: Maximum value of squared subtraction of possible start and end point of lane marking", 36, 0, 100) -gen.add("angleAdjacentLeg", int_t, 0, "-Y position of where the angle of polynomial is computed", 100, 0, 160) -gen.add("scanlinesVerticalDistance", int_t, 0, "Distance between 2 vertical scanlines", 2, 0, 160) -gen.add("scanlinesMaxCount", int_t, 0, "Maximum of scanlines", 100, 0, 160) +gen.add("interestDistancePoly", int_t, 0, "Accepted horizontal distance between a potential point and a detected polynomial", 10, 0, 100) +gen.add("interestDistanceDefault", int_t, 0, "Accepted horizontal distance between a potential point and a default line", 15, 0, 100) +gen.add("iterationsRansac", int_t, 0, "Iterations during ransac", 38, 0, 100) +gen.add("maxYRoi", int_t, 0, "Maximum y value for points on a line", 100, 0, 160) +gen.add("minYDefaultRoi", int_t, 0, "Minimum y value for points to check distance to default lane positions (defaultXLeft etc) ", 10, 0, 160) +gen.add("minYPolyRoi", int_t, 0, "Minimum y value for points on lane marking if a polynomial was detected in a previous frame", 11, 0, 160) +gen.add("polyY1", int_t, 0, "Y of first point of generated polynomial during ransac iteration. Horizontal distance of this point is compared to default lane and to previous found polynomial.", 20, 0, 160) +gen.add("polyY2", int_t, 0, "Y of second point. See polyY1", 50, 0, 160) +gen.add("polyY3", int_t, 0, "Y of third point. See polyY1", 70, 0, 160) +gen.add("maxAngleDiff", int_t, 0, "Optional smoothing when angle diff larger than this", 14, 1, 999) +gen.add("projYStart", int_t, 0, "Y position of processed window inside of IPmapped img", 60, 0, 80) +gen.add("roiTopW", int_t, 0, "Width of top of roi", 150, 0, 160) +gen.add("roiBottomW", int_t, 0, "Width of bottom of roi", 57, 0, 160) +gen.add("proportionThreshould", double_t, 0, "Minimum proportion of supporter points for possible polynomial to accept polynomial as valid during ransac iteration", 0.68, 0, 1) +gen.add("gradientThreshold", int_t, 0, "Edge Detection: Minimum value of sum after applying kernel", 10, 0, 100) +gen.add("nonMaxWidth", int_t, 0, "Edge Detection: Minimum width between 2 maxima points on a scanline", 16, 0, 100) +gen.add("laneMarkingSquaredThreshold", int_t, 0, "Lane Extraction: Maximum value of squared subtraction of possible start and end point of lane marking", 36, 0, 100) +gen.add("angleAdjacentLeg", int_t, 0, "-Y position of where the angle of polynomial is computed", 18, 0, 160) +gen.add("scanlinesVerticalDistance", int_t, 0, "Distance between 2 vertical scanlines", 2, 0, 160) +gen.add("scanlinesMaxCount", int_t, 0, "Maximum of scanlines", 100, 0, 160) exit(gen.generate(PACKAGE, "line_detection_fu", "LaneDetection")) diff --git a/src/line_detection_fu/launch/line_detection_fu.launch b/src/line_detection_fu/launch/line_detection_fu.launch index 875b8774..e765d497 100644 --- a/src/line_detection_fu/launch/line_detection_fu.launch +++ b/src/line_detection_fu/launch/line_detection_fu.launch @@ -1,36 +1,35 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + - - - + + + diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index 37b5f18b..8efdea17 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -2,114 +2,136 @@ using namespace std; +// publish ransac and grouped lane frames to show it in rviz +const bool PUBLISH_IMAGES = true; +// save frames as images in ~/.ros/ +const bool SAVE_FRAME_IMAGES = true; + +// show windows with results of each step in pipeline of one frame +const bool SHOW_EDGE_WINDOW = true; +const bool SHOW_LANE_MARKINGS_WINDOW = true; +const bool SHOW_GROUPED_LANE_MARKINGS_WINDOW = true; +const bool SHOW_RANSAC_WINDOW = true; +const bool SHOW_ANGLE_WINDOW = true; + // try kernel width 5 for now const static int g_kernel1DWidth = 5; +// params for pubRGBImageMsg +unsigned int head_sequence_id = 0; +ros::Time head_time_stamp; +std::string rgb_frame_id = "_rgb_optical_frame"; +sensor_msgs::CameraInfoPtr rgb_camera_info; + +// frame number for saving image files +std::size_t frame = 0; + +const uint32_t MY_ROS_QUEUE_SIZE = 1; +const double PI = 3.14159265; + cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") { std::string node_name = ros::this_node::getName(); - ROS_ERROR("Node name: %s",node_name.c_str()); + ROS_ERROR("Node name: %s", node_name.c_str()); - priv_nh_.param(node_name+"/camera_name", camera_name, "/usb_cam/image_raw"); + priv_nh_.param(node_name + "/camera_name", camera_name, "/usb_cam/image_raw"); - priv_nh_.param(node_name+"/cam_w", cam_w, 640); - priv_nh_.param(node_name+"/cam_h", cam_h, 480); - priv_nh_.param(node_name+"/proj_y_start", proj_y_start, 415); - priv_nh_.param(node_name+"/proj_image_h", proj_image_h, 40); - priv_nh_.param(node_name+"/proj_image_w", proj_image_w, 80); - priv_nh_.param(node_name+"/proj_image_horizontal_offset", proj_image_horizontal_offset, 0); - priv_nh_.param(node_name+"/roiTopW", roi_top_w, 62); - priv_nh_.param(node_name+"/roiBottomW", roi_bottom_w, 30); - - priv_nh_.param(node_name+"/maxYRoi", maxYRoi, 5); - priv_nh_.param(node_name+"/minYDefaultRoi", minYDefaultRoi, 39); - priv_nh_.param(node_name+"/minYPolyRoi", minYPolyRoi, 39); + priv_nh_.param(node_name + "/camW", cam_w, 640); + priv_nh_.param(node_name + "/camH", cam_h, 480); + priv_nh_.param(node_name + "/projYStart", projYStart, 60); + priv_nh_.param(node_name + "/projImageH", projImageH, 100); + priv_nh_.param(node_name + "/projImageW", projImageW, 150); + priv_nh_.param(node_name + "/projImageHorizontalOffset", projImageHorizontalOffset, 0); + priv_nh_.param(node_name + "/roiTopW", roiTopW, 150); + priv_nh_.param(node_name + "/roiBottomW", roiBottomW, 57); - priv_nh_.param(node_name+"/interestDistancePoly", interestDistancePoly, 10); - priv_nh_.param(node_name+"/interestDistanceDefault", interestDistanceDefault, 10); - - priv_nh_.param(node_name+"/iterationsRansac", iterationsRansac, 10); - priv_nh_.param(node_name+"/proportionThreshould", proportionThreshould, 0.5); - - priv_nh_.param(node_name+"/m_gradientThreshold", m_gradientThreshold, 10); - priv_nh_.param(node_name+"/m_nonMaxWidth", m_nonMaxWidth, 10); - priv_nh_.param(node_name+"/laneMarkingSquaredThreshold", laneMarkingSquaredThreshold, 25); + priv_nh_.param(node_name + "/maxYRoi", maxYRoi, 100); + priv_nh_.param(node_name + "/minYDefaultRoi", minYDefaultRoi, 10); + priv_nh_.param(node_name + "/minYPolyRoi", minYPolyRoi, 11); - priv_nh_.param(node_name+"/angleAdjacentLeg", angleAdjacentLeg, 20); - - priv_nh_.param(node_name+"/scanlinesVerticalDistance", scanlinesVerticalDistance, 1); - priv_nh_.param(node_name+"/scanlinesMaxCount", scanlinesMaxCount, 100); + priv_nh_.param(node_name + "/interestDistancePoly", interestDistancePoly, 10); + priv_nh_.param(node_name + "/interestDistanceDefault", interestDistanceDefault, 5); - priv_nh_.param(node_name+"/detectLaneStartX", detectLaneStartX, 38); + priv_nh_.param(node_name + "/iterationsRansac", iterationsRansac, 38); + priv_nh_.param(node_name + "/proportionThreshould", proportionThreshould, 0.68); - priv_nh_.param(node_name+"/maxAngleDiff", maxAngleDiff, 10); + priv_nh_.param(node_name + "/gradientThreshold", gradientThreshold, 10); + priv_nh_.param(node_name + "/nonMaxWidth", nonMaxWidth, 16); + priv_nh_.param(node_name + "/laneMarkingSquaredThreshold", laneMarkingSquaredThreshold, 36); - priv_nh_.param(node_name+"/polyY1", polyY1, 35); - priv_nh_.param(node_name+"/polyY2", polyY2, 30); - priv_nh_.param(node_name+"/polyY3", polyY3, 15); + priv_nh_.param(node_name + "/angleAdjacentLeg", angleAdjacentLeg, 18); + + priv_nh_.param(node_name + "/scanlinesVerticalDistance", scanlinesVerticalDistance, 2); + priv_nh_.param(node_name + "/scanlinesMaxCount", scanlinesMaxCount, 100); + + priv_nh_.param(node_name + "/maxAngleDiff", maxAngleDiff, 14); + + priv_nh_.param(node_name + "/polyY1", polyY1, 20); + priv_nh_.param(node_name + "/polyY2", polyY2, 50); + priv_nh_.param(node_name + "/polyY3", polyY3, 70); double f_u; double f_v; double c_u; double c_v; - double cam_deg; - double cam_height; - int cam_h_half = cam_h/2; + double camDeg; + double camHeight; + int cam_h_half = cam_h / 2; - priv_nh_.param(node_name+"/f_u", f_u, 624.650635); - priv_nh_.param(node_name+"/f_v", f_v, 626.987244); - priv_nh_.param(node_name+"/c_u", c_u, 309.703230); - priv_nh_.param(node_name+"/c_v", c_v, 231.473613); - priv_nh_.param(node_name+"/cam_deg", cam_deg, 27); - priv_nh_.param(node_name+"/cam_height", cam_height, 18); + priv_nh_.param(node_name + "/f_u", f_u, 624.650635); + priv_nh_.param(node_name + "/f_v", f_v, 626.987244); + priv_nh_.param(node_name + "/c_u", c_u, 309.703230); + priv_nh_.param(node_name + "/c_v", c_v, 231.473613); + priv_nh_.param(node_name + "/camDeg", camDeg, 13.0); + priv_nh_.param(node_name + "/camHeight", camHeight, 36.2); - ipMapper = IPMapper(cam_w, cam_h_half, f_u, f_v, c_u, c_v, cam_deg, cam_height); + ipMapper = IPMapper(cam_w, cam_h_half, f_u, f_v, c_u, c_v, camDeg, camHeight); - proj_image_w_half = proj_image_w/2; + proj_image_w_half = projImageW / 2; - polyDetectedLeft = false; - polyDetectedCenter = false; - polyDetectedRight = false; + polyDetectedLeft = false; + polyDetectedCenter = false; + polyDetectedRight = false; - bestPolyLeft = std::make_pair(NewtonPolynomial(), 0); - bestPolyCenter = std::make_pair(NewtonPolynomial(), 0); - bestPolyRight = std::make_pair(NewtonPolynomial(), 0); + bestPolyLeft = std::make_pair(NewtonPolynomial(), 0); + bestPolyCenter = std::make_pair(NewtonPolynomial(), 0); + bestPolyRight = std::make_pair(NewtonPolynomial(), 0); - laneMarkingsLeft = std::vector>(); - laneMarkingsCenter = std::vector>(); - laneMarkingsRight = std::vector>(); - laneMarkingsNotUsed = std::vector>(); + laneMarkingsLeft = std::vector>(); + laneMarkingsCenter = std::vector>(); + laneMarkingsRight = std::vector>(); + laneMarkingsNotUsed = std::vector>(); - polyLeft = NewtonPolynomial(); - polyCenter = NewtonPolynomial(); - polyRight = NewtonPolynomial(); + polyLeft = NewtonPolynomial(); + polyCenter = NewtonPolynomial(); + polyRight = NewtonPolynomial(); - supportersLeft = std::vector>(); - supportersCenter = std::vector>(); - supportersRight = std::vector>(); + supportersLeft = std::vector>(); + supportersCenter = std::vector>(); + supportersRight = std::vector>(); - prevPolyLeft = NewtonPolynomial(); - prevPolyCenter = NewtonPolynomial(); - prevPolyRight = NewtonPolynomial(); + prevPolyLeft = NewtonPolynomial(); + prevPolyCenter = NewtonPolynomial(); + prevPolyRight = NewtonPolynomial(); - pointsLeft = std::vector>(); - pointsCenter = std::vector>(); - pointsRight = std::vector>(); + pointsLeft = std::vector>(); + pointsCenter = std::vector>(); + pointsRight = std::vector>(); - movedPolyLeft = NewtonPolynomial(); - movedPolyCenter = NewtonPolynomial(); - movedPolyRight = NewtonPolynomial(); + movedPolyLeft = NewtonPolynomial(); + movedPolyCenter = NewtonPolynomial(); + movedPolyRight = NewtonPolynomial(); - movedPointsRight = std::vector>(); + movedPointsRight = std::vector>(); movedPointForAngle = FuPoint(); pointForAngle = FuPoint(); maxDistance = 1; lastAngle = 0; - - read_images_ = nh.subscribe(nh_.resolveName(camera_name), MY_ROS_QUEUE_SIZE, &cLaneDetectionFu::ProcessInput,this); + + read_images_ = nh.subscribe(nh_.resolveName(camera_name), MY_ROS_QUEUE_SIZE, &cLaneDetectionFu::ProcessInput, this); publish_angle = nh.advertise("/lane_model/angle", MY_ROS_QUEUE_SIZE); @@ -119,13 +141,35 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") //we should generate this only once in the beginning! or even just have it pregenerated for our cam scanlines = getScanlines(); - debugUtils = DebugUtils(nh, minYPolyRoi, maxYRoi, roi_bottom_w, roi_top_w, proj_image_w, proj_image_h); + head_time_stamp = ros::Time::now(); + + if (PUBLISH_IMAGES) { + image_transport::ImageTransport image_transport(nh); + + image_publisher = image_transport.advertiseCamera("/lane_model/lane_model_image", MY_ROS_QUEUE_SIZE); + image_publisher_ransac = image_transport.advertiseCamera("/lane_model/ransac", MY_ROS_QUEUE_SIZE); + image_publisher_lane_markings = image_transport.advertiseCamera("/lane_model/lane_markings", MY_ROS_QUEUE_SIZE); + + if (!rgb_camera_info) { + rgb_camera_info.reset(new sensor_msgs::CameraInfo()); + rgb_camera_info->width = projImageW; + rgb_camera_info->height = projImageH + 50; + } + } + + if (SAVE_FRAME_IMAGES) { + mkdir("groupedLaneMarkings", S_IRWXU); + mkdir("ransac", S_IRWXU); + + system("exec rm -r ./groupedLaneMarkings/*"); + system("exec rm -r ./ransac/*"); + } } cLaneDetectionFu::~cLaneDetectionFu() { } -void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) { +void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr &msg) { // clear some stuff from the last cycle bestPolyLeft = std::make_pair(NewtonPolynomial(), 0); bestPolyCenter = std::make_pair(NewtonPolynomial(), 0); @@ -139,14 +183,14 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) { cv_bridge::CvImagePtr cv_ptr; cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8); - + cv::Mat image = cv_ptr->image.clone(); - Mat cut_image = image(cv::Rect(0,cam_h * 0.25f,cam_w,cam_h * 0.75f)); + Mat cut_image = image(cv::Rect(0, cam_h * 0.25f, cam_w, cam_h * 0.75f)); Mat remapped_image = ipMapper.remap(cut_image); - cv::Mat transformedImage = remapped_image(cv::Rect((cam_w/2)-proj_image_w_half+proj_image_horizontal_offset, - proj_y_start,proj_image_w,proj_image_h)).clone(); + cv::Mat transformedImage = remapped_image(cv::Rect((cam_w / 2) - proj_image_w_half + projImageHorizontalOffset, + projYStart, projImageW, projImageH)).clone(); cv::flip(transformedImage, transformedImage, 0); @@ -155,34 +199,29 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) { vector> edges = cLaneDetectionFu::scanImage(transformedImage); cv::Mat transformedImagePaintable; - debugUtils.drawEdgeWindow(transformedImage, edges); + drawEdgeWindow(transformedImage, edges); //edges -> lane markings vector> laneMarkings = cLaneDetectionFu::extractLaneMarkings(edges); - debugUtils.drawLaneMarkingsWindow(transformedImage, laneMarkings); + drawLaneMarkingsWindow(transformedImage, laneMarkings); findLanePositions(laneMarkings); // start actual execution buildLaneMarkingsLists(laneMarkings); - debugUtils.drawGroupedLaneMarkingsWindow(transformedImage, defaultXLeft, defaultXCenter, defaultXRight, - polyDetectedLeft ? polyLeft : movedPolyLeft, - polyDetectedCenter ? polyCenter : movedPolyCenter, - polyDetectedRight ? polyRight : movedPolyRight, - laneMarkingsLeft, laneMarkingsCenter, laneMarkingsRight, laneMarkingsNotUsed); + drawGroupedLaneMarkingsWindow(transformedImage); ransac(); generateMovedPolynomials(); - - debugUtils.drawRansacWindow(transformedImage, polyLeft, polyCenter, polyRight, - movedPolyLeft, movedPolyCenter, movedPolyRight); + drawRansacWindow(transformedImage); pubAngle(); - debugUtils.drawAngleWindow(transformedImage, polyDetectedRight, isPolyMovedRight, lastAngle, gradientForAngle, - angleAdjacentLeg, pointForAngle, movedPointForAngle); + drawAngleWindow(transformedImage); } -/* EdgeDetector methods */ +/* + * Pipeline: + */ /** * Compute scanlines. Each may consist of multiple segments, split at regions @@ -190,41 +229,41 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr& msg) { * @param side * @return vector of segments of scanlines, walk these segments with the kernel */ -vector> > cLaneDetectionFu::getScanlines() { - vector> > scanlines; +vector>> cLaneDetectionFu::getScanlines() { + vector>> scanlines; vector checkContour; - checkContour.push_back(cv::Point(proj_image_w_half-(roi_bottom_w/2),maxYRoi-1)); - checkContour.push_back(cv::Point(proj_image_w_half+(roi_bottom_w/2),maxYRoi-1)); - checkContour.push_back(cv::Point(proj_image_w_half+(roi_top_w/2),minYPolyRoi)); - checkContour.push_back(cv::Point(proj_image_w_half-(roi_top_w/2),minYPolyRoi)); - + checkContour.push_back(cv::Point(proj_image_w_half - (roiBottomW / 2), maxYRoi - 1)); + checkContour.push_back(cv::Point(proj_image_w_half + (roiBottomW / 2), maxYRoi - 1)); + checkContour.push_back(cv::Point(proj_image_w_half + (roiTopW / 2), minYPolyRoi)); + checkContour.push_back(cv::Point(proj_image_w_half - (roiTopW / 2), minYPolyRoi)); + int scanlineStart = 0; - int scanlineEnd = proj_image_w; + int scanlineEnd = projImageW; int segmentStart = -1; vector> scanline; //i = y; j = x; for (int i = 1; - (i/scanlinesVerticalDistance) < scanlinesMaxCount && i <= proj_image_h; - i += scanlinesVerticalDistance) { + (i / scanlinesVerticalDistance) < scanlinesMaxCount && i <= projImageH; + i += scanlinesVerticalDistance) { scanline = vector>(); // walk along line - for (int j = scanlineStart; j <= scanlineEnd; j ++) { - bool isInside = pointPolygonTest(checkContour, cv::Point(j, i),false) >= 0; - + for (int j = scanlineStart; j <= scanlineEnd; j++) { + bool isInside = pointPolygonTest(checkContour, cv::Point(j, i), false) >= 0; + // start new scanline segment if (isInside && j < scanlineEnd) { if (segmentStart == -1) segmentStart = j; - // found end of scanline segment, reset start + // found end of scanline segment, reset start } else if (segmentStart != -1) { scanline.push_back( LineSegment( FuPoint(segmentStart, i), - FuPoint(j-1, i) - ) - ); + FuPoint(j - 1, i) + ) + ); segmentStart = -1; } @@ -245,14 +284,14 @@ vector> > cLaneDetectionFu::getScanlines() { vector> cLaneDetectionFu::scanImage(cv::Mat image) { //ROS_INFO_STREAM("scanImage() - " << scanlines.size() << " scanlines."); vector> edgePoints; - + //const Image &image = getImage(); //const ImageDimensions &imgDim = getImageDimensions(); //const OmnidirectionalCameraMatrix &cameraMatrix = getOmnidirectionalCameraMatrix(); // scanline length can maximal be image height/width int scanlineMaxLength = image.cols; - + // store kernel results on current scanline in here vector scanlineVals(scanlineMaxLength, 0); @@ -270,38 +309,38 @@ vector> cLaneDetectionFu::scanImage(cv::Mat image) { for (auto segment : scanline) { int start = segment.getStart().getX(); int end = segment.getEnd().getX(); - + // walk along segment for (int i = start; i < end - g_kernel1DWidth; i++) { int sum = 0; //cv::Mat uses ROW-major system -> .at(y,x) // use kernel width 5 and try sobel kernel - sum -= image.at(offset-1, i); - sum -= image.at(offset-1, i+1); + sum -= image.at(offset - 1, i); + sum -= image.at(offset - 1, i + 1); // kernel is 0 - sum += image.at(offset-1, i+2); - sum += image.at(offset-1, i+4); + sum += image.at(offset - 1, i + 2); + sum += image.at(offset - 1, i + 4); - sum -= 2*image.at(offset, i); - sum -= 2*image.at(offset, i+1); + sum -= 2 * image.at(offset, i); + sum -= 2 * image.at(offset, i + 1); // kernel is 0 - sum += 2*image.at(offset, i+2); - sum += 2*image.at(offset, i+4); + sum += 2 * image.at(offset, i + 2); + sum += 2 * image.at(offset, i + 4); - sum -= image.at(offset+1, i); - sum -= image.at(offset+1, i+1); + sum -= image.at(offset + 1, i); + sum -= image.at(offset + 1, i + 1); // kernel is 0 - sum += image.at(offset+1, i+2); - sum += image.at(offset+1, i+4); + sum += image.at(offset + 1, i + 2); + sum += image.at(offset + 1, i + 4); // +4 because of sobel weighting sum = sum / (3 * g_kernel1DWidth + 4); //ROS_INFO_STREAM(sum << " is kernel sum."); - if (std::abs(sum) > m_gradientThreshold) { + if (std::abs(sum) > gradientThreshold) { // set scanlineVals at center of kernel - scanlineVals[i + g_kernel1DWidth/2] = sum; + scanlineVals[i + g_kernel1DWidth / 2] = sum; } } } @@ -314,28 +353,25 @@ vector> cLaneDetectionFu::scanImage(cv::Mat image) { int valueOfLastMaximum = 0; int indexOfLastMinimum = 0; int valueOfLastMinimum = 0; - for (int i = 1; i < scanlineMaxLength -1; i++) { + for (int i = 1; i < scanlineMaxLength - 1; i++) { // check if maximum if (scanlineVals[i] > 0) { - if (scanlineVals[i] < scanlineVals[i-1] or scanlineVals[i] < scanlineVals[i+1]) { + if (scanlineVals[i] < scanlineVals[i - 1] or scanlineVals[i] < scanlineVals[i + 1]) { scanlineVals[i] = 0; - } - else { + } else { // this pixel can just survive if the next maximum is not too close - if (i - indexOfLastMaximum > m_nonMaxWidth) { + if (i - indexOfLastMaximum > nonMaxWidth) { // this is a new maximum indexOfLastMaximum = i; valueOfLastMaximum = scanlineVals[i]; - } - else { + } else { if (valueOfLastMaximum < scanlineVals[i]) { // this is a new maximum // drop the old maximum scanlineVals[indexOfLastMaximum] = 0; indexOfLastMaximum = i; valueOfLastMaximum = scanlineVals[i]; - } - else { + } else { scanlineVals[i] = 0; } } @@ -343,25 +379,22 @@ vector> cLaneDetectionFu::scanImage(cv::Mat image) { } // check if minimum if (scanlineVals[i] < 0) { - if (scanlineVals[i] > scanlineVals[i-1] or scanlineVals[i] > scanlineVals[i+1]) { + if (scanlineVals[i] > scanlineVals[i - 1] or scanlineVals[i] > scanlineVals[i + 1]) { scanlineVals[i] = 0; - } - else { + } else { // this pixel can just survive if the next minimum is not too close - if (i - indexOfLastMinimum > m_nonMaxWidth) { + if (i - indexOfLastMinimum > nonMaxWidth) { // this is a new minimum indexOfLastMinimum = i; valueOfLastMinimum = scanlineVals[i]; - } - else { + } else { if (valueOfLastMinimum > scanlineVals[i]) { // this is a new maximum // drop the old maximum scanlineVals[indexOfLastMinimum] = 0; indexOfLastMinimum = i; valueOfLastMinimum = scanlineVals[i]; - } - else { + } else { scanlineVals[i] = 0; } } @@ -373,7 +406,7 @@ vector> cLaneDetectionFu::scanImage(cv::Mat image) { for (int i = 0; i < static_cast(scanlineVals.size()); i++) { if (scanlineVals[i] != 0) { FuPoint imgPos = FuPoint(i, offset); - + FuPoint relPos = FuPoint();//offset, i);//cameraMatrix.transformToLocalCoordinates(imgPos); scanlineEdgePoints.push_back(EdgePoint(imgPos, relPos, scanlineVals[i])); } @@ -386,21 +419,20 @@ vector> cLaneDetectionFu::scanImage(cv::Mat image) { } -/* LaneMarkingDetector methods */ - - -//uses Edges to extract lane markings -std::vector> cLaneDetectionFu::extractLaneMarkings(const std::vector>& edges) { +/** + * uses Edges to extract lane markings + */ +std::vector> cLaneDetectionFu::extractLaneMarkings(const std::vector> &edges) { vector> result; - for (const auto& line : edges) { + for (const auto &line : edges) { if (line.empty()) continue; - + for ( - auto edgePosition = line.begin(), nextEdgePosition = edgePosition + 1; - nextEdgePosition != line.end(); - edgePosition = nextEdgePosition, ++nextEdgePosition - ) { + auto edgePosition = line.begin(), nextEdgePosition = edgePosition + 1; + nextEdgePosition != line.end(); + edgePosition = nextEdgePosition, ++nextEdgePosition + ) { if (edgePosition->isPositive() and not nextEdgePosition->isPositive()) { FuPoint candidateStartEdge = edgePosition->getImgPos(); FuPoint candidateEndEdge = nextEdgePosition->getImgPos(); @@ -442,18 +474,18 @@ void cLaneDetectionFu::findLanePositions(vector> &laneMarkings) { vector rightSupporter; // possible start points of center lane - vector*> centerStart; - vector*> rightStart; + vector *> centerStart; + vector *> rightStart; for (int j = 0; j < laneMarkings.size(); j++) { - FuPoint* laneMarking = &laneMarkings.at(j); + FuPoint *laneMarking = &laneMarkings.at(j); if (laneMarking->getY() > maxYRoi) { continue; } bool isSupporter = false; - if (laneMarking->getX() < proj_image_w_half + proj_image_horizontal_offset) { + if (laneMarking->getX() < proj_image_w_half + projImageHorizontalOffset) { for (int i = 0; i < centerStart.size(); i++) { if (isInRange(*centerStart.at(i), *laneMarking)) { isSupporter = true; @@ -492,7 +524,7 @@ void cLaneDetectionFu::findLanePositions(vector> &laneMarkings) { } } -defaultLineCalculation: + defaultLineCalculation: // use x-value of lane marking point with most (and at least 3) supporters if (centerStart.size() > 0) { @@ -529,7 +561,7 @@ void cLaneDetectionFu::findLanePositions(vector> &laneMarkings) { * * @param laneMarkings a vector containing all detected lane markings */ -void cLaneDetectionFu::buildLaneMarkingsLists( const std::vector> &laneMarkings) { +void cLaneDetectionFu::buildLaneMarkingsLists(const std::vector> &laneMarkings) { laneMarkingsLeft.clear(); laneMarkingsCenter.clear(); laneMarkingsRight.clear(); @@ -632,36 +664,188 @@ void cLaneDetectionFu::buildLaneMarkingsLists( const std::vector> & } } -void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, FuPoint &origin) { - shiftPoint(p, m, offset, origin.getX(), origin.getY()); +/** + * Starts the RANSAC algorithm for detecting each of the three lane marking + * polynomials. + */ +void cLaneDetectionFu::ransac() { + polyDetectedLeft = ransacInternal(LEFT, laneMarkingsLeft, bestPolyLeft, + polyLeft, supportersLeft, prevPolyLeft, pointsLeft); + + polyDetectedCenter = ransacInternal(CENTER, laneMarkingsCenter, + bestPolyCenter, polyCenter, supportersCenter, prevPolyCenter, + pointsCenter); + + polyDetectedRight = ransacInternal(RIGHT, laneMarkingsRight, bestPolyRight, + polyRight, supportersRight, prevPolyRight, pointsRight); } /** + * Detects a polynomial with RANSAC in a given list of lane marking edge points. * - * @param p - * @param m - * @param offset Positive if shifting to the left, negative to the right - * @param y - * @param x + * @param position The position of the wanted polynomial + * @param laneMarkings A reference to the list of lane marking edge points + * @param bestPoly A reference to a pair containing the present best + * detected polynomial and a value representing the fitting + * quality called proportion + * @param poly A reference to the polynomial that gets detected + * @param supporters A reference to the supporter points of the present best + * polynomial + * @param prevPoly A reference to the previous polynomial detected at this + * position + * @param points A reference to the points selected for interpolating the + * present best polynomial + * @return true if a polynomial could be detected and false when not */ -void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, int x, int y) { - /* - * Depending on the sign of the gradient of the poly at the different - * x-values and depending on which position we are, we have to add or - * subtract the expected distance to the respective lane polynomial, to get - * the wanted points. - * - * The calculation is done for the x- and y-components of the points - * separately using the trigonometric ratios of right triangles and the fact - * that arctan of some gradient equals its angle to the x-axis in degree. - */ - if (m >= 0) { - p.setX(x - offset * sin(atan(-1 / m))); - p.setY(y - offset * cos(atan(-1 / m))); - return; +bool cLaneDetectionFu::ransacInternal(ePosition position, + std::vector> &laneMarkings, + std::pair &bestPoly, NewtonPolynomial &poly, + std::vector> &supporters, NewtonPolynomial &prevPoly, + std::vector> &points) { + + if (laneMarkings.size() < 7) { + prevPoly = poly; + poly.clear(); + return false; } - p.setX(x + offset * sin(atan(-1 / m))); - p.setY(y + offset * cos(atan(-1 / m))); + + std::vector> tmpSupporters = std::vector>(); + + // vectors for points selected from the bottom, mid and top of the sorted + // point vector + std::vector> markings1 = std::vector>(); + std::vector> markings2 = std::vector>(); + std::vector> markings3 = std::vector>(); + + bool highEnoughY = false; + + // Points are selected from the bottom, mid and top. The selection regions + // are spread apart for better results during RANSAC + for (std::vector>::size_type i = 0; i != laneMarkings.size(); i++) { + if (i < double(laneMarkings.size()) / 7) { + markings1.push_back(laneMarkings[i]); + } else if (i >= (double(laneMarkings.size()) / 7) * 3 + && i < (double(laneMarkings.size()) / 7) * 4) { + markings2.push_back(laneMarkings[i]); + } else if (i >= (double(laneMarkings.size()) / 7) * 6) { + markings3.push_back(laneMarkings[i]); + } + + if (laneMarkings[i].getY() > 5) { + highEnoughY = true; + } + } + + //what is this for? + if (position == CENTER) { + if (!highEnoughY) { + prevPoly = poly; + poly.clear(); + return false; + } + } + + // save the polynomial from the previous picture + prevPoly = poly; + + for (int i = 0; i < iterationsRansac; i++) { + + // randomly select 3 different lane marking points from bottom, mid and + // top + int pos1 = rand() % markings1.size(); + int pos2 = rand() % markings2.size(); + int pos3 = rand() % markings3.size(); + + FuPoint p1 = markings1[pos1]; + FuPoint p2 = markings2[pos2]; + FuPoint p3 = markings3[pos3]; + + double p1X = p1.getX(); + double p1Y = p1.getY(); + double p2X = p2.getX(); + double p2Y = p2.getY(); + double p3X = p3.getX(); + double p3Y = p3.getY(); + + // clear poly for reuse + poly.clear(); + + // create a polynomial with the selected points + poly.addData(p1X, p1Y); + poly.addData(p2X, p2Y); + poly.addData(p3X, p3Y); + + // check if this polynomial is not useful + if (!polyValid(position, poly, prevPoly)) { + poly.clear(); + continue; + } + + // count the supporters and save them for debugging + int count1 = 0; + int count2 = 0; + int count3 = 0; + + // find the supporters + tmpSupporters.clear(); + + for (FuPoint p : markings1) { + if (horizDistanceToPolynomial(poly, p) <= maxDistance) { + count1++; + tmpSupporters.push_back(p); + } + } + + for (FuPoint p : markings2) { + if (horizDistanceToPolynomial(poly, p) <= maxDistance) { + count2++; + tmpSupporters.push_back(p); + } + } + + for (FuPoint p : markings3) { + if (horizDistanceToPolynomial(poly, p) <= maxDistance) { + count3++; + tmpSupporters.push_back(p); + } + } + + if (count1 == 0 || count2 == 0 || count3 == 0) { + poly.clear(); + //DEBUG_TEXT(dbgMessages, "Poly had no supporters in one of the regions"); + continue; + } + + // calculate the proportion of supporters of all lane markings + double proportion = (double(count1) / markings1.size() + + double(count2) / markings2.size() + + 3 * (double(count3) / markings3.size())) / 5; + + if (proportion < proportionThreshould) { + poly.clear(); + //DEBUG_TEXT(dbgMessages, "Poly proportion was smaller than threshold"); + continue; + } + + // check if poly is better than bestPoly + if (proportion > bestPoly.second) { + bestPoly = std::make_pair(poly, proportion); + supporters = tmpSupporters; + + points.clear(); + points.push_back(p1); + points.push_back(p2); + points.push_back(p3); + } + } + + poly = bestPoly.first; + + if (poly.getDegree() == -1) { + return false; + } + + return true; } void cLaneDetectionFu::generateMovedPolynomials() { @@ -715,19 +899,18 @@ void cLaneDetectionFu::generateMovedPolynomials() { isPolyMovedCenter = true; - shiftPoint(pointCenter1,m1, -laneWidth, pointsRight.at(0)); - shiftPoint(pointCenter2,m2, -laneWidth, pointsRight.at(1)); - shiftPoint(pointCenter3,m3, -laneWidth, pointsRight.at(2)); + shiftPoint(pointCenter1, m1, -laneWidth, pointsRight.at(0)); + shiftPoint(pointCenter2, m2, -laneWidth, pointsRight.at(1)); + shiftPoint(pointCenter3, m3, -laneWidth, pointsRight.at(2)); if (!polyDetectedLeft) { isPolyMovedLeft = true; - shiftPoint(pointLeft1,m1, -2 * laneWidth, pointsRight.at(0)); - shiftPoint(pointLeft2,m2, -2 * laneWidth, pointsRight.at(1)); - shiftPoint(pointLeft3,m3, -2 * laneWidth, pointsRight.at(2)); + shiftPoint(pointLeft1, m1, -2 * laneWidth, pointsRight.at(0)); + shiftPoint(pointLeft2, m2, -2 * laneWidth, pointsRight.at(1)); + shiftPoint(pointLeft3, m3, -2 * laneWidth, pointsRight.at(2)); } - } - else if (polyDetectedLeft && !polyDetectedCenter) { + } else if (polyDetectedLeft && !polyDetectedCenter) { usedPoly = polyLeft; m1 = gradient(pointsLeft.at(0).getY(), pointsLeft, usedPoly.getCoefficients()); m2 = gradient(pointsLeft.at(1).getY(), pointsLeft, usedPoly.getCoefficients()); @@ -735,19 +918,18 @@ void cLaneDetectionFu::generateMovedPolynomials() { isPolyMovedCenter = true; - shiftPoint(pointCenter1,m1, laneWidth, pointsLeft.at(0)); - shiftPoint(pointCenter2,m2, laneWidth, pointsLeft.at(1)); - shiftPoint(pointCenter3,m3, laneWidth, pointsLeft.at(2)); + shiftPoint(pointCenter1, m1, laneWidth, pointsLeft.at(0)); + shiftPoint(pointCenter2, m2, laneWidth, pointsLeft.at(1)); + shiftPoint(pointCenter3, m3, laneWidth, pointsLeft.at(2)); if (!polyDetectedRight) { isPolyMovedRight = true; - shiftPoint(pointRight1,m1, 2 * laneWidth, pointsLeft.at(0)); - shiftPoint(pointRight2,m2, 2 * laneWidth, pointsLeft.at(1)); - shiftPoint(pointRight3,m3, 2 * laneWidth, pointsLeft.at(2)); + shiftPoint(pointRight1, m1, 2 * laneWidth, pointsLeft.at(0)); + shiftPoint(pointRight2, m2, 2 * laneWidth, pointsLeft.at(1)); + shiftPoint(pointRight3, m3, 2 * laneWidth, pointsLeft.at(2)); } - } - else if (polyDetectedCenter) { + } else if (polyDetectedCenter) { usedPoly = polyCenter; m1 = gradient(pointsCenter.at(0).getY(), pointsCenter, usedPoly.getCoefficients()); m2 = gradient(pointsCenter.at(1).getY(), pointsCenter, usedPoly.getCoefficients()); @@ -756,17 +938,17 @@ void cLaneDetectionFu::generateMovedPolynomials() { if (!polyDetectedLeft) { isPolyMovedLeft = true; - shiftPoint(pointLeft1,m1, -laneWidth, pointsCenter.at(0)); - shiftPoint(pointLeft2,m2, -laneWidth, pointsCenter.at(1)); - shiftPoint(pointLeft3,m3, -laneWidth, pointsCenter.at(2)); + shiftPoint(pointLeft1, m1, -laneWidth, pointsCenter.at(0)); + shiftPoint(pointLeft2, m2, -laneWidth, pointsCenter.at(1)); + shiftPoint(pointLeft3, m3, -laneWidth, pointsCenter.at(2)); } if (!polyDetectedRight) { isPolyMovedRight = true; - shiftPoint(pointRight1,m1, laneWidth, pointsCenter.at(0)); - shiftPoint(pointRight2,m2, laneWidth, pointsCenter.at(1)); - shiftPoint(pointRight3,m3, laneWidth, pointsCenter.at(2)); + shiftPoint(pointRight1, m1, laneWidth, pointsCenter.at(0)); + shiftPoint(pointRight2, m2, laneWidth, pointsCenter.at(1)); + shiftPoint(pointRight3, m3, laneWidth, pointsCenter.at(2)); } } @@ -795,22 +977,110 @@ void cLaneDetectionFu::generateMovedPolynomials() { } } -bool cLaneDetectionFu::isInRange(FuPoint &lanePoint, FuPoint &p) { - if (p.getY() < minYDefaultRoi || p.getY() > maxYRoi) { - return false; +void cLaneDetectionFu::pubAngle() { + if (!polyDetectedRight && !isPolyMovedRight) { + return; } - if (p.getY() > lanePoint.getY()) { - return false; + + int y = projImageH - angleAdjacentLeg; + double xRightLane; + double m; + + if (polyDetectedRight) { + xRightLane = polyRight.at(y); + m = gradient(y, pointsRight, polyRight.getCoefficients()); + } else { + xRightLane = movedPolyRight.at(y); + m = gradient(y, movedPointsRight, movedPolyRight.getCoefficients()); } - double distanceX = std::abs(p.getX() - lanePoint.getX()); - double distanceY = std::abs(p.getY() - lanePoint.getY()); + double offset = -1 * laneWidth / 2; + shiftPoint(movedPointForAngle, m, offset, (int) xRightLane, y); - return distanceX < interestDistancePoly && distanceY < interestDistancePoly; -} + pointForAngle.setX(xRightLane); + pointForAngle.setY(y); + gradientForAngle = m; -/** + oppositeLeg = movedPointForAngle.getX() - proj_image_w_half; + adjacentLeg = projImageH - movedPointForAngle.getY(); + double result = atan(oppositeLeg / adjacentLeg) * 180 / PI; + + /* + * filter too rash steering angles / jitter in polynomial data + */ + if (std::abs(result - lastAngle) > maxAngleDiff) { + if (result - lastAngle > 0) + result = lastAngle + maxAngleDiff; + else + result = lastAngle - maxAngleDiff; + } + + lastAngle = result; + + std_msgs::Float32 angleMsg; + + angleMsg.data = result; + + publish_angle.publish(angleMsg); +} + + + +/* + * Utils: + */ + + + +void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, FuPoint &origin) { + shiftPoint(p, m, offset, origin.getX(), origin.getY()); +} + +/** + * + * @param p + * @param m + * @param offset Positive if shifting to the left, negative to the right + * @param y + * @param x + */ +void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, int x, int y) { + /* + * Depending on the sign of the gradient of the poly at the different + * x-values and depending on which position we are, we have to add or + * subtract the expected distance to the respective lane polynomial, to get + * the wanted points. + * + * The calculation is done for the x- and y-components of the points + * separately using the trigonometric ratios of right triangles and the fact + * that arctan of some gradient equals its angle to the x-axis in degree. + */ + if (m >= 0) { + p.setX(x - offset * sin(atan(-1 / m))); + p.setY(y - offset * cos(atan(-1 / m))); + return; + } + p.setX(x + offset * sin(atan(-1 / m))); + p.setY(y + offset * cos(atan(-1 / m))); +} + +bool cLaneDetectionFu::isInRange(FuPoint &lanePoint, FuPoint &p) { + if (p.getY() < minYDefaultRoi || p.getY() > maxYRoi) { + return false; + } + if (p.getY() > lanePoint.getY()) { + return false; + } + + double distanceX = std::abs(p.getX() - lanePoint.getX()); + double distanceY = std::abs(p.getY() - lanePoint.getY()); + + return distanceX < interestDistancePoly && distanceY < interestDistancePoly; +} + + +/** * Calculates the horizontal distance between a point and the default line given * by its position. * @@ -823,15 +1093,15 @@ int cLaneDetectionFu::horizDistanceToDefaultLine(ePosition &line, FuPoint & double distance = 0; switch (line) { - case LEFT: - distance = std::abs(pX - defaultXLeft); - break; - case CENTER: - distance = std::abs(pX - defaultXCenter); - break; - case RIGHT: - distance = std::abs(pX - defaultXRight); - break; + case LEFT: + distance = std::abs(pX - defaultXLeft); + break; + case CENTER: + distance = std::abs(pX - defaultXCenter); + break; + case RIGHT: + distance = std::abs(pX - defaultXRight); + break; } return distance; @@ -844,7 +1114,7 @@ int cLaneDetectionFu::horizDistanceToDefaultLine(ePosition &line, FuPoint & * @param p The given point * @return The horizontal distance between the polynomial and the point, horizontal distance = difference in X!!! */ -int cLaneDetectionFu::horizDistanceToPolynomial(NewtonPolynomial& poly, FuPoint &p) { +int cLaneDetectionFu::horizDistanceToPolynomial(NewtonPolynomial &poly, FuPoint &p) { double pY = p.getY(); double pX = p.getX(); @@ -919,432 +1189,378 @@ int cLaneDetectionFu::horizDistance(FuPoint &p1, FuPoint &p2) { */ double cLaneDetectionFu::gradient(double x, vector> &points, vector coeffs) { return (2 * coeffs[2] * x + coeffs[1]) - - (coeffs[2] * points[1].getY()) - - (coeffs[2] * points[0].getY()); + - (coeffs[2] * points[1].getY()) + - (coeffs[2] * points[0].getY()); } /** - * Calculates the x value of the point where the normal of the tangent of a - * polynomial at a given point p intersects with a second polynomial. - * - * The formula for the intersection point is obtained by setting equal the - * following two formula: - * - * 1. the formula of the normal in point-slope-form: - * y - p_y = -(1 / m) * (x - p_x) which is the same as - * y = -(x / m) + (p_x / m) + p_y - * - * 2. the formula of the second polynomial of 2nd degree in newton basis form: - * y = c0 + c1(x - x0) + c2(x - x0)(x - x1) - * - * Expanding everything and moving it to the right side gives a quadratic - * equation in the general form of 0 = ax^2 + bx + c, which can be solved using - * the general quadratic formula x = (-b +- sqrt(b^2 - 4ac)) / 2a - * - * The three cases for the discriminant are taken into account. + * Method, that checks, if a polynomial produced during RANSAC counts as usable. * - * @param p The point of the first poly at which its tangent is used - * @param m The gradient of the tangent - * @param points The data points used for interpolating the second polynomial - * @param coeffs The coeffs of the second polynomial with newton basis - * @return The x value of the intersection point of normal and 2nd poly + * @param position The position of the polynomial, that is checked + * @param poly The polynomial, that is checked + * @param prevPoly The previous polynomial detected at this position + * @return True, if the polynomial counts as valid */ -double cLaneDetectionFu::intersection(FuPoint &p, double &m, - std::vector> &points, std::vector &coeffs) { - double a = coeffs[2]; - double b = coeffs[1] - (coeffs[2] * points[1].getY()) - - (coeffs[2] * points[0].getY()) + (1.0 / m); - double c = coeffs[0] - (coeffs[1] * points[0].getY()) - + (coeffs[2] * points[0].getY() * points[1].getY()) - - p.getY() - (p.getX() / m); - - double dis = std::pow(b, 2) - (4 * a * c); - double x1 = 0; - double x2 = 0; +bool cLaneDetectionFu::polyValid(ePosition position, NewtonPolynomial poly, NewtonPolynomial prevPoly) { - if (dis < 0) { - return -1; + if (prevPoly.getDegree() != -1) { + return isSimilar(poly, prevPoly); } - else if (dis == 0) { - return -b / (2 * a); + if (position == RIGHT) { + if (polyDetectedRight) { + return isSimilar(poly, polyRight); + } + if (isPolyMovedRight) { + return isSimilar(poly, movedPolyRight); + } } - else { - x1 = (-b + std::sqrt(std::pow(b, 2) - (4 * a * c))) / (2 * a); - x2 = (-b - std::sqrt(std::pow(b, 2) - (4 * a * c))) / (2 * a); + if (position == CENTER) { + if (polyDetectedCenter) { + return isSimilar(poly, polyCenter); + } + if (isPolyMovedCenter) { + return isSimilar(poly, movedPolyCenter); + } + } + if (position == LEFT) { + if (polyDetectedLeft) { + return isSimilar(poly, polyLeft); + } + if (isPolyMovedLeft) { + return isSimilar(poly, movedPolyLeft); + } } - return fmax(x1, x2); + return true; } -/** - * Calculates the gradient of a second polynomial at the point, at which the - * normal of the tangent of the first polynomial at the given point - * intersects with the second polynomial. - * - * @param x The given x value of the point on the first polynomial - * @param poly1 The first polynomial - * @param points1 The data points used for interpolating the first poly - * @param points2 The data points used for interpolating the second poly - * @param coeffs1 The coeffs of the first poly using newton basis - * @param coeffs2 The coeffs of the second poly using newton basis - * @param m1 The gradient of the first poly at x - * @return The gradient of the second poly at the intersection point - */ -double cLaneDetectionFu::nextGradient(double x, NewtonPolynomial &poly1, - std::vector> &points1, std::vector> &points2, - std::vector coeffs1, std::vector coeffs2, double m1) { - - FuPoint p = FuPoint(x, poly1.at(x)); - double x2 = intersection(p, m1, points2, coeffs2); +bool cLaneDetectionFu::isSimilar(const NewtonPolynomial &poly1, const NewtonPolynomial &poly2) { + FuPoint p1 = FuPoint(poly1.at(polyY1), polyY1); + FuPoint p2 = FuPoint(poly2.at(polyY1), polyY1); - return gradient(x2, points2, coeffs2); -} + if (horizDistance(p1, p2) > interestDistancePoly) { //0.05 * meters + return false; + } -/** - * Check two gradients for similarity. Return true if the difference in degree - * is less than 10. - * - * @param m1 The first gradient - * @param m2 The second gradient - * @return True, if the diffenence between the gradients is less than 10° - */ -bool cLaneDetectionFu::gradientsSimilar(double &m1, double &m2) { - double a1 = atan(m1) * 180 / PI; - double a2 = atan(m2) * 180 / PI; + FuPoint p3 = FuPoint(poly1.at(polyY2), polyY2); + FuPoint p4 = FuPoint(poly2.at(polyY2), polyY2); - if (abs(a1 - a2) < 10) { - return true; + if (horizDistance(p3, p4) > interestDistancePoly) { //0.05 * meters + return false; } - else { + + FuPoint p5 = FuPoint(poly1.at(polyY3), polyY3); + FuPoint p6 = FuPoint(poly2.at(polyY3), polyY3); + + if (horizDistance(p5, p6) > interestDistancePoly) { //0.05 * meters return false; } + + return true; } -/** - * Finds the position of the polynomial with the highest proportion. - * @return The position of the best polynomial + + +/* + * debug functions */ -ePosition cLaneDetectionFu::maxProportion() { - ePosition maxPos = LEFT; - double maxVal = bestPolyLeft.second; - if (bestPolyCenter.second > maxVal) { - maxPos = CENTER; - maxVal = bestPolyCenter.second; + + +void cLaneDetectionFu::drawEdgeWindow(Mat &img, vector> edges) { + if (!SHOW_EDGE_WINDOW) { + return; } - if (bestPolyRight.second > maxVal) { - maxPos = RIGHT; + Mat transformedImagePaintable = img.clone(); + cvtColor(transformedImagePaintable, transformedImagePaintable, CV_GRAY2BGR); + for (int i = 0; i < (int) edges.size(); i++) { + for (int j = 0; j < edges[i].size(); j++) { + FuPoint edge = edges[i][j].getImgPos(); + Point edgeLoc = Point(edge.getX(), edge.getY()); + circle(transformedImagePaintable, edgeLoc, 1, Scalar(0, 0, edges[i][j].getValue()), -1); + } } - return maxPos; + /*cv::Point2d p1(projImageHalfW-(roiBottomW/2),maxYRoi-1); + cv::Point2d p2(projImageHalfW+(roiBottomW/2),maxYRoi-1); + cv::Point2d p3(projImageHalfW+(roiTopW/2),minYPolyRoi); + cv::Point2d p4(projImageHalfW-(roiTopW/2),minYPolyRoi); + cv::line(transformedImagePaintable,p1,p2,cv::Scalar(0,200,0)); + cv::line(transformedImagePaintable,p2,p3,cv::Scalar(0,200,0)); + cv::line(transformedImagePaintable,p3,p4,cv::Scalar(0,200,0)); + cv::line(transformedImagePaintable,p4,p1,cv::Scalar(0,200,0));*/ + + /*for(int i = 0; i < (int)scanlines.size(); i++) + { + LineSegment scanline = scanlines[i][0]; + cv::Point scanlineStart = cv::Point(scanline.getStart().getX(), scanline.getStart().getY()); + cv::Point scanlineEnd = cv::Point(scanline.getEnd().getX(), scanline.getEnd().getY()); + cv::line(transformedImagePaintable,scanlineStart,scanlineEnd,cv::Scalar(255,0,0)); + }*/ + + + cv::namedWindow("ROI, scanlines and edges", WINDOW_NORMAL); + cv::imshow("ROI, scanlines and edges", transformedImagePaintable); + cv::waitKey(1); } -/** - * Starts the RANSAC algorithm for detecting each of the three lane marking - * polynomials. - */ -void cLaneDetectionFu::ransac() { - polyDetectedLeft = ransacInternal(LEFT, laneMarkingsLeft, bestPolyLeft, - polyLeft, supportersLeft, prevPolyLeft, pointsLeft); +void cLaneDetectionFu::drawLaneMarkingsWindow(Mat &img, vector> &laneMarkings) { + if (!SHOW_LANE_MARKINGS_WINDOW) { + return; + } - polyDetectedCenter = ransacInternal(CENTER, laneMarkingsCenter, - bestPolyCenter, polyCenter, supportersCenter, prevPolyCenter, - pointsCenter); + Mat transformedImagePaintable = img.clone(); + cv::cvtColor(transformedImagePaintable, transformedImagePaintable, CV_GRAY2BGR); + for (int i = 0; i < (int) laneMarkings.size(); i++) { + FuPoint marking = laneMarkings[i]; + cv::Point markingLoc = cv::Point(marking.getX(), marking.getY()); + cv::circle(transformedImagePaintable, markingLoc, 1, cv::Scalar(0, 255, 0), -1); + } - polyDetectedRight = ransacInternal(RIGHT, laneMarkingsRight, bestPolyRight, - polyRight, supportersRight, prevPolyRight, pointsRight); + cv::namedWindow("Lane Markings", WINDOW_NORMAL); + cv::imshow("Lane Markings", transformedImagePaintable); + cv::waitKey(1); } -/** - * Detects a polynomial with RANSAC in a given list of lane marking edge points. - * - * @param position The position of the wanted polynomial - * @param laneMarkings A reference to the list of lane marking edge points - * @param bestPoly A reference to a pair containing the present best - * detected polynomial and a value representing the fitting - * quality called proportion - * @param poly A reference to the polynomial that gets detected - * @param supporters A reference to the supporter points of the present best - * polynomial - * @param prevPoly A reference to the previous polynomial detected at this - * position - * @param points A reference to the points selected for interpolating the - * present best polynomial - * @return true if a polynomial could be detected and false when not - */ -bool cLaneDetectionFu::ransacInternal(ePosition position, - std::vector>& laneMarkings, - std::pair& bestPoly, NewtonPolynomial& poly, - std::vector>& supporters, NewtonPolynomial& prevPoly, - std::vector>& points) { - - if (laneMarkings.size() < 7) { - prevPoly = poly; - poly.clear(); - return false; +void cLaneDetectionFu::drawGroupedLaneMarkingsWindow(Mat &img) { + if (!PUBLISH_IMAGES && !SHOW_GROUPED_LANE_MARKINGS_WINDOW && !SAVE_FRAME_IMAGES) { + return; } - std::vector> tmpSupporters = std::vector>(); + Mat transformedImagePaintable = img.clone(); + cv::cvtColor(transformedImagePaintable, transformedImagePaintable, CV_GRAY2BGR); + + debugPaintPoints(transformedImagePaintable, Scalar(0, 0, 255), laneMarkingsLeft); + debugPaintPoints(transformedImagePaintable, Scalar(0, 255, 0), laneMarkingsCenter); + debugPaintPoints(transformedImagePaintable, Scalar(255, 0, 0), laneMarkingsRight); + debugPaintPoints(transformedImagePaintable, Scalar(0, 255, 255), laneMarkingsNotUsed); + + debugPaintPolynom(transformedImagePaintable, cv::Scalar(0, 255, 255), polyDetectedLeft ? polyLeft : movedPolyLeft, + minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintable, cv::Scalar(255, 255, 0), + polyDetectedCenter ? polyCenter : movedPolyCenter, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintable, cv::Scalar(255, 0, 255), + polyDetectedRight ? polyRight : movedPolyRight, minYPolyRoi, maxYRoi); + + cv::Point2d p1l(defaultXLeft, minYPolyRoi); + cv::Point2d p2l(defaultXLeft, maxYRoi - 1); + cv::line(transformedImagePaintable, p1l, p2l, cv::Scalar(0, 0, 255)); + + cv::Point2d p1c(defaultXCenter, minYPolyRoi); + cv::Point2d p2c(defaultXCenter, maxYRoi - 1); + cv::line(transformedImagePaintable, p1c, p2c, cv::Scalar(0, 255, 0)); + + cv::Point2d p1r(defaultXRight, minYPolyRoi); + cv::Point2d p2r(defaultXRight, maxYRoi - 1); + cv::line(transformedImagePaintable, p1r, p2r, cv::Scalar(255, 0, 0)); + + cv::Point2d p1(proj_image_w_half - (roiBottomW / 2), maxYRoi - 1); + cv::Point2d p2(proj_image_w_half + (roiBottomW / 2), maxYRoi - 1); + cv::Point2d p3(proj_image_w_half + (roiTopW / 2), minYPolyRoi); + cv::Point2d p4(proj_image_w_half - (roiTopW / 2), minYPolyRoi); + cv::line(transformedImagePaintable, p1, p2, cv::Scalar(0, 200, 0)); + cv::line(transformedImagePaintable, p2, p3, cv::Scalar(0, 200, 0)); + cv::line(transformedImagePaintable, p3, p4, cv::Scalar(0, 200, 0)); + cv::line(transformedImagePaintable, p4, p1, cv::Scalar(0, 200, 0)); + + if (PUBLISH_IMAGES) { + pubRGBImageMsg(transformedImagePaintable, image_publisher_lane_markings); + } - // vectors for points selected from the bottom, mid and top of the sorted - // point vector - std::vector> markings1 = std::vector>(); - std::vector> markings2 = std::vector>(); - std::vector> markings3 = std::vector>(); - bool highEnoughY = false; + if (SHOW_GROUPED_LANE_MARKINGS_WINDOW) { + cv::namedWindow("Grouped Lane Markings", WINDOW_NORMAL); + cv::imshow("Grouped Lane Markings", transformedImagePaintable); + cv::waitKey(1); + } - // Points are selected from the bottom, mid and top. The selection regions - // are spread apart for better results during RANSAC - for (std::vector>::size_type i = 0; i != laneMarkings.size(); i++) { - if (i < double(laneMarkings.size()) / 7) { - markings1.push_back(laneMarkings[i]); - } - else if (i >= (double(laneMarkings.size()) / 7) * 3 - && i < (double(laneMarkings.size()) / 7) * 4) { - markings2.push_back(laneMarkings[i]); - } - else if (i >= (double(laneMarkings.size()) / 7) * 6) { - markings3.push_back(laneMarkings[i]); - } - - if (laneMarkings[i].getY() > 5) { - highEnoughY = true; - } - } - - //what is this for? - if (position == CENTER) { - if (!highEnoughY) { - prevPoly = poly; - poly.clear(); - return false; - } + if (SAVE_FRAME_IMAGES) { + debugWriteImg(transformedImagePaintable, "groupedLaneMarkings"); } +} - // save the polynomial from the previous picture - prevPoly = poly; - - for (int i = 0; i < iterationsRansac; i++) { +void cLaneDetectionFu::drawRansacWindow(cv::Mat &img) { - // randomly select 3 different lane marking points from bottom, mid and - // top - int pos1 = rand() % markings1.size(); - int pos2 = rand() % markings2.size(); - int pos3 = rand() % markings3.size(); + if (!PUBLISH_IMAGES && !SHOW_RANSAC_WINDOW && !SAVE_FRAME_IMAGES) { + return; + } - FuPoint p1 = markings1[pos1]; - FuPoint p2 = markings2[pos2]; - FuPoint p3 = markings3[pos3]; + cv::Mat transformedImagePaintableRansac = img.clone(); + cv::cvtColor(transformedImagePaintableRansac, transformedImagePaintableRansac, CV_GRAY2BGR); - double p1X = p1.getX(); - double p1Y = p1.getY(); - double p2X = p2.getX(); - double p2Y = p2.getY(); - double p3X = p3.getX(); - double p3Y = p3.getY(); + debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(0, 0, 255), polyLeft, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(0, 255, 0), polyCenter, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(255, 0, 0), polyRight, minYPolyRoi, maxYRoi); - // clear poly for reuse - poly.clear(); + debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(139, 0, 139), movedPolyLeft, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(0, 0, 0), movedPolyCenter, minYPolyRoi, maxYRoi); + debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(255, 120, 0), movedPolyRight, minYPolyRoi, maxYRoi); - // create a polynomial with the selected points - poly.addData(p1X, p1Y); - poly.addData(p2X, p2Y); - poly.addData(p3X, p3Y); - // check if this polynomial is not useful - if (!polyValid(position, poly, prevPoly)) { - poly.clear(); - continue; - } + if (PUBLISH_IMAGES) { + pubRGBImageMsg(transformedImagePaintableRansac, image_publisher_ransac); + } - // count the supporters and save them for debugging - int count1 = 0; - int count2 = 0; - int count3 = 0; + if (SHOW_RANSAC_WINDOW) { + cv::namedWindow("RANSAC results", WINDOW_NORMAL); + cv::imshow("RANSAC results", transformedImagePaintableRansac); + cv::waitKey(1); + } - // find the supporters - tmpSupporters.clear(); + if (SAVE_FRAME_IMAGES) { + debugWriteImg(transformedImagePaintableRansac, "ransac"); + } +} - for (FuPoint p : markings1) { - if (horizDistanceToPolynomial(poly, p) <= maxDistance) { - count1++; - tmpSupporters.push_back(p); - } - } +void cLaneDetectionFu::drawAngleWindow(Mat &img) { + frame++; - for (FuPoint p : markings2) { - if (horizDistanceToPolynomial(poly, p) <= maxDistance) { - count2++; - tmpSupporters.push_back(p); - } - } + if (!PUBLISH_IMAGES && !SHOW_ANGLE_WINDOW) { + return; + } - for (FuPoint p : markings3) { - if (horizDistanceToPolynomial(poly, p) <= maxDistance) { - count3++; - tmpSupporters.push_back(p); - } - } + Mat transformedImagePaintableLaneModel = img.clone(); + cvtColor(transformedImagePaintableLaneModel, transformedImagePaintableLaneModel, CV_GRAY2BGR); - if (count1 == 0 || count2 == 0 || count3 == 0) { - poly.clear(); - //DEBUG_TEXT(dbgMessages, "Poly had no supporters in one of the regions"); - continue; - } + if (polyDetectedRight || isPolyMovedRight) { + /*int r = lanePolynomial.getLastUsedPosition() == LEFT ? 255 : 0; + int g = lanePolynomial.getLastUsedPosition() == CENTER ? 255 : 0; + int b = lanePolynomial.getLastUsedPosition() == RIGHT ? 255 : 0; - // calculate the proportion of supporters of all lane markings - double proportion = (double(count1) / markings1.size() - + double(count2) / markings2.size() - + 3 * (double(count3) / markings3.size())) / 5; - if (proportion < proportionThreshould) { - poly.clear(); - //DEBUG_TEXT(dbgMessages, "Poly proportion was smaller than threshold"); - continue; + for(int i = minYPolyRoi; i < maxYRoi; i++) { + cv::Point pointLoc = cv::Point(lanePolynomial.getLanePoly().at(i)+projImageHalfW, i); + cv::circle(transformedImagePaintableLaneModel, pointLoc, 0, cv::Scalar(b,g,r), -1); } - // check if poly is better than bestPoly - if (proportion > bestPoly.second) { - bestPoly = std::make_pair(poly, proportion); - supporters = tmpSupporters; + std::vector> supps; + switch (lanePolynomial.getLastUsedPosition()) { + case LEFT: + supps = supportersLeft; + break; + case CENTER: + supps = supportersCenter; + break; + case RIGHT: + supps = supportersRight; + break; + default: + ROS_ERROR("No last used position"); + supps = supportersRight; + break; + }; + + for(int i = 0; i < (int)supps.size(); i++) { + FuPoint supp = supps[i]; + cv::Point suppLoc = cv::Point(supp.getX(), supp.getY()); + cv::circle(transformedImagePaintableLaneModel, suppLoc, 1, cv::Scalar(b,g,r), -1); + }*/ + + cv::Point pointLoc = cv::Point(proj_image_w_half, projImageH); + cv::circle(transformedImagePaintableLaneModel, pointLoc, 2, cv::Scalar(0, 0, 255), -1); + + cv::Point anglePointLoc = cv::Point(sin(lastAngle * PI / 180) * angleAdjacentLeg + proj_image_w_half, + projImageH - (cos(lastAngle * PI / 180) * angleAdjacentLeg)); + cv::line(transformedImagePaintableLaneModel, pointLoc, anglePointLoc, cv::Scalar(255, 255, 255)); + + cv::Point startNormalPoint = cv::Point(pointForAngle.getX(), pointForAngle.getY()); + cv::circle(transformedImagePaintableLaneModel, startNormalPoint, 2, cv::Scalar(100, 100, 100), -1); + + cv::Point targetPoint = cv::Point(movedPointForAngle.getX(), movedPointForAngle.getY()); + cv::circle(transformedImagePaintableLaneModel, targetPoint, 2, cv::Scalar(0, 0, 255), -1); + + /*cv::Point adjacentLegPoint = cv::Point(projImageHalfW, projImageH - adjacentLeg); + cv::line(transformedImagePaintableLaneModel, pointLoc, adjacentLegPoint, cv::Scalar(255,0,0)); + + cv::Point oppositeLegPoint = cv::Point(projImageHalfW + oppositeLeg, projImageH - adjacentLeg); + cv::line(transformedImagePaintableLaneModel, adjacentLegPoint, oppositeLegPoint, cv::Scalar(0,255,0));*/ + + double m = -gradientForAngle; + + double n = pointForAngle.getY() - m * pointForAngle.getX(); + double x = 10; + double y = m * x + n; + + cv::Point endNormalPoint = cv::Point(x, y); + cv::line(transformedImagePaintableLaneModel, startNormalPoint, endNormalPoint, cv::Scalar(0, 0, 0)); - points.clear(); - points.push_back(p1); - points.push_back(p2); - points.push_back(p3); - } + } else { + cv::Point pointLoc = cv::Point(5, 5); + cv::circle(transformedImagePaintableLaneModel, pointLoc, 3, cv::Scalar(0, 0, 255), 0); } - poly = bestPoly.first; - - if (poly.getDegree() == -1) { - return false; + if (PUBLISH_IMAGES) { + pubRGBImageMsg(transformedImagePaintableLaneModel, image_publisher); } - return true; -} - -/** - * Method, that checks, if a polynomial produced during RANSAC counts as usable. - * - * @param position The position of the polynomial, that is checked - * @param poly The polynomial, that is checked - * @param prevPoly The previous polynomial detected at this position - * @return True, if the polynomial counts as valid - */ -bool cLaneDetectionFu::polyValid(ePosition position, NewtonPolynomial poly, NewtonPolynomial prevPoly) { - - if (prevPoly.getDegree() != -1) { - return isSimilar(poly, prevPoly); - } - if (position == RIGHT) { - if (polyDetectedRight) { - return isSimilar(poly, polyRight); - } - if (isPolyMovedRight) { - return isSimilar(poly, movedPolyRight); - } - } - if (position == CENTER) { - if (polyDetectedCenter) { - return isSimilar(poly, polyCenter); - } - if (isPolyMovedCenter) { - return isSimilar(poly, movedPolyCenter); - } - } - if (position == LEFT) { - if (polyDetectedLeft) { - return isSimilar(poly, polyLeft); - } - if (isPolyMovedLeft) { - return isSimilar(poly, movedPolyLeft); - } + if (SHOW_ANGLE_WINDOW) { + cv::namedWindow("Lane polynomial", WINDOW_NORMAL); + cv::imshow("Lane polynomial", transformedImagePaintableLaneModel); + cv::waitKey(1); } - - return true; } -bool cLaneDetectionFu::isSimilar(const NewtonPolynomial &poly1, const NewtonPolynomial &poly2) { - FuPoint p1 = FuPoint(poly1.at(polyY1), polyY1); - FuPoint p2 = FuPoint(poly2.at(polyY1), polyY1); +void cLaneDetectionFu::pubRGBImageMsg(cv::Mat &rgb_mat, image_transport::CameraPublisher publisher) { + sensor_msgs::ImagePtr rgb_img(new sensor_msgs::Image); - if (horizDistance(p1, p2) > interestDistancePoly) { //0.05 * meters - return false; - } + rgb_img->header.seq = head_sequence_id; + rgb_img->header.stamp = head_time_stamp; + rgb_img->header.frame_id = rgb_frame_id; - FuPoint p3 = FuPoint(poly1.at(polyY2), polyY2); - FuPoint p4 = FuPoint(poly2.at(polyY2), polyY2); + rgb_img->width = rgb_mat.cols; + rgb_img->height = rgb_mat.rows; - if (horizDistance(p3, p4) > interestDistancePoly) { //0.05 * meters - return false; - } + rgb_img->encoding = sensor_msgs::image_encodings::BGR8; + rgb_img->is_bigendian = 0; - FuPoint p5 = FuPoint(poly1.at(polyY3), polyY3); - FuPoint p6 = FuPoint(poly2.at(polyY3), polyY3); + int step = sizeof(unsigned char) * 3 * rgb_img->width; + int size = step * rgb_img->height; + rgb_img->step = step; + rgb_img->data.resize(size); + memcpy(&(rgb_img->data[0]), rgb_mat.data, size); - if (horizDistance(p5, p6) > interestDistancePoly) { //0.05 * meters - return false; - } + rgb_camera_info->header.frame_id = rgb_frame_id; + rgb_camera_info->header.stamp = head_time_stamp; + rgb_camera_info->header.seq = head_sequence_id; - return true; + publisher.publish(rgb_img, rgb_camera_info); } -void cLaneDetectionFu::pubAngle() { - if (!polyDetectedRight && !isPolyMovedRight) { - return; +void cLaneDetectionFu::debugPaintPolynom(cv::Mat &m, cv::Scalar color, NewtonPolynomial &p, int start, int end) { + cv::Point point; + for (int i = start; i < end; i++) { + point = cv::Point(p.at(i), i); + cv::circle(m, point, 0, color, -1); } +} - int y = proj_image_h - angleAdjacentLeg; - double xRightLane; - double m; - - if (polyDetectedRight) { - xRightLane = polyRight.at(y); - m = gradient(y, pointsRight, polyRight.getCoefficients()); - } else { - xRightLane = movedPolyRight.at(y); - m = gradient(y, movedPointsRight, movedPolyRight.getCoefficients()); +void cLaneDetectionFu::debugPaintPoints(cv::Mat &m, cv::Scalar color, vector> &points) { + for (FuPoint point : points) { + cv::Point pointLoc = cv::Point(point.getX(), point.getY()); + cv::circle(m, pointLoc, 1, color, -1); } +} - double offset = -1 * laneWidth / 2; - shiftPoint(movedPointForAngle, m, offset, (int) xRightLane, y); - - pointForAngle.setX(xRightLane); - pointForAngle.setY(y); - - gradientForAngle = m; - - oppositeLeg = movedPointForAngle.getX() - proj_image_w_half; - adjacentLeg = proj_image_h - movedPointForAngle.getY(); - double result = atan(oppositeLeg / adjacentLeg) * 180 / PI; +void cLaneDetectionFu::debugWriteImg(cv::Mat &image, string folder) { + stringstream img; + img << "./" << folder << "/" << frame << ".jpg"; + cv::imwrite(img.str(), image); +} - /* - * filter too rash steering angles / jitter in polynomial data - */ - if (std::abs(result - lastAngle) > maxAngleDiff) { - if (result - lastAngle > 0) - result = lastAngle + maxAngleDiff; - else - result = lastAngle - maxAngleDiff; - } - lastAngle = result; - std_msgs::Float32 angleMsg; +/* + * ROS: + */ - angleMsg.data = result; - publish_angle.publish(angleMsg); -} void cLaneDetectionFu::config_callback(line_detection_fu::LaneDetectionConfig &config, uint32_t level) { ROS_ERROR("Reconfigure Request"); interestDistancePoly = config.interestDistancePoly; - interestDistanceDefault= config.interestDistanceDefault; + interestDistanceDefault = config.interestDistanceDefault; iterationsRansac = config.iterationsRansac; maxYRoi = config.maxYRoi; minYDefaultRoi = config.minYDefaultRoi; @@ -1352,14 +1568,13 @@ void cLaneDetectionFu::config_callback(line_detection_fu::LaneDetectionConfig &c polyY1 = config.polyY1; polyY2 = config.polyY2; polyY3 = config.polyY3; - detectLaneStartX = config.detectLaneStartX; maxAngleDiff = config.maxAngleDiff; - proj_y_start = config.proj_y_start; - roi_top_w = config.roi_top_w; - roi_bottom_w = config.roi_bottom_w; + projYStart = config.projYStart; + roiTopW = config.roiTopW; + roiBottomW = config.roiBottomW; proportionThreshould = config.proportionThreshould; - m_gradientThreshold = config.m_gradientThreshold; - m_nonMaxWidth = config.m_nonMaxWidth; + gradientThreshold = config.gradientThreshold; + nonMaxWidth = config.nonMaxWidth; laneMarkingSquaredThreshold = config.laneMarkingSquaredThreshold; angleAdjacentLeg = config.angleAdjacentLeg; scanlinesVerticalDistance = config.scanlinesVerticalDistance; @@ -1371,7 +1586,7 @@ void cLaneDetectionFu::config_callback(line_detection_fu::LaneDetectionConfig &c int main(int argc, char **argv) { ros::init(argc, argv, "cLaneDetectionFu"); ros::NodeHandle nh; - + cLaneDetectionFu node = cLaneDetectionFu(nh); dynamic_reconfigure::Server server; @@ -1379,8 +1594,130 @@ int main(int argc, char **argv) { f = boost::bind(&cLaneDetectionFu::config_callback, &node, _1, _2); server.setCallback(f); - while(ros::ok()) { + while (ros::ok()) { ros::spinOnce(); } return 0; } + + + +/* + * Unused (but may be useful in future) + */ + + + +/** + * Calculates the x value of the point where the normal of the tangent of a + * polynomial at a given point p intersects with a second polynomial. + * + * The formula for the intersection point is obtained by setting equal the + * following two formula: + * + * 1. the formula of the normal in point-slope-form: + * y - p_y = -(1 / m) * (x - p_x) which is the same as + * y = -(x / m) + (p_x / m) + p_y + * + * 2. the formula of the second polynomial of 2nd degree in newton basis form: + * y = c0 + c1(x - x0) + c2(x - x0)(x - x1) + * + * Expanding everything and moving it to the right side gives a quadratic + * equation in the general form of 0 = ax^2 + bx + c, which can be solved using + * the general quadratic formula x = (-b +- sqrt(b^2 - 4ac)) / 2a + * + * The three cases for the discriminant are taken into account. + * + * @param p The point of the first poly at which its tangent is used + * @param m The gradient of the tangent + * @param points The data points used for interpolating the second polynomial + * @param coeffs The coeffs of the second polynomial with newton basis + * @return The x value of the intersection point of normal and 2nd poly + */ +double cLaneDetectionFu::intersection(FuPoint &p, double &m, + std::vector> &points, std::vector &coeffs) { + double a = coeffs[2]; + double b = coeffs[1] - (coeffs[2] * points[1].getY()) + - (coeffs[2] * points[0].getY()) + (1.0 / m); + double c = coeffs[0] - (coeffs[1] * points[0].getY()) + + (coeffs[2] * points[0].getY() * points[1].getY()) + - p.getY() - (p.getX() / m); + + double dis = std::pow(b, 2) - (4 * a * c); + double x1 = 0; + double x2 = 0; + + if (dis < 0) { + return -1; + } else if (dis == 0) { + return -b / (2 * a); + } else { + x1 = (-b + std::sqrt(std::pow(b, 2) - (4 * a * c))) / (2 * a); + x2 = (-b - std::sqrt(std::pow(b, 2) - (4 * a * c))) / (2 * a); + } + + return fmax(x1, x2); +} + +/** + * Calculates the gradient of a second polynomial at the point, at which the + * normal of the tangent of the first polynomial at the given point + * intersects with the second polynomial. + * + * @param x The given x value of the point on the first polynomial + * @param poly1 The first polynomial + * @param points1 The data points used for interpolating the first poly + * @param points2 The data points used for interpolating the second poly + * @param coeffs1 The coeffs of the first poly using newton basis + * @param coeffs2 The coeffs of the second poly using newton basis + * @param m1 The gradient of the first poly at x + * @return The gradient of the second poly at the intersection point + */ +double cLaneDetectionFu::nextGradient(double x, NewtonPolynomial &poly1, + std::vector> &points1, std::vector> &points2, + std::vector coeffs1, std::vector coeffs2, double m1) { + + FuPoint p = FuPoint(x, poly1.at(x)); + double x2 = intersection(p, m1, points2, coeffs2); + + return gradient(x2, points2, coeffs2); +} + +/** + * Check two gradients for similarity. Return true if the difference in degree + * is less than 10. + * + * @param m1 The first gradient + * @param m2 The second gradient + * @return True, if the diffenence between the gradients is less than 10° + */ +bool cLaneDetectionFu::gradientsSimilar(double &m1, double &m2) { + double a1 = atan(m1) * 180 / PI; + double a2 = atan(m2) * 180 / PI; + + if (abs(a1 - a2) < 10) { + return true; + } else { + return false; + } +} + +/** + * Finds the position of the polynomial with the highest proportion. + * @return The position of the best polynomial + */ +ePosition cLaneDetectionFu::maxProportion() { + ePosition maxPos = LEFT; + double maxVal = bestPolyLeft.second; + + if (bestPolyCenter.second > maxVal) { + maxPos = CENTER; + maxVal = bestPolyCenter.second; + } + + if (bestPolyRight.second > maxVal) { + maxPos = RIGHT; + } + + return maxPos; +} \ No newline at end of file diff --git a/src/line_detection_fu/src/laneDetection.h b/src/line_detection_fu/src/laneDetection.h index 6dc8b62c..989c634c 100644 --- a/src/line_detection_fu/src/laneDetection.h +++ b/src/line_detection_fu/src/laneDetection.h @@ -30,11 +30,13 @@ THIS SOFTWARE IS PROVIDED BY AUDI AG AND CONTRIBUTORS �AS IS� AND ANY EXPRES #ifndef _LaneDetection_FILTER_HEADER_ #define _LaneDetection_FILTER_HEADER_ + #include #include #include #include +#include #include #include #include @@ -48,6 +50,7 @@ THIS SOFTWARE IS PROVIDED BY AUDI AG AND CONTRIBUTORS �AS IS� AND ANY EXPRES #include #include +#include #include "tools/LineSegment.h" #include "tools/Edges.h" @@ -55,7 +58,6 @@ THIS SOFTWARE IS PROVIDED BY AUDI AG AND CONTRIBUTORS �AS IS� AND ANY EXPRES #include "tools/LanePolynomial.h" #include "tools/enums.h" #include "tools/IPMapper.h" -#include "tools/DebugUtils.h" #include #include @@ -63,268 +65,281 @@ THIS SOFTWARE IS PROVIDED BY AUDI AG AND CONTRIBUTORS �AS IS� AND ANY EXPRES using namespace std; -class DebugUtils; class cLaneDetectionFu { - private: - - // the node handle - ros::NodeHandle nh_; - - // Node handle in the private namespace - ros::NodeHandle priv_nh_; - - // subscribers - ros::Subscriber read_images_; - - // publishers - //ros::Publisher publish_images; - //ros::Publisher publish_curvature; - ros::Publisher publish_angle; - - IPMapper ipMapper; - - std::string camera_name; - - int cam_w; - int cam_h; - int proj_y_start; - int proj_image_h; - int proj_image_w; - int proj_image_w_half; - int proj_image_horizontal_offset; - int roi_top_w; - int roi_bottom_w; - - int scanlinesVerticalDistance; - int scanlinesMaxCount; - - vector> > scanlines; - - int m_gradientThreshold; - int m_nonMaxWidth; - - int polyY1; - int polyY2; - int polyY3; - - - /** - * The lane marking polynomials detected in the current picture. - */ - NewtonPolynomial polyLeft; - NewtonPolynomial polyCenter; - NewtonPolynomial polyRight; - - /** - * Horizontal relative positions of the default lane marking lines. - * - * These lines are situated in a position, where the lane markings of a - * straight lane would show up in the relative coordinate system with the - * car standing in the center of the right lane. - */ - int defaultXLeft = 0; - int defaultXCenter = 0; - int defaultXRight = 0; - - /** - * The maximum distance of a point to a polynomial so that it counts as a - * supporter. - */ - int maxDistance; - - /** - * The horizontal distance to the last detected polynomial, within lane - * markings have to lie to be considered for the detection of the next - * polynomial. The width of the polynomial region of interest is two times - * this distance. - */ - int interestDistancePoly; - - /** - * The horizontal distance to the default line, within lane markings have to - * lie to be considered for the detection of a polynomial. The width of the - * default region of interest is two times this distance. - */ - int interestDistanceDefault; - - /** - * The minimal y of the ROIs. Points with smaller y-Values are not - * used in RANSAC. - */ - int maxYRoi; - - /** - * The maximal y of default ROIs. Points with bigger y-Values are not used. - */ - int minYDefaultRoi; - - /** - * The maximal y of the polynomial ROIs. Points with bigger y-Values are not - * used. - */ - int minYPolyRoi; - - /** - * The minimal proportion of supporters of all points within a ROI. - * Polynomials with lower proportions are discarded. - */ - double proportionThreshould; - - /** - * Number of RANSAC iterations - */ - int iterationsRansac; - - /** - * flags to determine if a valid polynomial was detected in the last frame - * and therefore the polynomial ROI should be used or if no polynomial could - * be detected and the default ROI is used. - */ - bool polyDetectedLeft; - bool polyDetectedCenter; - bool polyDetectedRight; - - /** - * pairs for saving the best lane polynomials produced during RANSAC - * - * first : current best NewtonPolynomial - * second: proportion of supporters of used lane marking points (quality) - */ - std::pair bestPolyLeft; - std::pair bestPolyCenter; - std::pair bestPolyRight; +private: + + // the node handle + ros::NodeHandle nh_; + + // Node handle in the private namespace + ros::NodeHandle priv_nh_; + + // subscribers + ros::Subscriber read_images_; + + // publishers + //ros::Publisher publish_images; + //ros::Publisher publish_curvature; + ros::Publisher publish_angle; + + IPMapper ipMapper; + + std::string camera_name; + + image_transport::CameraPublisher image_publisher; + image_transport::CameraPublisher image_publisher_ransac; + image_transport::CameraPublisher image_publisher_lane_markings; + + int cam_w; + int cam_h; + int projYStart; + int projImageH; + int projImageW; + int proj_image_w_half; + int projImageHorizontalOffset; + int roiTopW; + int roiBottomW; + + int scanlinesVerticalDistance; + int scanlinesMaxCount; + + vector>> scanlines; + + int gradientThreshold; + int nonMaxWidth; + + int polyY1; + int polyY2; + int polyY3; + + + /** + * The lane marking polynomials detected in the current picture. + */ + NewtonPolynomial polyLeft; + NewtonPolynomial polyCenter; + NewtonPolynomial polyRight; + + /** + * Horizontal relative positions of the default lane marking lines. + * + * These lines are situated in a position, where the lane markings of a + * straight lane would show up in the relative coordinate system with the + * car standing in the center of the right lane. + */ + int defaultXLeft = 0; + int defaultXCenter = 0; + int defaultXRight = 0; + + /** + * The maximum distance of a point to a polynomial so that it counts as a + * supporter. + */ + int maxDistance; + + /** + * The horizontal distance to the last detected polynomial, within lane + * markings have to lie to be considered for the detection of the next + * polynomial. The width of the polynomial region of interest is two times + * this distance. + */ + int interestDistancePoly; + + /** + * The horizontal distance to the default line, within lane markings have to + * lie to be considered for the detection of a polynomial. The width of the + * default region of interest is two times this distance. + */ + int interestDistanceDefault; + + /** + * The minimal y of the ROIs. Points with smaller y-Values are not + * used in RANSAC. + */ + int maxYRoi; + + /** + * The maximal y of default ROIs. Points with bigger y-Values are not used. + */ + int minYDefaultRoi; + + /** + * The maximal y of the polynomial ROIs. Points with bigger y-Values are not + * used. + */ + int minYPolyRoi; + + /** + * The minimal proportion of supporters of all points within a ROI. + * Polynomials with lower proportions are discarded. + */ + double proportionThreshould; + + /** + * Number of RANSAC iterations + */ + int iterationsRansac; + + /** + * flags to determine if a valid polynomial was detected in the last frame + * and therefore the polynomial ROI should be used or if no polynomial could + * be detected and the default ROI is used. + */ + bool polyDetectedLeft; + bool polyDetectedCenter; + bool polyDetectedRight; + + /** + * pairs for saving the best lane polynomials produced during RANSAC + * + * first : current best NewtonPolynomial + * second: proportion of supporters of used lane marking points (quality) + */ + std::pair bestPolyLeft; + std::pair bestPolyCenter; + std::pair bestPolyRight; + + + /** + * Lists containing the lane marking points selected for detecting the lane + * polynomials during RANSAC + */ + std::vector> laneMarkingsLeft; + std::vector> laneMarkingsCenter; + std::vector> laneMarkingsRight; + std::vector> laneMarkingsNotUsed; + + /** + * Newton interpolation data points selected for the best polynomial + */ + std::vector> pointsLeft; + std::vector> pointsCenter; + std::vector> pointsRight; + + /** + * Vectors containing the supporters of the best polynomial + */ + std::vector> supportersLeft; + std::vector> supportersCenter; + std::vector> supportersRight; + + /** + * The polynomials detected on the previous picture + */ + NewtonPolynomial prevPolyLeft; + NewtonPolynomial prevPolyCenter; + NewtonPolynomial prevPolyRight; + + NewtonPolynomial movedPolyLeft; + NewtonPolynomial movedPolyCenter; + NewtonPolynomial movedPolyRight; + + bool isPolyMovedRight = false; + bool isPolyMovedCenter = false; + bool isPolyMovedLeft = false; + + vector> movedPointsRight; + + int laneMarkingSquaredThreshold; + int angleAdjacentLeg; - /** - * Lists containing the lane marking points selected for detecting the lane - * polynomials during RANSAC - */ - std::vector> laneMarkingsLeft; - std::vector> laneMarkingsCenter; - std::vector> laneMarkingsRight; - std::vector> laneMarkingsNotUsed; - - /** - * Newton interpolation data points selected for the best polynomial - */ - std::vector> pointsLeft; - std::vector> pointsCenter; - std::vector> pointsRight; + double lastAngle; - /** - * Vectors containing the supporters of the best polynomial - */ - std::vector> supportersLeft; - std::vector> supportersCenter; - std::vector> supportersRight; + int maxAngleDiff; - /** - * The polynomials detected on the previous picture - */ - NewtonPolynomial prevPolyLeft; - NewtonPolynomial prevPolyCenter; - NewtonPolynomial prevPolyRight; + FuPoint movedPointForAngle; + FuPoint pointForAngle; + double oppositeLeg; + double adjacentLeg; + double gradientForAngle; - NewtonPolynomial movedPolyLeft; - NewtonPolynomial movedPolyCenter; - NewtonPolynomial movedPolyRight; + double laneWidth = 45.f; - bool isPolyMovedRight = false; - bool isPolyMovedCenter = false; - bool isPolyMovedLeft = false; + void drawEdgeWindow(cv::Mat &img, std::vector> edges); - vector> movedPointsRight; + void drawLaneMarkingsWindow(cv::Mat &img, std::vector> &laneMarkings); - DebugUtils debugUtils; + void drawGroupedLaneMarkingsWindow(cv::Mat &img); - int laneMarkingSquaredThreshold; + void drawRansacWindow(cv::Mat &img); - int angleAdjacentLeg; + void drawAngleWindow(cv::Mat &img); - int detectLaneStartX; + void pubRGBImageMsg(cv::Mat &rgb_mat, image_transport::CameraPublisher publisher); - double lastAngle; + void debugPaintPolynom(cv::Mat &m, cv::Scalar color, NewtonPolynomial &p, int start, int end); - int maxAngleDiff; + void debugPaintPoints(cv::Mat &m, cv::Scalar color, std::vector> &points); - FuPoint movedPointForAngle; - FuPoint pointForAngle; - double oppositeLeg; - double adjacentLeg; - double gradientForAngle; + void debugWriteImg(cv::Mat &image, std::string folder); - double laneWidth = 45.f; +public: - public: + cLaneDetectionFu(ros::NodeHandle nh); - static const uint32_t MY_ROS_QUEUE_SIZE = 1; + virtual ~cLaneDetectionFu(); - static constexpr double PI = 3.14159265; - - cLaneDetectionFu(ros::NodeHandle nh); + void ProcessInput(const sensor_msgs::Image::ConstPtr &msg); - virtual ~cLaneDetectionFu(); - - void ProcessInput(const sensor_msgs::Image::ConstPtr& msg); + void pubAngle(); - void pubAngle(); + std::vector>> getScanlines(); - std::vector> > getScanlines(); + std::vector > scanImage(cv::Mat image); - std::vector > scanImage(cv::Mat image); + std::vector> extractLaneMarkings(const std::vector> &edges); - std::vector> extractLaneMarkings(const std::vector>& edges); + void buildLaneMarkingsLists(const std::vector> &laneMarkings); - void buildLaneMarkingsLists(const std::vector> &laneMarkings); + void findLanePositions(vector> &laneMarkings); - void findLanePositions(vector> &laneMarkings); + void shiftPoint(FuPoint &p, double m, double offset, int x, int y); - void shiftPoint(FuPoint &p, double m, double offset, int x, int y); + void shiftPoint(FuPoint &p, double m, double offset, FuPoint &origin); - void shiftPoint(FuPoint &p, double m, double offset, FuPoint &origin); + void generateMovedPolynomials(); - void generateMovedPolynomials(); + bool isInRange(FuPoint &lanePoint, FuPoint &p); - bool isInRange(FuPoint &lanePoint, FuPoint &p); + int horizDistanceToDefaultLine(ePosition &line, FuPoint &p); - int horizDistanceToDefaultLine(ePosition &line, FuPoint &p); + int horizDistanceToPolynomial(NewtonPolynomial &poly, FuPoint &p); - int horizDistanceToPolynomial(NewtonPolynomial& poly, FuPoint &p); + bool isInDefaultRoi(ePosition position, FuPoint &p); - bool isInDefaultRoi(ePosition position, FuPoint &p); + bool isInPolyRoi(NewtonPolynomial &poly, FuPoint &p); - bool isInPolyRoi(NewtonPolynomial &poly, FuPoint &p); + void ransac(); - void ransac(); + bool ransacInternal(ePosition position, + std::vector> &laneMarkings, + std::pair &bestPoly, NewtonPolynomial &poly, + std::vector> &supporters, NewtonPolynomial &prevPoly, + std::vector> &points); - bool ransacInternal(ePosition position, - std::vector>& laneMarkings, - std::pair& bestPoly, NewtonPolynomial& poly, - std::vector>& supporters, NewtonPolynomial& prevPoly, - std::vector>& points); + bool polyValid(ePosition, NewtonPolynomial, NewtonPolynomial); - bool polyValid(ePosition, NewtonPolynomial, NewtonPolynomial); + bool isSimilar(const NewtonPolynomial &poly1, const NewtonPolynomial &poly2); - bool isSimilar(const NewtonPolynomial &poly1, const NewtonPolynomial &poly2); + int horizDistance(FuPoint &p1, FuPoint &p2); - int horizDistance(FuPoint &p1, FuPoint &p2); + double gradient(double, std::vector> &, std::vector); - double gradient(double, std::vector>&, std::vector); + double intersection(FuPoint &, double &, std::vector> &, + std::vector &); - double intersection(FuPoint&, double&, std::vector>&, - std::vector&); + double nextGradient(double, NewtonPolynomial &, + std::vector> &, std::vector> &, + std::vector, std::vector, double); - double nextGradient(double, NewtonPolynomial&, - std::vector>&, std::vector>&, - std::vector, std::vector, double); + bool gradientsSimilar(double &, double &); - bool gradientsSimilar(double&, double&); + ePosition maxProportion(); - ePosition maxProportion(); + void config_callback(line_detection_fu::LaneDetectionConfig &config, uint32_t level); - void config_callback(line_detection_fu::LaneDetectionConfig &config, uint32_t level); - }; #endif diff --git a/src/line_detection_fu/src/tools/DebugUtils.cpp b/src/line_detection_fu/src/tools/DebugUtils.cpp deleted file mode 100644 index f2ac2880..00000000 --- a/src/line_detection_fu/src/tools/DebugUtils.cpp +++ /dev/null @@ -1,324 +0,0 @@ -#include "DebugUtils.h" - -using namespace std; -using namespace cv; - -DebugUtils::DebugUtils(ros::NodeHandle &nh, int minYPolyRoi, int maxYRoi, int roiBottomW, int roiTopW, int projImageW, int projImageH) { - this->minYPolyRoi = minYPolyRoi; - this->maxYRoi = maxYRoi; - this->roiBottomW = roiBottomW; - this->roiTopW = roiTopW; - this->projImageHalfW = projImageW / 2; - this->projImageH = projImageH; - - head_time_stamp = ros::Time::now(); - - if (PUBLISH_IMAGES) { - image_transport::ImageTransport image_transport(nh); - - image_publisher = image_transport.advertiseCamera("/lane_model/lane_model_image", cLaneDetectionFu::MY_ROS_QUEUE_SIZE); - image_publisher_ransac = image_transport.advertiseCamera("/lane_model/ransac", cLaneDetectionFu::MY_ROS_QUEUE_SIZE); - image_publisher_lane_markings = image_transport.advertiseCamera("/lane_model/lane_markings", cLaneDetectionFu::MY_ROS_QUEUE_SIZE); - - if (!rgb_camera_info) { - rgb_camera_info.reset(new sensor_msgs::CameraInfo()); - rgb_camera_info->width = projImageW; - rgb_camera_info->height = projImageH + 50; - } - } - - if (SAVE_FRAME_IMAGES) { - mkdir("groupedLaneMarkings", S_IRWXU); - mkdir("ransac", S_IRWXU); - - system("exec rm -r ./groupedLaneMarkings/*"); - system("exec rm -r ./ransac/*"); - } -} - -void DebugUtils::drawEdgeWindow(Mat &img, vector> edges) { - if (!SHOW_EDGE_WINDOW) { - return; - } - - Mat transformedImagePaintable = img.clone(); - cvtColor(transformedImagePaintable, transformedImagePaintable, CV_GRAY2BGR); - for (int i = 0; i < (int) edges.size(); i++) { - for (int j = 0; j < edges[i].size(); j++) { - FuPoint edge = edges[i][j].getImgPos(); - Point edgeLoc = Point(edge.getX(), edge.getY()); - circle(transformedImagePaintable, edgeLoc, 1, Scalar(0, 0, edges[i][j].getValue()), -1); - } - } - - /*cv::Point2d p1(projImageHalfW-(roiBottomW/2),maxYRoi-1); - cv::Point2d p2(projImageHalfW+(roiBottomW/2),maxYRoi-1); - cv::Point2d p3(projImageHalfW+(roiTopW/2),minYPolyRoi); - cv::Point2d p4(projImageHalfW-(roiTopW/2),minYPolyRoi); - cv::line(transformedImagePaintable,p1,p2,cv::Scalar(0,200,0)); - cv::line(transformedImagePaintable,p2,p3,cv::Scalar(0,200,0)); - cv::line(transformedImagePaintable,p3,p4,cv::Scalar(0,200,0)); - cv::line(transformedImagePaintable,p4,p1,cv::Scalar(0,200,0));*/ - - /*for(int i = 0; i < (int)scanlines.size(); i++) - { - LineSegment scanline = scanlines[i][0]; - cv::Point scanlineStart = cv::Point(scanline.getStart().getX(), scanline.getStart().getY()); - cv::Point scanlineEnd = cv::Point(scanline.getEnd().getX(), scanline.getEnd().getY()); - cv::line(transformedImagePaintable,scanlineStart,scanlineEnd,cv::Scalar(255,0,0)); - }*/ - - - cv::namedWindow("ROI, scanlines and edges", WINDOW_NORMAL); - cv::imshow("ROI, scanlines and edges", transformedImagePaintable); - cv::waitKey(1); -} - -void DebugUtils::drawLaneMarkingsWindow(Mat &img, vector> &laneMarkings) { - if (!SHOW_LANE_MARKINGS_WINDOW) { - return; - } - - Mat transformedImagePaintable = img.clone(); - cv::cvtColor(transformedImagePaintable, transformedImagePaintable, CV_GRAY2BGR); - for (int i = 0; i < (int) laneMarkings.size(); i++) { - FuPoint marking = laneMarkings[i]; - cv::Point markingLoc = cv::Point(marking.getX(), marking.getY()); - cv::circle(transformedImagePaintable, markingLoc, 1, cv::Scalar(0, 255, 0), -1); - } - - cv::namedWindow("Lane Markings", WINDOW_NORMAL); - cv::imshow("Lane Markings", transformedImagePaintable); - cv::waitKey(1); -} - -void DebugUtils::drawGroupedLaneMarkingsWindow(Mat &img, int defaultXLeft, int defaultXCenter, int defaultXRight, - NewtonPolynomial &polyLeft, NewtonPolynomial &polyCenter, - NewtonPolynomial &polyRight, - std::vector> &laneMarkingsLeft, - std::vector> &laneMarkingsCenter, - std::vector> &laneMarkingsRight, - std::vector> &laneMarkingsNotUsed) { - if (!PUBLISH_IMAGES && !SHOW_GROUPED_LANE_MARKINGS_WINDOW && !SAVE_FRAME_IMAGES) { - return; - } - - Mat transformedImagePaintable = img.clone(); - cv::cvtColor(transformedImagePaintable,transformedImagePaintable,CV_GRAY2BGR); - - debugPaintPoints(transformedImagePaintable, Scalar(0,0,255), laneMarkingsLeft); - debugPaintPoints(transformedImagePaintable, Scalar(0,255,0), laneMarkingsCenter); - debugPaintPoints(transformedImagePaintable, Scalar(255,0,0), laneMarkingsRight); - debugPaintPoints(transformedImagePaintable, Scalar(0,255,255), laneMarkingsNotUsed); - - debugPaintPolynom(transformedImagePaintable, cv::Scalar(0, 255, 255), polyLeft, minYPolyRoi, maxYRoi); - debugPaintPolynom(transformedImagePaintable, cv::Scalar(255, 255, 0), polyCenter, minYPolyRoi, maxYRoi); - debugPaintPolynom(transformedImagePaintable, cv::Scalar(255, 0, 255), polyRight, minYPolyRoi, maxYRoi); - - cv::Point2d p1l(defaultXLeft,minYPolyRoi); - cv::Point2d p2l(defaultXLeft,maxYRoi-1); - cv::line(transformedImagePaintable,p1l,p2l,cv::Scalar(0,0,255)); - - cv::Point2d p1c(defaultXCenter,minYPolyRoi); - cv::Point2d p2c(defaultXCenter,maxYRoi-1); - cv::line(transformedImagePaintable,p1c,p2c,cv::Scalar(0,255,0)); - - cv::Point2d p1r(defaultXRight,minYPolyRoi); - cv::Point2d p2r(defaultXRight,maxYRoi-1); - cv::line(transformedImagePaintable,p1r,p2r,cv::Scalar(255,0,0)); - - cv::Point2d p1(projImageHalfW-(roiBottomW/2),maxYRoi-1); - cv::Point2d p2(projImageHalfW+(roiBottomW/2),maxYRoi-1); - cv::Point2d p3(projImageHalfW+(roiTopW/2),minYPolyRoi); - cv::Point2d p4(projImageHalfW-(roiTopW/2),minYPolyRoi); - cv::line(transformedImagePaintable,p1,p2,cv::Scalar(0,200,0)); - cv::line(transformedImagePaintable,p2,p3,cv::Scalar(0,200,0)); - cv::line(transformedImagePaintable,p3,p4,cv::Scalar(0,200,0)); - cv::line(transformedImagePaintable,p4,p1,cv::Scalar(0,200,0)); - - if (PUBLISH_IMAGES) { - pubRGBImageMsg(transformedImagePaintable, image_publisher_lane_markings); - } - - - if (SHOW_GROUPED_LANE_MARKINGS_WINDOW) { - cv::namedWindow("Grouped Lane Markings", WINDOW_NORMAL); - cv::imshow("Grouped Lane Markings", transformedImagePaintable); - cv::waitKey(1); - } - - if (SAVE_FRAME_IMAGES) { - debugWriteImg(transformedImagePaintable, "groupedLaneMarkings"); - } -} - -void DebugUtils::drawRansacWindow(cv::Mat &img, NewtonPolynomial &polyLeft, NewtonPolynomial &polyCenter, - NewtonPolynomial &polyRight, NewtonPolynomial &movedPolyLeft, - NewtonPolynomial &movedPolyCenter, NewtonPolynomial &movedPolyRight) { - - if (!PUBLISH_IMAGES && !SHOW_RANSAC_WINDOW && !SAVE_FRAME_IMAGES) { - return; - } - - cv::Mat transformedImagePaintableRansac = img.clone(); - cv::cvtColor(transformedImagePaintableRansac,transformedImagePaintableRansac,CV_GRAY2BGR); - - debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(0, 0, 255), polyLeft, minYPolyRoi, maxYRoi); - debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(0, 255, 0), polyCenter, minYPolyRoi, maxYRoi); - debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(255, 0, 0), polyRight, minYPolyRoi, maxYRoi); - - debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(139, 0, 139), movedPolyLeft, minYPolyRoi, maxYRoi); - debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(0, 0, 0), movedPolyCenter, minYPolyRoi, maxYRoi); - debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(255, 120, 0), movedPolyRight, minYPolyRoi, maxYRoi); - - - if (PUBLISH_IMAGES) { - pubRGBImageMsg(transformedImagePaintableRansac, image_publisher_ransac); - } - - if (SHOW_RANSAC_WINDOW) { - cv::namedWindow("RANSAC results", WINDOW_NORMAL); - cv::imshow("RANSAC results", transformedImagePaintableRansac); - cv::waitKey(1); - } - - if (SAVE_FRAME_IMAGES) { - debugWriteImg(transformedImagePaintableRansac, "ransac"); - } -} - -void DebugUtils::drawAngleWindow(Mat &img, bool polyDetectedRight, bool isPolyMovedRight, double lastAngle, - double gradientForAngle, int angleAdjacentLeg, FuPoint &pointForAngle, - FuPoint &movedPointForAngle) { - frame++; - - if (!PUBLISH_IMAGES && !SHOW_ANGLE_WINDOW) { - return; - } - - Mat transformedImagePaintableLaneModel = img.clone(); - cvtColor(transformedImagePaintableLaneModel,transformedImagePaintableLaneModel,CV_GRAY2BGR); - - if (polyDetectedRight || isPolyMovedRight) { - /*int r = lanePolynomial.getLastUsedPosition() == LEFT ? 255 : 0; - int g = lanePolynomial.getLastUsedPosition() == CENTER ? 255 : 0; - int b = lanePolynomial.getLastUsedPosition() == RIGHT ? 255 : 0; - - - for(int i = minYPolyRoi; i < maxYRoi; i++) { - cv::Point pointLoc = cv::Point(lanePolynomial.getLanePoly().at(i)+projImageHalfW, i); - cv::circle(transformedImagePaintableLaneModel, pointLoc, 0, cv::Scalar(b,g,r), -1); - } - - std::vector> supps; - switch (lanePolynomial.getLastUsedPosition()) { - case LEFT: - supps = supportersLeft; - break; - case CENTER: - supps = supportersCenter; - break; - case RIGHT: - supps = supportersRight; - break; - default: - ROS_ERROR("No last used position"); - supps = supportersRight; - break; - }; - - for(int i = 0; i < (int)supps.size(); i++) { - FuPoint supp = supps[i]; - cv::Point suppLoc = cv::Point(supp.getX(), supp.getY()); - cv::circle(transformedImagePaintableLaneModel, suppLoc, 1, cv::Scalar(b,g,r), -1); - }*/ - - cv::Point pointLoc = cv::Point(projImageHalfW, projImageH); - cv::circle(transformedImagePaintableLaneModel, pointLoc, 2, cv::Scalar(0,0,255), -1); - - cv::Point anglePointLoc = cv::Point(sin(lastAngle * cLaneDetectionFu::PI / 180) * angleAdjacentLeg + projImageHalfW, projImageH - (cos(lastAngle * cLaneDetectionFu::PI / 180) * angleAdjacentLeg)); - cv::line(transformedImagePaintableLaneModel, pointLoc, anglePointLoc, cv::Scalar(255,255,255)); - - cv::Point startNormalPoint = cv::Point(pointForAngle.getX(), pointForAngle.getY()); - cv::circle(transformedImagePaintableLaneModel, startNormalPoint, 2, cv::Scalar(100,100,100), -1); - - cv::Point targetPoint = cv::Point(movedPointForAngle.getX(), movedPointForAngle.getY()); - cv::circle(transformedImagePaintableLaneModel, targetPoint, 2, cv::Scalar(0,0,255), -1); - - /*cv::Point adjacentLegPoint = cv::Point(projImageHalfW, proj_image_h - adjacentLeg); - cv::line(transformedImagePaintableLaneModel, pointLoc, adjacentLegPoint, cv::Scalar(255,0,0)); - - cv::Point oppositeLegPoint = cv::Point(projImageHalfW + oppositeLeg, proj_image_h - adjacentLeg); - cv::line(transformedImagePaintableLaneModel, adjacentLegPoint, oppositeLegPoint, cv::Scalar(0,255,0));*/ - - double m = -gradientForAngle; - - double n = pointForAngle.getY() - m * pointForAngle.getX(); - double x = 10; - double y = m * x + n; - - cv::Point endNormalPoint = cv::Point(x, y); - cv::line(transformedImagePaintableLaneModel, startNormalPoint, endNormalPoint, cv::Scalar(0,0,0)); - - } else { - cv::Point pointLoc = cv::Point(5, 5); - cv::circle(transformedImagePaintableLaneModel, pointLoc, 3, cv::Scalar(0,0,255), 0); - } - - if (PUBLISH_IMAGES) { - pubRGBImageMsg(transformedImagePaintableLaneModel, image_publisher); - } - - if (SHOW_ANGLE_WINDOW) { - cv::namedWindow("Lane polynomial", WINDOW_NORMAL); - cv::imshow("Lane polynomial", transformedImagePaintableLaneModel); - cv::waitKey(1); - } -} - -void DebugUtils::pubRGBImageMsg(cv::Mat& rgb_mat, image_transport::CameraPublisher publisher) { - sensor_msgs::ImagePtr rgb_img(new sensor_msgs::Image); - - rgb_img->header.seq = head_sequence_id; - rgb_img->header.stamp = head_time_stamp; - rgb_img->header.frame_id = rgb_frame_id; - - rgb_img->width = rgb_mat.cols; - rgb_img->height = rgb_mat.rows; - - rgb_img->encoding = sensor_msgs::image_encodings::BGR8; - rgb_img->is_bigendian = 0; - - int step = sizeof(unsigned char) * 3 * rgb_img->width; - int size = step * rgb_img->height; - rgb_img->step = step; - rgb_img->data.resize(size); - memcpy(&(rgb_img->data[0]), rgb_mat.data, size); - - rgb_camera_info->header.frame_id = rgb_frame_id; - rgb_camera_info->header.stamp = head_time_stamp; - rgb_camera_info->header.seq = head_sequence_id; - - publisher.publish(rgb_img, rgb_camera_info); -} - -void DebugUtils::debugPaintPolynom(cv::Mat &m, cv::Scalar color, NewtonPolynomial &p, int start, int end) { - cv::Point point; - for (int i = start; i < end; i++) { - point = cv::Point(p.at(i), i); - cv::circle(m, point, 0, color, -1); - } -} - -void DebugUtils::debugPaintPoints(cv::Mat &m, cv::Scalar color, vector> &points) { - for(FuPoint point : points) { - cv::Point pointLoc = cv::Point(point.getX(), point.getY()); - cv::circle(m, pointLoc, 1, color, -1); - } -} - -void DebugUtils::debugWriteImg(cv::Mat &image, string folder) { - stringstream img; - img << "./" << folder << "/" << frame << ".jpg"; - cv::imwrite(img.str(), image); -} \ No newline at end of file diff --git a/src/line_detection_fu/src/tools/DebugUtils.h b/src/line_detection_fu/src/tools/DebugUtils.h deleted file mode 100644 index af2f14c5..00000000 --- a/src/line_detection_fu/src/tools/DebugUtils.h +++ /dev/null @@ -1,85 +0,0 @@ -#ifndef _DebugUtils_FILTER_HEADER_ -#define _DebugUtils_FILTER_HEADER_ - -#include - -#include -#include - -#include -#include -#include - -#include "../laneDetection.h" -#include "Edges.h" -#include "NewtonPolynomial.h" -#include "FuPoint.h" - -#include -#include - - -class DebugUtils { -public: - DebugUtils(ros::NodeHandle &nh, int minYPolyRoi, int maxYRoi, int roiBottomW, int roiTopW, - int projImageW, int projImageH); - - void drawEdgeWindow(cv::Mat &img, std::vector> edges); - - void drawLaneMarkingsWindow(cv::Mat &img, std::vector> &laneMarkings); - - void drawGroupedLaneMarkingsWindow(cv::Mat &img, int defaultXLeft, int defaultXCenter, int defaultXRight, - NewtonPolynomial &polyLeft, NewtonPolynomial &polyCenter, NewtonPolynomial &polyRight, - std::vector> &laneMarkingsLeft, - std::vector> &laneMarkingsCenter, - std::vector> &laneMarkingsRight, - std::vector> &laneMarkingsNotUsed); - - void drawRansacWindow(cv::Mat &img, NewtonPolynomial &polyLeft, NewtonPolynomial &polyCenter, NewtonPolynomial &polyRight, - NewtonPolynomial &movedPolyLeft, NewtonPolynomial &movedPolyCenter, NewtonPolynomial &movedPolyRight); - - void drawAngleWindow(cv::Mat &img, bool polyDetectedRight, bool isPolyMovedRight, double lastAngle, - double gradientForAngle, int angleAdjacentLeg, FuPoint &pointForAngle, - FuPoint &movedPointForAngle); - -private: - const bool PUBLISH_IMAGES = true; - const bool SAVE_FRAME_IMAGES = true; - - bool SHOW_EDGE_WINDOW = true; - bool SHOW_LANE_MARKINGS_WINDOW = true; - bool SHOW_GROUPED_LANE_MARKINGS_WINDOW = true; - bool SHOW_RANSAC_WINDOW = true; - bool SHOW_ANGLE_WINDOW = true; - - image_transport::CameraPublisher image_publisher; - image_transport::CameraPublisher image_publisher_ransac; - image_transport::CameraPublisher image_publisher_lane_markings; - - // params for pubRGBImageMsg - unsigned int head_sequence_id = 0; - ros::Time head_time_stamp; - std::string rgb_frame_id = "_rgb_optical_frame"; - sensor_msgs::CameraInfoPtr rgb_camera_info; - - // params from laneDetectionNode - int minYPolyRoi; - int maxYRoi; - int roiBottomW; - int roiTopW; - int projImageHalfW; - int projImageH; - - // frame number for saving image files - std::size_t frame = 0; - - void pubRGBImageMsg(cv::Mat& rgb_mat, image_transport::CameraPublisher publisher); - - void debugPaintPolynom(cv::Mat &m, cv::Scalar color, NewtonPolynomial &p, int start, int end); - - void debugPaintPoints(cv::Mat &m, cv::Scalar color, std::vector> &points); - - void debugWriteImg(cv::Mat &image, std::string folder); -}; - -#endif \ No newline at end of file From 8547a67bc526fdac3084beb6f4310af425c5c563 Mon Sep 17 00:00:00 2001 From: lars Date: Sat, 16 Sep 2017 17:20:24 +0200 Subject: [PATCH 60/79] added some comments --- src/line_detection_fu/src/laneDetection.cpp | 39 +++++++++++++-------- src/line_detection_fu/src/laneDetection.h | 15 ++++++++ 2 files changed, 39 insertions(+), 15 deletions(-) diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index 8efdea17..b51bedab 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -453,14 +453,16 @@ std::vector> cLaneDetectionFu::extractLaneMarkings(const std::vecto } /** - * Calculates x positions of the lanes. Lane markings of first 10 rows from the bottom + * Calculates x positions of the lanes. Lane markings of the first 10 rows from the bottom * of the image are used. Car must start between right and center lane because all lane * marking points in left half of the image are considered as possible lane edge points - * of center lane (analog: right half of image for right lane). Lane marking points in - * range of other lane marking points are supporters because they form a line. The x-value - * of found lane points with maximum supporters will be used. This ensures that non-lane - * points are ignored. Start position of left lane is calculated after start positions - * of center and right lanes are found. + * of center lane (analog: right half of image for right lane). + * + * Lane marking points in range of other lane marking points are supporters because they form a line. + * The x-value of found lane points with maximum supporters will be used. This ensures that non-lane + * points are ignored. + * + *Start position of left lane is calculated after start positions of center and right lanes are found. */ void cLaneDetectionFu::findLanePositions(vector> &laneMarkings) { // defaultXLeft is calculated after center and right lane position is found @@ -848,6 +850,10 @@ bool cLaneDetectionFu::ransacInternal(ePosition position, return true; } +/** + * Shifts detected lane polynomials to the position of the undected lane polyominals, + * so they can be used in the next cycle to group the lane markings + */ void cLaneDetectionFu::generateMovedPolynomials() { movedPolyLeft.clear(); movedPolyCenter.clear(); @@ -1033,19 +1039,23 @@ void cLaneDetectionFu::pubAngle() { -void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, FuPoint &origin) { +void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, FuPoint &origin) +{ shiftPoint(p, m, offset, origin.getX(), origin.getY()); } /** + * Shifts a point by a given offset along the given gradient. + * The sign of the offset dictates the direction of the shift relative to the offset * - * @param p - * @param m + * @param p the shifted point + * @param m the gradient of the polynomial at x * @param offset Positive if shifting to the left, negative to the right - * @param y - * @param x + * @param x the x coordinate of the original point + * @param y the y coordinate of the original point */ -void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, int x, int y) { +void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, int x, int y) +{ /* * Depending on the sign of the gradient of the poly at the different * x-values and depending on which position we are, we have to add or @@ -1266,7 +1276,6 @@ bool cLaneDetectionFu::isSimilar(const NewtonPolynomial &poly1, const NewtonPoly */ - void cLaneDetectionFu::drawEdgeWindow(Mat &img, vector> edges) { if (!SHOW_EDGE_WINDOW) { return; @@ -1689,7 +1698,7 @@ double cLaneDetectionFu::nextGradient(double x, NewtonPolynomial &poly1, * * @param m1 The first gradient * @param m2 The second gradient - * @return True, if the diffenence between the gradients is less than 10° + * @return True, if the diffenence between the gradients is less than 10 degrees */ bool cLaneDetectionFu::gradientsSimilar(double &m1, double &m2) { double a1 = atan(m1) * 180 / PI; @@ -1720,4 +1729,4 @@ ePosition cLaneDetectionFu::maxProportion() { } return maxPos; -} \ No newline at end of file +} diff --git a/src/line_detection_fu/src/laneDetection.h b/src/line_detection_fu/src/laneDetection.h index 989c634c..25275d14 100644 --- a/src/line_detection_fu/src/laneDetection.h +++ b/src/line_detection_fu/src/laneDetection.h @@ -231,10 +231,18 @@ class cLaneDetectionFu { NewtonPolynomial prevPolyCenter; NewtonPolynomial prevPolyRight; + /** + * The polynomials not detected on the current picture and generated by + * shifting the detected polynomials accordingly + */ NewtonPolynomial movedPolyLeft; NewtonPolynomial movedPolyCenter; NewtonPolynomial movedPolyRight; + /** + * Booleans to denote whether a polynomial wasn't detected and had to be + * shifted to the appropriate location + */ bool isPolyMovedRight = false; bool isPolyMovedCenter = false; bool isPolyMovedLeft = false; @@ -255,8 +263,15 @@ class cLaneDetectionFu { double adjacentLeg; double gradientForAngle; + /** + * The default width between two lanemarkings + */ double laneWidth = 45.f; + /** + * Debugging methods for cleaner code in the ProcessInput() function + */ + void drawEdgeWindow(cv::Mat &img, std::vector> edges); void drawLaneMarkingsWindow(cv::Mat &img, std::vector> &laneMarkings); From 772272a6b53e57851a42fab052d56d2b308ef442 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Sat, 16 Sep 2017 18:33:14 +0200 Subject: [PATCH 61/79] reverted back node initialization --- src/line_detection_fu/src/laneDetection.cpp | 31 +++++++++++---------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index b51bedab..027d5d3b 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -131,32 +131,35 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") lastAngle = 0; + head_time_stamp = ros::Time::now(); + read_images_ = nh.subscribe(nh_.resolveName(camera_name), MY_ROS_QUEUE_SIZE, &cLaneDetectionFu::ProcessInput, this); + //publish_curvature = nh.advertise("/lane_model/curvature", MY_ROS_QUEUE_SIZE); publish_angle = nh.advertise("/lane_model/angle", MY_ROS_QUEUE_SIZE); - //from camera properties and ROI etc we get scanlines (=line segments, úseÄky) - //these line segments are lines in image on whose we look for edges - //the outer vector represents rows on image, inner vector is vector of line segments of one row, usualy just one line segment - //we should generate this only once in the beginning! or even just have it pregenerated for our cam - scanlines = getScanlines(); + image_transport::ImageTransport image_transport(nh); - head_time_stamp = ros::Time::now(); + image_publisher = image_transport.advertiseCamera("/lane_model/lane_model_image", MY_ROS_QUEUE_SIZE); if (PUBLISH_IMAGES) { - image_transport::ImageTransport image_transport(nh); - - image_publisher = image_transport.advertiseCamera("/lane_model/lane_model_image", MY_ROS_QUEUE_SIZE); image_publisher_ransac = image_transport.advertiseCamera("/lane_model/ransac", MY_ROS_QUEUE_SIZE); image_publisher_lane_markings = image_transport.advertiseCamera("/lane_model/lane_markings", MY_ROS_QUEUE_SIZE); + } - if (!rgb_camera_info) { - rgb_camera_info.reset(new sensor_msgs::CameraInfo()); - rgb_camera_info->width = projImageW; - rgb_camera_info->height = projImageH + 50; - } + + if (!rgb_camera_info) { + rgb_camera_info.reset(new sensor_msgs::CameraInfo()); + rgb_camera_info->width = projImageW; + rgb_camera_info->height = projImageH + 50; } + //from camera properties and ROI etc we get scanlines (=line segments, úseÄky) + //these line segments are lines in image on whose we look for edges + //the outer vector represents rows on image, inner vector is vector of line segments of one row, usualy just one line segment + //we should generate this only once in the beginning! or even just have it pregenerated for our cam + scanlines = getScanlines(); + if (SAVE_FRAME_IMAGES) { mkdir("groupedLaneMarkings", S_IRWXU); mkdir("ransac", S_IRWXU); From 9903331b1d9a415a8a50a1d4420f4f43ab5e442a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Sun, 17 Sep 2017 13:59:14 +0200 Subject: [PATCH 62/79] improved parameter naming --- src/line_detection_fu/src/laneDetection.cpp | 76 ++++++++++----------- src/line_detection_fu/src/laneDetection.h | 20 ++---- 2 files changed, 44 insertions(+), 52 deletions(-) diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index 027d5d3b..b9f37db1 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -15,13 +15,13 @@ const bool SHOW_RANSAC_WINDOW = true; const bool SHOW_ANGLE_WINDOW = true; // try kernel width 5 for now -const static int g_kernel1DWidth = 5; +const static int kernel1DWidth = 5; // params for pubRGBImageMsg -unsigned int head_sequence_id = 0; -ros::Time head_time_stamp; -std::string rgb_frame_id = "_rgb_optical_frame"; -sensor_msgs::CameraInfoPtr rgb_camera_info; +unsigned int headSequenceId = 0; +ros::Time headTimeStamp; +std::string rgbFrameId = "_rgb_optical_frame"; +sensor_msgs::CameraInfoPtr rgbCameraInfo; // frame number for saving image files std::size_t frame = 0; @@ -34,10 +34,10 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") ROS_ERROR("Node name: %s", node_name.c_str()); - priv_nh_.param(node_name + "/camera_name", camera_name, "/usb_cam/image_raw"); + priv_nh_.param(node_name + "/camera_name", cameraName, "/usb_cam/image_raw"); - priv_nh_.param(node_name + "/camW", cam_w, 640); - priv_nh_.param(node_name + "/camH", cam_h, 480); + priv_nh_.param(node_name + "/camW", camW, 640); + priv_nh_.param(node_name + "/camH", camH, 480); priv_nh_.param(node_name + "/projYStart", projYStart, 60); priv_nh_.param(node_name + "/projImageH", projImageH, 100); priv_nh_.param(node_name + "/projImageW", projImageW, 150); @@ -77,7 +77,7 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") double c_v; double camDeg; double camHeight; - int cam_h_half = cam_h / 2; + int camHHalf = camH / 2; priv_nh_.param(node_name + "/f_u", f_u, 624.650635); priv_nh_.param(node_name + "/f_v", f_v, 626.987244); @@ -86,7 +86,7 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") priv_nh_.param(node_name + "/camDeg", camDeg, 13.0); priv_nh_.param(node_name + "/camHeight", camHeight, 36.2); - ipMapper = IPMapper(cam_w, cam_h_half, f_u, f_v, c_u, c_v, camDeg, camHeight); + ipMapper = IPMapper(camW, camHHalf, f_u, f_v, c_u, c_v, camDeg, camHeight); proj_image_w_half = projImageW / 2; @@ -131,27 +131,25 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") lastAngle = 0; - head_time_stamp = ros::Time::now(); + headTimeStamp = ros::Time::now(); - read_images_ = nh.subscribe(nh_.resolveName(camera_name), MY_ROS_QUEUE_SIZE, &cLaneDetectionFu::ProcessInput, this); + read_images_ = nh.subscribe(nh_.resolveName(cameraName), MY_ROS_QUEUE_SIZE, &cLaneDetectionFu::ProcessInput, this); - //publish_curvature = nh.advertise("/lane_model/curvature", MY_ROS_QUEUE_SIZE); - publish_angle = nh.advertise("/lane_model/angle", MY_ROS_QUEUE_SIZE); + publishAngle = nh.advertise("/lane_model/angle", MY_ROS_QUEUE_SIZE); image_transport::ImageTransport image_transport(nh); - image_publisher = image_transport.advertiseCamera("/lane_model/lane_model_image", MY_ROS_QUEUE_SIZE); + imagePublisher = image_transport.advertiseCamera("/lane_model/lane_model_image", MY_ROS_QUEUE_SIZE); if (PUBLISH_IMAGES) { - image_publisher_ransac = image_transport.advertiseCamera("/lane_model/ransac", MY_ROS_QUEUE_SIZE); - image_publisher_lane_markings = image_transport.advertiseCamera("/lane_model/lane_markings", MY_ROS_QUEUE_SIZE); + imagePublisherRansac = image_transport.advertiseCamera("/lane_model/ransac", MY_ROS_QUEUE_SIZE); + imagePublisherLaneMarkings = image_transport.advertiseCamera("/lane_model/lane_markings", MY_ROS_QUEUE_SIZE); } - - if (!rgb_camera_info) { - rgb_camera_info.reset(new sensor_msgs::CameraInfo()); - rgb_camera_info->width = projImageW; - rgb_camera_info->height = projImageH + 50; + if (!rgbCameraInfo) { + rgbCameraInfo.reset(new sensor_msgs::CameraInfo()); + rgbCameraInfo->width = projImageW; + rgbCameraInfo->height = projImageH + 50; } //from camera properties and ROI etc we get scanlines (=line segments, úseÄky) @@ -188,11 +186,11 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr &msg) { cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8); cv::Mat image = cv_ptr->image.clone(); - Mat cut_image = image(cv::Rect(0, cam_h * 0.25f, cam_w, cam_h * 0.75f)); + Mat cutImage = image(cv::Rect(0, camH * 0.25f, camW, camH * 0.75f)); - Mat remapped_image = ipMapper.remap(cut_image); + Mat remappedImage = ipMapper.remap(cutImage); - cv::Mat transformedImage = remapped_image(cv::Rect((cam_w / 2) - proj_image_w_half + projImageHorizontalOffset, + cv::Mat transformedImage = remappedImage(cv::Rect((camW / 2) - proj_image_w_half + projImageHorizontalOffset, projYStart, projImageW, projImageH)).clone(); cv::flip(transformedImage, transformedImage, 0); @@ -314,7 +312,7 @@ vector> cLaneDetectionFu::scanImage(cv::Mat image) { int end = segment.getEnd().getX(); // walk along segment - for (int i = start; i < end - g_kernel1DWidth; i++) { + for (int i = start; i < end - kernel1DWidth; i++) { int sum = 0; //cv::Mat uses ROW-major system -> .at(y,x) @@ -339,11 +337,11 @@ vector> cLaneDetectionFu::scanImage(cv::Mat image) { // +4 because of sobel weighting - sum = sum / (3 * g_kernel1DWidth + 4); + sum = sum / (3 * kernel1DWidth + 4); //ROS_INFO_STREAM(sum << " is kernel sum."); if (std::abs(sum) > gradientThreshold) { // set scanlineVals at center of kernel - scanlineVals[i + g_kernel1DWidth / 2] = sum; + scanlineVals[i + kernel1DWidth / 2] = sum; } } } @@ -1031,7 +1029,7 @@ void cLaneDetectionFu::pubAngle() { angleMsg.data = result; - publish_angle.publish(angleMsg); + publishAngle.publish(angleMsg); } @@ -1377,7 +1375,7 @@ void cLaneDetectionFu::drawGroupedLaneMarkingsWindow(Mat &img) { cv::line(transformedImagePaintable, p4, p1, cv::Scalar(0, 200, 0)); if (PUBLISH_IMAGES) { - pubRGBImageMsg(transformedImagePaintable, image_publisher_lane_markings); + pubRGBImageMsg(transformedImagePaintable, imagePublisherLaneMarkings); } @@ -1411,7 +1409,7 @@ void cLaneDetectionFu::drawRansacWindow(cv::Mat &img) { if (PUBLISH_IMAGES) { - pubRGBImageMsg(transformedImagePaintableRansac, image_publisher_ransac); + pubRGBImageMsg(transformedImagePaintableRansac, imagePublisherRansac); } if (SHOW_RANSAC_WINDOW) { @@ -1503,7 +1501,7 @@ void cLaneDetectionFu::drawAngleWindow(Mat &img) { } if (PUBLISH_IMAGES) { - pubRGBImageMsg(transformedImagePaintableLaneModel, image_publisher); + pubRGBImageMsg(transformedImagePaintableLaneModel, imagePublisher); } if (SHOW_ANGLE_WINDOW) { @@ -1516,9 +1514,9 @@ void cLaneDetectionFu::drawAngleWindow(Mat &img) { void cLaneDetectionFu::pubRGBImageMsg(cv::Mat &rgb_mat, image_transport::CameraPublisher publisher) { sensor_msgs::ImagePtr rgb_img(new sensor_msgs::Image); - rgb_img->header.seq = head_sequence_id; - rgb_img->header.stamp = head_time_stamp; - rgb_img->header.frame_id = rgb_frame_id; + rgb_img->header.seq = headSequenceId; + rgb_img->header.stamp = headTimeStamp; + rgb_img->header.frame_id = rgbFrameId; rgb_img->width = rgb_mat.cols; rgb_img->height = rgb_mat.rows; @@ -1532,11 +1530,11 @@ void cLaneDetectionFu::pubRGBImageMsg(cv::Mat &rgb_mat, image_transport::CameraP rgb_img->data.resize(size); memcpy(&(rgb_img->data[0]), rgb_mat.data, size); - rgb_camera_info->header.frame_id = rgb_frame_id; - rgb_camera_info->header.stamp = head_time_stamp; - rgb_camera_info->header.seq = head_sequence_id; + rgbCameraInfo->header.frame_id = rgbFrameId; + rgbCameraInfo->header.stamp = headTimeStamp; + rgbCameraInfo->header.seq = headSequenceId; - publisher.publish(rgb_img, rgb_camera_info); + publisher.publish(rgb_img, rgbCameraInfo); } void cLaneDetectionFu::debugPaintPolynom(cv::Mat &m, cv::Scalar color, NewtonPolynomial &p, int start, int end) { diff --git a/src/line_detection_fu/src/laneDetection.h b/src/line_detection_fu/src/laneDetection.h index 25275d14..92add3ca 100644 --- a/src/line_detection_fu/src/laneDetection.h +++ b/src/line_detection_fu/src/laneDetection.h @@ -62,10 +62,6 @@ THIS SOFTWARE IS PROVIDED BY AUDI AG AND CONTRIBUTORS �AS IS� AND ANY EXPRES #include #include - -using namespace std; - - class cLaneDetectionFu { private: @@ -79,20 +75,18 @@ class cLaneDetectionFu { ros::Subscriber read_images_; // publishers - //ros::Publisher publish_images; - //ros::Publisher publish_curvature; - ros::Publisher publish_angle; + ros::Publisher publishAngle; IPMapper ipMapper; - std::string camera_name; + std::string cameraName; - image_transport::CameraPublisher image_publisher; - image_transport::CameraPublisher image_publisher_ransac; - image_transport::CameraPublisher image_publisher_lane_markings; + image_transport::CameraPublisher imagePublisher; + image_transport::CameraPublisher imagePublisherRansac; + image_transport::CameraPublisher imagePublisherLaneMarkings; - int cam_w; - int cam_h; + int camW; + int camH; int projYStart; int projImageH; int projImageW; From 43f8f2ae54a11b928832f72891cd786e682e12b5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Sun, 17 Sep 2017 15:41:59 +0200 Subject: [PATCH 63/79] added comments --- src/line_detection_fu/src/laneDetection.cpp | 29 ++-- src/line_detection_fu/src/laneDetection.h | 150 ++++++++++---------- 2 files changed, 91 insertions(+), 88 deletions(-) diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index b9f37db1..fc2ae573 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -4,6 +4,7 @@ using namespace std; // publish ransac and grouped lane frames to show it in rviz const bool PUBLISH_IMAGES = true; + // save frames as images in ~/.ros/ const bool SAVE_FRAME_IMAGES = true; @@ -70,7 +71,6 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") priv_nh_.param(node_name + "/polyY2", polyY2, 50); priv_nh_.param(node_name + "/polyY3", polyY3, 70); - double f_u; double f_v; double c_u; @@ -189,33 +189,34 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr &msg) { Mat cutImage = image(cv::Rect(0, camH * 0.25f, camW, camH * 0.75f)); Mat remappedImage = ipMapper.remap(cutImage); - cv::Mat transformedImage = remappedImage(cv::Rect((camW / 2) - proj_image_w_half + projImageHorizontalOffset, projYStart, projImageW, projImageH)).clone(); cv::flip(transformedImage, transformedImage, 0); - //scanlines -> edges (in each scanline we find maximum and minimum of kernel fn ~= where the edge is) - //this is where we use input image! + // scanlines -> edges (in each scanline we find maximum and minimum of kernel fn ~= where the edge is) + // this is where we use input image! vector> edges = cLaneDetectionFu::scanImage(transformedImage); - - cv::Mat transformedImagePaintable; drawEdgeWindow(transformedImage, edges); - //edges -> lane markings + // edges -> lane markings vector> laneMarkings = cLaneDetectionFu::extractLaneMarkings(edges); drawLaneMarkingsWindow(transformedImage, laneMarkings); + // initialize defaultXLeft, defaultXCenter and defaultXRight values findLanePositions(laneMarkings); - // start actual execution + // assign lane markings to lanes buildLaneMarkingsLists(laneMarkings); drawGroupedLaneMarkingsWindow(transformedImage); + // try to fit a polynomial for each lane ransac(); + // generate new polynomials based on polynomials found in ransac for lanes without ransac polynomial generateMovedPolynomials(); drawRansacWindow(transformedImage); + // calculate and publish the angle the car should drive pubAngle(); drawAngleWindow(transformedImage); } @@ -853,7 +854,7 @@ bool cLaneDetectionFu::ransacInternal(ePosition position, /** * Shifts detected lane polynomials to the position of the undected lane polyominals, - * so they can be used in the next cycle to group the lane markings + * so they can be used in the next cycle to group the lane markings. */ void cLaneDetectionFu::generateMovedPolynomials() { movedPolyLeft.clear(); @@ -984,6 +985,12 @@ void cLaneDetectionFu::generateMovedPolynomials() { } } +/** + * Calculates the angle the car should drive to. Uses a point of the right (shifted) polynomial + * at distance of angleAdjacentLeg away from the car and shifts it to the center between the right + * and center lane in direction of the normal vector. The angle between the shifted point and the + * car is published. + */ void cLaneDetectionFu::pubAngle() { if (!polyDetectedRight && !isPolyMovedRight) { return; @@ -1009,8 +1016,8 @@ void cLaneDetectionFu::pubAngle() { gradientForAngle = m; - oppositeLeg = movedPointForAngle.getX() - proj_image_w_half; - adjacentLeg = projImageH - movedPointForAngle.getY(); + double oppositeLeg = movedPointForAngle.getX() - proj_image_w_half; + double adjacentLeg = projImageH - movedPointForAngle.getY(); double result = atan(oppositeLeg / adjacentLeg) * 180 / PI; /* diff --git a/src/line_detection_fu/src/laneDetection.h b/src/line_detection_fu/src/laneDetection.h index 92add3ca..09cc7de7 100644 --- a/src/line_detection_fu/src/laneDetection.h +++ b/src/line_detection_fu/src/laneDetection.h @@ -65,25 +65,9 @@ THIS SOFTWARE IS PROVIDED BY AUDI AG AND CONTRIBUTORS �AS IS� AND ANY EXPRES class cLaneDetectionFu { private: - // the node handle - ros::NodeHandle nh_; - - // Node handle in the private namespace - ros::NodeHandle priv_nh_; - - // subscribers - ros::Subscriber read_images_; - - // publishers - ros::Publisher publishAngle; - - IPMapper ipMapper; - - std::string cameraName; - - image_transport::CameraPublisher imagePublisher; - image_transport::CameraPublisher imagePublisherRansac; - image_transport::CameraPublisher imagePublisherLaneMarkings; + /** + * Ros parameters (values can be changed in launch file) + */ int camW; int camH; @@ -93,13 +77,12 @@ class cLaneDetectionFu { int proj_image_w_half; int projImageHorizontalOffset; int roiTopW; + int roiBottomW; int scanlinesVerticalDistance; int scanlinesMaxCount; - vector>> scanlines; - int gradientThreshold; int nonMaxWidth; @@ -107,29 +90,12 @@ class cLaneDetectionFu { int polyY2; int polyY3; + int laneMarkingSquaredThreshold; - /** - * The lane marking polynomials detected in the current picture. - */ - NewtonPolynomial polyLeft; - NewtonPolynomial polyCenter; - NewtonPolynomial polyRight; - - /** - * Horizontal relative positions of the default lane marking lines. - * - * These lines are situated in a position, where the lane markings of a - * straight lane would show up in the relative coordinate system with the - * car standing in the center of the right lane. - */ - int defaultXLeft = 0; - int defaultXCenter = 0; - int defaultXRight = 0; + int angleAdjacentLeg; + int maxAngleDiff; - /** - * The maximum distance of a point to a polynomial so that it counts as a - * supporter. - */ + // The maximum distance of a point to a polynomial so that it counts as a supporter. int maxDistance; /** @@ -147,21 +113,13 @@ class cLaneDetectionFu { */ int interestDistanceDefault; - /** - * The minimal y of the ROIs. Points with smaller y-Values are not - * used in RANSAC. - */ + // The minimal y of the ROIs. Points with smaller y-Values are not used in RANSAC. int maxYRoi; - /** - * The maximal y of default ROIs. Points with bigger y-Values are not used. - */ + // The maximal y of default ROIs. Points with bigger y-Values are not used. int minYDefaultRoi; - /** - * The maximal y of the polynomial ROIs. Points with bigger y-Values are not - * used. - */ + // The maximal y of the polynomial ROIs. Points with bigger y-Values are not used. int minYPolyRoi; /** @@ -170,13 +128,56 @@ class cLaneDetectionFu { */ double proportionThreshould; + // Number of RANSAC iterations + int iterationsRansac; + + + /* + * Non-Ros-parameter variables: + */ + + + // the node handle + ros::NodeHandle nh_; + + // Node handle in the private namespace + ros::NodeHandle priv_nh_; + + // subscribers + ros::Subscriber read_images_; + + // publishers + ros::Publisher publishAngle; + image_transport::CameraPublisher imagePublisher; + image_transport::CameraPublisher imagePublisherRansac; + image_transport::CameraPublisher imagePublisherLaneMarkings; + + IPMapper ipMapper; + + std::string cameraName; + + // scanelines contain detected edges + vector>> scanlines; + + // The lane marking polynomials detected in the current picture. + NewtonPolynomial polyLeft; + NewtonPolynomial polyCenter; + NewtonPolynomial polyRight; + /** - * Number of RANSAC iterations + * Horizontal relative positions of the default lane marking lines. + + * + * These lines are situated in a position, where the lane markings of a + * straight lane would show up in the relative coordinate system with the + * car standing in the center of the right lane. */ - int iterationsRansac; + int defaultXLeft = 0; + int defaultXCenter = 0; + int defaultXRight = 0; /** - * flags to determine if a valid polynomial was detected in the last frame + * Flags to determine if a valid polynomial was detected in the last frame * and therefore the polynomial ROI should be used or if no polynomial could * be detected and the default ROI is used. */ @@ -185,7 +186,7 @@ class cLaneDetectionFu { bool polyDetectedRight; /** - * pairs for saving the best lane polynomials produced during RANSAC + * Pairs for saving the best lane polynomials produced during RANSAC * * first : current best NewtonPolynomial * second: proportion of supporters of used lane marking points (quality) @@ -204,23 +205,17 @@ class cLaneDetectionFu { std::vector> laneMarkingsRight; std::vector> laneMarkingsNotUsed; - /** - * Newton interpolation data points selected for the best polynomial - */ + // Newton interpolation data points selected for the best polynomial std::vector> pointsLeft; std::vector> pointsCenter; std::vector> pointsRight; - /** - * Vectors containing the supporters of the best polynomial - */ + // Vectors containing the supporters of the best polynomial std::vector> supportersLeft; std::vector> supportersCenter; std::vector> supportersRight; - /** - * The polynomials detected on the previous picture - */ + // The polynomials detected on the previous picture NewtonPolynomial prevPolyLeft; NewtonPolynomial prevPolyCenter; NewtonPolynomial prevPolyRight; @@ -241,27 +236,28 @@ class cLaneDetectionFu { bool isPolyMovedCenter = false; bool isPolyMovedLeft = false; + // Points used to calculate shifted polynomial to right lane vector> movedPointsRight; - int laneMarkingSquaredThreshold; - - int angleAdjacentLeg; - + // Published angle in last frame double lastAngle; - int maxAngleDiff; + // Point on right polynomial (or shifted right polynomial) at x=angleAdjacentLeg + FuPoint pointForAngle; + /* + * Result of moving pointForAngle half of laneWidth in direction of normal vector to left. + * This is the target point where the car will move to. + */ FuPoint movedPointForAngle; - FuPoint pointForAngle; - double oppositeLeg; - double adjacentLeg; + + // Gradient of normal vector of right (shifted) polynomial at x=angleAdjacentLeg double gradientForAngle; - /** - * The default width between two lanemarkings - */ + // The default width between two lanes double laneWidth = 45.f; + /** * Debugging methods for cleaner code in the ProcessInput() function */ From 733c2a0cf3d1e934b1d5b5e00dcd3ce516fdb057 Mon Sep 17 00:00:00 2001 From: lars Date: Sun, 17 Sep 2017 18:24:42 +0200 Subject: [PATCH 64/79] possible fix --- src/line_detection_fu/src/laneDetection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index fc2ae573..7139c7fa 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -35,7 +35,7 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") ROS_ERROR("Node name: %s", node_name.c_str()); - priv_nh_.param(node_name + "/camera_name", cameraName, "/usb_cam/image_raw"); + priv_nh_.param(node_name + "/cameraName", cameraName, "/usb_cam/image_raw"); priv_nh_.param(node_name + "/camW", camW, 640); priv_nh_.param(node_name + "/camH", camH, 480); From 77e5ae56d1985c9e9a0daa7709ccae196b629c0c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Sun, 17 Sep 2017 18:40:15 +0200 Subject: [PATCH 65/79] improved debug window output --- src/line_detection_fu/src/laneDetection.cpp | 64 --------------------- 1 file changed, 64 deletions(-) diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index 7139c7fa..a4ea09f5 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -1299,24 +1299,6 @@ void cLaneDetectionFu::drawEdgeWindow(Mat &img, vector> edges) } } - /*cv::Point2d p1(projImageHalfW-(roiBottomW/2),maxYRoi-1); - cv::Point2d p2(projImageHalfW+(roiBottomW/2),maxYRoi-1); - cv::Point2d p3(projImageHalfW+(roiTopW/2),minYPolyRoi); - cv::Point2d p4(projImageHalfW-(roiTopW/2),minYPolyRoi); - cv::line(transformedImagePaintable,p1,p2,cv::Scalar(0,200,0)); - cv::line(transformedImagePaintable,p2,p3,cv::Scalar(0,200,0)); - cv::line(transformedImagePaintable,p3,p4,cv::Scalar(0,200,0)); - cv::line(transformedImagePaintable,p4,p1,cv::Scalar(0,200,0));*/ - - /*for(int i = 0; i < (int)scanlines.size(); i++) - { - LineSegment scanline = scanlines[i][0]; - cv::Point scanlineStart = cv::Point(scanline.getStart().getX(), scanline.getStart().getY()); - cv::Point scanlineEnd = cv::Point(scanline.getEnd().getX(), scanline.getEnd().getY()); - cv::line(transformedImagePaintable,scanlineStart,scanlineEnd,cv::Scalar(255,0,0)); - }*/ - - cv::namedWindow("ROI, scanlines and edges", WINDOW_NORMAL); cv::imshow("ROI, scanlines and edges", transformedImagePaintable); cv::waitKey(1); @@ -1353,13 +1335,6 @@ void cLaneDetectionFu::drawGroupedLaneMarkingsWindow(Mat &img) { debugPaintPoints(transformedImagePaintable, Scalar(255, 0, 0), laneMarkingsRight); debugPaintPoints(transformedImagePaintable, Scalar(0, 255, 255), laneMarkingsNotUsed); - debugPaintPolynom(transformedImagePaintable, cv::Scalar(0, 255, 255), polyDetectedLeft ? polyLeft : movedPolyLeft, - minYPolyRoi, maxYRoi); - debugPaintPolynom(transformedImagePaintable, cv::Scalar(255, 255, 0), - polyDetectedCenter ? polyCenter : movedPolyCenter, minYPolyRoi, maxYRoi); - debugPaintPolynom(transformedImagePaintable, cv::Scalar(255, 0, 255), - polyDetectedRight ? polyRight : movedPolyRight, minYPolyRoi, maxYRoi); - cv::Point2d p1l(defaultXLeft, minYPolyRoi); cv::Point2d p2l(defaultXLeft, maxYRoi - 1); cv::line(transformedImagePaintable, p1l, p2l, cv::Scalar(0, 0, 255)); @@ -1441,39 +1416,6 @@ void cLaneDetectionFu::drawAngleWindow(Mat &img) { cvtColor(transformedImagePaintableLaneModel, transformedImagePaintableLaneModel, CV_GRAY2BGR); if (polyDetectedRight || isPolyMovedRight) { - /*int r = lanePolynomial.getLastUsedPosition() == LEFT ? 255 : 0; - int g = lanePolynomial.getLastUsedPosition() == CENTER ? 255 : 0; - int b = lanePolynomial.getLastUsedPosition() == RIGHT ? 255 : 0; - - - for(int i = minYPolyRoi; i < maxYRoi; i++) { - cv::Point pointLoc = cv::Point(lanePolynomial.getLanePoly().at(i)+projImageHalfW, i); - cv::circle(transformedImagePaintableLaneModel, pointLoc, 0, cv::Scalar(b,g,r), -1); - } - - std::vector> supps; - switch (lanePolynomial.getLastUsedPosition()) { - case LEFT: - supps = supportersLeft; - break; - case CENTER: - supps = supportersCenter; - break; - case RIGHT: - supps = supportersRight; - break; - default: - ROS_ERROR("No last used position"); - supps = supportersRight; - break; - }; - - for(int i = 0; i < (int)supps.size(); i++) { - FuPoint supp = supps[i]; - cv::Point suppLoc = cv::Point(supp.getX(), supp.getY()); - cv::circle(transformedImagePaintableLaneModel, suppLoc, 1, cv::Scalar(b,g,r), -1); - }*/ - cv::Point pointLoc = cv::Point(proj_image_w_half, projImageH); cv::circle(transformedImagePaintableLaneModel, pointLoc, 2, cv::Scalar(0, 0, 255), -1); @@ -1487,12 +1429,6 @@ void cLaneDetectionFu::drawAngleWindow(Mat &img) { cv::Point targetPoint = cv::Point(movedPointForAngle.getX(), movedPointForAngle.getY()); cv::circle(transformedImagePaintableLaneModel, targetPoint, 2, cv::Scalar(0, 0, 255), -1); - /*cv::Point adjacentLegPoint = cv::Point(projImageHalfW, projImageH - adjacentLeg); - cv::line(transformedImagePaintableLaneModel, pointLoc, adjacentLegPoint, cv::Scalar(255,0,0)); - - cv::Point oppositeLegPoint = cv::Point(projImageHalfW + oppositeLeg, projImageH - adjacentLeg); - cv::line(transformedImagePaintableLaneModel, adjacentLegPoint, oppositeLegPoint, cv::Scalar(0,255,0));*/ - double m = -gradientForAngle; double n = pointForAngle.getY() - m * pointForAngle.getX(); From 645ebcbb9cad150ab00dffb8027fbee95f8ee503 Mon Sep 17 00:00:00 2001 From: lars Date: Mon, 18 Sep 2017 15:10:14 +0200 Subject: [PATCH 66/79] added a function to shift polynomials --- src/line_detection_fu/src/laneDetection.cpp | 139 ++++++++------------ src/line_detection_fu/src/laneDetection.h | 2 + 2 files changed, 60 insertions(+), 81 deletions(-) diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index a4ea09f5..01dd1d4f 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -871,113 +871,61 @@ void cLaneDetectionFu::generateMovedPolynomials() { return; } - FuPoint pointLeft1 = FuPoint(); - FuPoint pointLeft2 = FuPoint(); - FuPoint pointLeft3 = FuPoint(); - - FuPoint pointCenter1 = FuPoint(); - FuPoint pointCenter2 = FuPoint(); - FuPoint pointCenter3 = FuPoint(); - - FuPoint pointRight1 = FuPoint(); - FuPoint pointRight2 = FuPoint(); - FuPoint pointRight3 = FuPoint(); - - double m1 = 0; - double m2 = 0; - double m3 = 0; - NewtonPolynomial usedPoly; + vector> usedPoints; + double offset = 0.f; - /* - * Depending on the sign of the gradient of the poly at the different - * x-values and depending on which position we are, we have to add or - * subtract the expected distance to the respective lane polynomial, to get - * the wanted points. - * - * The calculation is done for the x- and y-components of the points - * separately using the trigonometric ratios of right triangles and the fact - * that arctan of some gradient equals its angle to the x-axis in degree. - */ if (polyDetectedRight && !polyDetectedCenter) { - usedPoly = polyRight; - m1 = gradient(pointsRight.at(0).getY(), pointsRight, usedPoly.getCoefficients()); - m2 = gradient(pointsRight.at(1).getY(), pointsRight, usedPoly.getCoefficients()); - m3 = gradient(pointsRight.at(2).getY(), pointsRight, usedPoly.getCoefficients()); - isPolyMovedCenter = true; - - shiftPoint(pointCenter1, m1, -laneWidth, pointsRight.at(0)); - shiftPoint(pointCenter2, m2, -laneWidth, pointsRight.at(1)); - shiftPoint(pointCenter3, m3, -laneWidth, pointsRight.at(2)); - + shiftPolynomial(polyRight, movedPolyCenter, -laneWidth, pointsRight); if (!polyDetectedLeft) { isPolyMovedLeft = true; - - shiftPoint(pointLeft1, m1, -2 * laneWidth, pointsRight.at(0)); - shiftPoint(pointLeft2, m2, -2 * laneWidth, pointsRight.at(1)); - shiftPoint(pointLeft3, m3, -2 * laneWidth, pointsRight.at(2)); + shiftPolynomial(polyRight, movedPolyLeft, -2 * laneWidth, pointsRight); } } else if (polyDetectedLeft && !polyDetectedCenter) { - usedPoly = polyLeft; - m1 = gradient(pointsLeft.at(0).getY(), pointsLeft, usedPoly.getCoefficients()); - m2 = gradient(pointsLeft.at(1).getY(), pointsLeft, usedPoly.getCoefficients()); - m3 = gradient(pointsLeft.at(2).getY(), pointsLeft, usedPoly.getCoefficients()); - isPolyMovedCenter = true; - - shiftPoint(pointCenter1, m1, laneWidth, pointsLeft.at(0)); - shiftPoint(pointCenter2, m2, laneWidth, pointsLeft.at(1)); - shiftPoint(pointCenter3, m3, laneWidth, pointsLeft.at(2)); + shiftPolynomial(polyLeft, movedPolyCenter, laneWidth, pointsLeft); if (!polyDetectedRight) { isPolyMovedRight = true; + shiftPolynomial(polyLeft, movedPolyRight, 2 * laneWidth, pointsLeft); - shiftPoint(pointRight1, m1, 2 * laneWidth, pointsLeft.at(0)); - shiftPoint(pointRight2, m2, 2 * laneWidth, pointsLeft.at(1)); - shiftPoint(pointRight3, m3, 2 * laneWidth, pointsLeft.at(2)); + usedPoly = polyLeft; + usedPoints = pointsLeft; + offset = 2 * laneWidth; } } else if (polyDetectedCenter) { - usedPoly = polyCenter; - m1 = gradient(pointsCenter.at(0).getY(), pointsCenter, usedPoly.getCoefficients()); - m2 = gradient(pointsCenter.at(1).getY(), pointsCenter, usedPoly.getCoefficients()); - m3 = gradient(pointsCenter.at(2).getY(), pointsCenter, usedPoly.getCoefficients()); - if (!polyDetectedLeft) { isPolyMovedLeft = true; - - shiftPoint(pointLeft1, m1, -laneWidth, pointsCenter.at(0)); - shiftPoint(pointLeft2, m2, -laneWidth, pointsCenter.at(1)); - shiftPoint(pointLeft3, m3, -laneWidth, pointsCenter.at(2)); + shiftPolynomial(polyCenter, movedPolyLeft, -laneWidth, pointsCenter); } - if (!polyDetectedRight) { isPolyMovedRight = true; + shiftPolynomial(polyCenter, movedPolyRight, laneWidth, pointsCenter); + + usedPoly = polyCenter; + usedPoints = pointsCenter; + offset = laneWidth; - shiftPoint(pointRight1, m1, laneWidth, pointsCenter.at(0)); - shiftPoint(pointRight2, m2, laneWidth, pointsCenter.at(1)); - shiftPoint(pointRight3, m3, laneWidth, pointsCenter.at(2)); } } - // create the lane polynomial out of the shifted points + if (isPolyMovedRight) { + FuPoint pointRight1 = FuPoint(); + FuPoint pointRight2 = FuPoint(); + FuPoint pointRight3 = FuPoint(); - if (isPolyMovedLeft) { - movedPolyLeft.addData(pointLeft1); - movedPolyLeft.addData(pointLeft2); - movedPolyLeft.addData(pointLeft3); - } + double m1 = 0; + double m2 = 0; + double m3 = 0; - if (isPolyMovedCenter) { - movedPolyCenter.addData(pointCenter1); - movedPolyCenter.addData(pointCenter2); - movedPolyCenter.addData(pointCenter3); - } + m1 = gradient(usedPoints.at(0).getY(), usedPoints, usedPoly.getCoefficients()); + m2 = gradient(usedPoints.at(1).getY(), usedPoints, usedPoly.getCoefficients()); + m3 = gradient(usedPoints.at(2).getY(), usedPoints, usedPoly.getCoefficients()); - if (isPolyMovedRight) { - movedPolyRight.addData(pointRight1); - movedPolyRight.addData(pointRight2); - movedPolyRight.addData(pointRight3); + shiftPoint(pointRight1, m1, offset, usedPoints.at(0)); + shiftPoint(pointRight2, m2, offset, usedPoints.at(1)); + shiftPoint(pointRight3, m3, offset, usedPoints.at(2)); movedPointsRight.push_back(FuPoint(pointRight1.getY(), pointRight1.getX())); movedPointsRight.push_back(FuPoint(pointRight2.getY(), pointRight2.getX())); @@ -1058,7 +1006,7 @@ void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, F * * @param p the shifted point * @param m the gradient of the polynomial at x - * @param offset Positive if shifting to the left, negative to the right + * @param offset positive if shifting to the left, negative to the right * @param x the x coordinate of the original point * @param y the y coordinate of the original point */ @@ -1083,6 +1031,35 @@ void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, i p.setY(y + offset * cos(atan(-1 / m))); } +/** + * Shifts a polynomial by a given offset along the horizontal axis + * The sign of the offset dictates the direction of the shift relative to the offset + * + * @param f the original polynomial + * @param g the shifted polynomial + * @param offset positive if shifting to the left, negative to the right + * @param f_interpolation the points used for interpolating f + */ +void cLaneDetectionFu::shiftPolynomial(NewtonPolynomial &f, NewtonPolynomial &g, double offset, vector> &f_interpolation) +{ + FuPoint shiftedPoint1; + FuPoint shiftedPoint2; + FuPoint shiftedPoint3; + + double m1 = gradient(f_interpolation.at(0).getY(), f_interpolation, f.getCoefficients()); + double m2 = gradient(f_interpolation.at(1).getY(), f_interpolation, f.getCoefficients()); + double m3 = gradient(f_interpolation.at(2).getY(), f_interpolation, f.getCoefficients()); + + shiftPoint(shiftedPoint1, m1, offset, f_interpolation.at(0)); + shiftPoint(shiftedPoint2, m2, offset, f_interpolation.at(1)); + shiftPoint(shiftedPoint3, m3, offset, f_interpolation.at(2)); + + g.addData(shiftedPoint1); + g.addData(shiftedPoint2); + g.addData(shiftedPoint3); +} + + bool cLaneDetectionFu::isInRange(FuPoint &lanePoint, FuPoint &p) { if (p.getY() < minYDefaultRoi || p.getY() > maxYRoi) { return false; diff --git a/src/line_detection_fu/src/laneDetection.h b/src/line_detection_fu/src/laneDetection.h index 09cc7de7..9dd017c3 100644 --- a/src/line_detection_fu/src/laneDetection.h +++ b/src/line_detection_fu/src/laneDetection.h @@ -304,6 +304,8 @@ class cLaneDetectionFu { void shiftPoint(FuPoint &p, double m, double offset, FuPoint &origin); + void shiftPolynomial(NewtonPolynomial &f, NewtonPolynomial &g, double offset, vector> &f_interpolation); + void generateMovedPolynomials(); bool isInRange(FuPoint &lanePoint, FuPoint &p); From 34b4689f7a3059a49a26259b170c6b36110e2cdb Mon Sep 17 00:00:00 2001 From: lars Date: Mon, 18 Sep 2017 15:27:05 +0200 Subject: [PATCH 67/79] minor code refactoring --- src/line_detection_fu/src/laneDetection.cpp | 45 +++++++-------------- 1 file changed, 14 insertions(+), 31 deletions(-) diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index 01dd1d4f..92db7a92 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -878,10 +878,12 @@ void cLaneDetectionFu::generateMovedPolynomials() { if (polyDetectedRight && !polyDetectedCenter) { isPolyMovedCenter = true; shiftPolynomial(polyRight, movedPolyCenter, -laneWidth, pointsRight); + if (!polyDetectedLeft) { isPolyMovedLeft = true; shiftPolynomial(polyRight, movedPolyLeft, -2 * laneWidth, pointsRight); } + return; } else if (polyDetectedLeft && !polyDetectedCenter) { isPolyMovedCenter = true; shiftPolynomial(polyLeft, movedPolyCenter, laneWidth, pointsLeft); @@ -1085,21 +1087,11 @@ bool cLaneDetectionFu::isInRange(FuPoint &lanePoint, FuPoint &p) { */ int cLaneDetectionFu::horizDistanceToDefaultLine(ePosition &line, FuPoint &p) { double pX = p.getX(); - double distance = 0; - - switch (line) { - case LEFT: - distance = std::abs(pX - defaultXLeft); - break; - case CENTER: - distance = std::abs(pX - defaultXCenter); - break; - case RIGHT: - distance = std::abs(pX - defaultXRight); - break; - } - return distance; + if (LEFT == line) return std::abs(pX - defaultXLeft); + if (CENTER == line) return std::abs(pX - defaultXCenter); + if (RIGHT == line) return std::abs(pX - defaultXRight); + return 0.f; } /** @@ -1577,15 +1569,11 @@ double cLaneDetectionFu::intersection(FuPoint &p, double &m, double x1 = 0; double x2 = 0; - if (dis < 0) { - return -1; - } else if (dis == 0) { - return -b / (2 * a); - } else { - x1 = (-b + std::sqrt(std::pow(b, 2) - (4 * a * c))) / (2 * a); - x2 = (-b - std::sqrt(std::pow(b, 2) - (4 * a * c))) / (2 * a); - } + if (dis < 0) return -1; + if (dis == 0) return -b / (2 * a); + x1 = (-b + std::sqrt(std::pow(b, 2) - (4 * a * c))) / (2 * a); + x2 = (-b - std::sqrt(std::pow(b, 2) - (4 * a * c))) / (2 * a); return fmax(x1, x2); } @@ -1625,11 +1613,7 @@ bool cLaneDetectionFu::gradientsSimilar(double &m1, double &m2) { double a1 = atan(m1) * 180 / PI; double a2 = atan(m2) * 180 / PI; - if (abs(a1 - a2) < 10) { - return true; - } else { - return false; - } + return (abs(a1 - a2) < 10); } /** @@ -1637,17 +1621,16 @@ bool cLaneDetectionFu::gradientsSimilar(double &m1, double &m2) { * @return The position of the best polynomial */ ePosition cLaneDetectionFu::maxProportion() { - ePosition maxPos = LEFT; double maxVal = bestPolyLeft.second; if (bestPolyCenter.second > maxVal) { - maxPos = CENTER; maxVal = bestPolyCenter.second; + return CENTER; } if (bestPolyRight.second > maxVal) { - maxPos = RIGHT; + return RIGHT; } - return maxPos; + return LEFT; } From bfb71b69dcb4e5a80d682bebf93fa2ac6f22299b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Tue, 19 Sep 2017 11:20:41 +0200 Subject: [PATCH 68/79] removed std:: --- src/line_detection_fu/src/laneDetection.cpp | 111 ++++++++++---------- 1 file changed, 54 insertions(+), 57 deletions(-) diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index 92db7a92..663bab28 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -21,21 +21,21 @@ const static int kernel1DWidth = 5; // params for pubRGBImageMsg unsigned int headSequenceId = 0; ros::Time headTimeStamp; -std::string rgbFrameId = "_rgb_optical_frame"; +string rgbFrameId = "_rgb_optical_frame"; sensor_msgs::CameraInfoPtr rgbCameraInfo; // frame number for saving image files -std::size_t frame = 0; +size_t frame = 0; const uint32_t MY_ROS_QUEUE_SIZE = 1; const double PI = 3.14159265; cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") { - std::string node_name = ros::this_node::getName(); + string node_name = ros::this_node::getName(); ROS_ERROR("Node name: %s", node_name.c_str()); - priv_nh_.param(node_name + "/cameraName", cameraName, "/usb_cam/image_raw"); + priv_nh_.param(node_name + "/cameraName", cameraName, "/usb_cam/image_raw"); priv_nh_.param(node_name + "/camW", camW, 640); priv_nh_.param(node_name + "/camH", camH, 480); @@ -94,36 +94,36 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") polyDetectedCenter = false; polyDetectedRight = false; - bestPolyLeft = std::make_pair(NewtonPolynomial(), 0); - bestPolyCenter = std::make_pair(NewtonPolynomial(), 0); - bestPolyRight = std::make_pair(NewtonPolynomial(), 0); + bestPolyLeft = make_pair(NewtonPolynomial(), 0); + bestPolyCenter = make_pair(NewtonPolynomial(), 0); + bestPolyRight = make_pair(NewtonPolynomial(), 0); - laneMarkingsLeft = std::vector>(); - laneMarkingsCenter = std::vector>(); - laneMarkingsRight = std::vector>(); - laneMarkingsNotUsed = std::vector>(); + laneMarkingsLeft = vector>(); + laneMarkingsCenter = vector>(); + laneMarkingsRight = vector>(); + laneMarkingsNotUsed = vector>(); polyLeft = NewtonPolynomial(); polyCenter = NewtonPolynomial(); polyRight = NewtonPolynomial(); - supportersLeft = std::vector>(); - supportersCenter = std::vector>(); - supportersRight = std::vector>(); + supportersLeft = vector>(); + supportersCenter = vector>(); + supportersRight = vector>(); prevPolyLeft = NewtonPolynomial(); prevPolyCenter = NewtonPolynomial(); prevPolyRight = NewtonPolynomial(); - pointsLeft = std::vector>(); - pointsCenter = std::vector>(); - pointsRight = std::vector>(); + pointsLeft = vector>(); + pointsCenter = vector>(); + pointsRight = vector>(); movedPolyLeft = NewtonPolynomial(); movedPolyCenter = NewtonPolynomial(); movedPolyRight = NewtonPolynomial(); - movedPointsRight = std::vector>(); + movedPointsRight = vector>(); movedPointForAngle = FuPoint(); pointForAngle = FuPoint(); @@ -172,9 +172,9 @@ cLaneDetectionFu::~cLaneDetectionFu() { void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr &msg) { // clear some stuff from the last cycle - bestPolyLeft = std::make_pair(NewtonPolynomial(), 0); - bestPolyCenter = std::make_pair(NewtonPolynomial(), 0); - bestPolyRight = std::make_pair(NewtonPolynomial(), 0); + bestPolyLeft = make_pair(NewtonPolynomial(), 0); + bestPolyCenter = make_pair(NewtonPolynomial(), 0); + bestPolyRight = make_pair(NewtonPolynomial(), 0); supportersLeft.clear(); supportersCenter.clear(); @@ -300,7 +300,7 @@ vector> cLaneDetectionFu::scanImage(cv::Mat image) { // walk over all scanlines for (auto scanline : scanlines) { // set all brightness values on scanline to 0; - std::fill(scanlineVals.begin(), scanlineVals.end(), 0); + fill(scanlineVals.begin(), scanlineVals.end(), 0); int offset = 0; if (scanline.size()) { offset = scanline.front().getStart().getY(); @@ -340,7 +340,7 @@ vector> cLaneDetectionFu::scanImage(cv::Mat image) { // +4 because of sobel weighting sum = sum / (3 * kernel1DWidth + 4); //ROS_INFO_STREAM(sum << " is kernel sum."); - if (std::abs(sum) > gradientThreshold) { + if (abs(sum) > gradientThreshold) { // set scanlineVals at center of kernel scanlineVals[i + kernel1DWidth / 2] = sum; } @@ -413,7 +413,7 @@ vector> cLaneDetectionFu::scanImage(cv::Mat image) { scanlineEdgePoints.push_back(EdgePoint(imgPos, relPos, scanlineVals[i])); } } - edgePoints.push_back(std::move(scanlineEdgePoints)); + edgePoints.push_back(move(scanlineEdgePoints)); } // after walking along all scanlines // return edgePoints @@ -424,7 +424,7 @@ vector> cLaneDetectionFu::scanImage(cv::Mat image) { /** * uses Edges to extract lane markings */ -std::vector> cLaneDetectionFu::extractLaneMarkings(const std::vector> &edges) { +vector> cLaneDetectionFu::extractLaneMarkings(const vector> &edges) { vector> result; for (const auto &line : edges) { @@ -446,7 +446,7 @@ std::vector> cLaneDetectionFu::extractLaneMarkings(const std::vecto } // sort the lane marking edge points - std::sort(result.begin(), result.end(), + sort(result.begin(), result.end(), [](FuPoint a, FuPoint b) { return a.getY() > b.getY(); }); @@ -565,7 +565,7 @@ void cLaneDetectionFu::findLanePositions(vector> &laneMarkings) { * * @param laneMarkings a vector containing all detected lane markings */ -void cLaneDetectionFu::buildLaneMarkingsLists(const std::vector> &laneMarkings) { +void cLaneDetectionFu::buildLaneMarkingsLists(const vector> &laneMarkings) { laneMarkingsLeft.clear(); laneMarkingsCenter.clear(); laneMarkingsRight.clear(); @@ -702,10 +702,10 @@ void cLaneDetectionFu::ransac() { * @return true if a polynomial could be detected and false when not */ bool cLaneDetectionFu::ransacInternal(ePosition position, - std::vector> &laneMarkings, - std::pair &bestPoly, NewtonPolynomial &poly, - std::vector> &supporters, NewtonPolynomial &prevPoly, - std::vector> &points) { + vector> &laneMarkings, + pair &bestPoly, NewtonPolynomial &poly, + vector> &supporters, NewtonPolynomial &prevPoly, + vector> &points) { if (laneMarkings.size() < 7) { prevPoly = poly; @@ -713,19 +713,19 @@ bool cLaneDetectionFu::ransacInternal(ePosition position, return false; } - std::vector> tmpSupporters = std::vector>(); + vector> tmpSupporters = vector>(); // vectors for points selected from the bottom, mid and top of the sorted // point vector - std::vector> markings1 = std::vector>(); - std::vector> markings2 = std::vector>(); - std::vector> markings3 = std::vector>(); + vector> markings1 = vector>(); + vector> markings2 = vector>(); + vector> markings3 = vector>(); bool highEnoughY = false; // Points are selected from the bottom, mid and top. The selection regions // are spread apart for better results during RANSAC - for (std::vector>::size_type i = 0; i != laneMarkings.size(); i++) { + for (vector>::size_type i = 0; i != laneMarkings.size(); i++) { if (i < double(laneMarkings.size()) / 7) { markings1.push_back(laneMarkings[i]); } else if (i >= (double(laneMarkings.size()) / 7) * 3 @@ -833,7 +833,7 @@ bool cLaneDetectionFu::ransacInternal(ePosition position, // check if poly is better than bestPoly if (proportion > bestPoly.second) { - bestPoly = std::make_pair(poly, proportion); + bestPoly = make_pair(poly, proportion); supporters = tmpSupporters; points.clear(); @@ -973,7 +973,7 @@ void cLaneDetectionFu::pubAngle() { /* * filter too rash steering angles / jitter in polynomial data */ - if (std::abs(result - lastAngle) > maxAngleDiff) { + if (abs(result - lastAngle) > maxAngleDiff) { if (result - lastAngle > 0) result = lastAngle + maxAngleDiff; else @@ -997,8 +997,7 @@ void cLaneDetectionFu::pubAngle() { -void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, FuPoint &origin) -{ +void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, FuPoint &origin) { shiftPoint(p, m, offset, origin.getX(), origin.getY()); } @@ -1012,8 +1011,7 @@ void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, F * @param x the x coordinate of the original point * @param y the y coordinate of the original point */ -void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, int x, int y) -{ +void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, int x, int y) { /* * Depending on the sign of the gradient of the poly at the different * x-values and depending on which position we are, we have to add or @@ -1042,8 +1040,7 @@ void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, i * @param offset positive if shifting to the left, negative to the right * @param f_interpolation the points used for interpolating f */ -void cLaneDetectionFu::shiftPolynomial(NewtonPolynomial &f, NewtonPolynomial &g, double offset, vector> &f_interpolation) -{ +void cLaneDetectionFu::shiftPolynomial(NewtonPolynomial &f, NewtonPolynomial &g, double offset, vector> &f_interpolation) { FuPoint shiftedPoint1; FuPoint shiftedPoint2; FuPoint shiftedPoint3; @@ -1070,8 +1067,8 @@ bool cLaneDetectionFu::isInRange(FuPoint &lanePoint, FuPoint &p) { return false; } - double distanceX = std::abs(p.getX() - lanePoint.getX()); - double distanceY = std::abs(p.getY() - lanePoint.getY()); + double distanceX = abs(p.getX() - lanePoint.getX()); + double distanceY = abs(p.getY() - lanePoint.getY()); return distanceX < interestDistancePoly && distanceY < interestDistancePoly; } @@ -1088,9 +1085,9 @@ bool cLaneDetectionFu::isInRange(FuPoint &lanePoint, FuPoint &p) { int cLaneDetectionFu::horizDistanceToDefaultLine(ePosition &line, FuPoint &p) { double pX = p.getX(); - if (LEFT == line) return std::abs(pX - defaultXLeft); - if (CENTER == line) return std::abs(pX - defaultXCenter); - if (RIGHT == line) return std::abs(pX - defaultXRight); + if (LEFT == line) return abs(pX - defaultXLeft); + if (CENTER == line) return abs(pX - defaultXCenter); + if (RIGHT == line) return abs(pX - defaultXRight); return 0.f; } @@ -1106,7 +1103,7 @@ int cLaneDetectionFu::horizDistanceToPolynomial(NewtonPolynomial &poly, FuPoint< double pX = p.getX(); double polyX = poly.at(pY); - double distance = std::abs(pX - polyX); + double distance = abs(pX - polyX); return distance; } @@ -1156,7 +1153,7 @@ int cLaneDetectionFu::horizDistance(FuPoint &p1, FuPoint &p2) { double x1 = p1.getX(); double x2 = p2.getX(); - return std::abs(x1 - x2); + return abs(x1 - x2); } /** @@ -1557,7 +1554,7 @@ int main(int argc, char **argv) { * @return The x value of the intersection point of normal and 2nd poly */ double cLaneDetectionFu::intersection(FuPoint &p, double &m, - std::vector> &points, std::vector &coeffs) { + vector> &points, vector &coeffs) { double a = coeffs[2]; double b = coeffs[1] - (coeffs[2] * points[1].getY()) - (coeffs[2] * points[0].getY()) + (1.0 / m); @@ -1565,15 +1562,15 @@ double cLaneDetectionFu::intersection(FuPoint &p, double &m, + (coeffs[2] * points[0].getY() * points[1].getY()) - p.getY() - (p.getX() / m); - double dis = std::pow(b, 2) - (4 * a * c); + double dis = pow(b, 2) - (4 * a * c); double x1 = 0; double x2 = 0; if (dis < 0) return -1; if (dis == 0) return -b / (2 * a); - x1 = (-b + std::sqrt(std::pow(b, 2) - (4 * a * c))) / (2 * a); - x2 = (-b - std::sqrt(std::pow(b, 2) - (4 * a * c))) / (2 * a); + x1 = (-b + sqrt(pow(b, 2) - (4 * a * c))) / (2 * a); + x2 = (-b - sqrt(pow(b, 2) - (4 * a * c))) / (2 * a); return fmax(x1, x2); } @@ -1592,8 +1589,8 @@ double cLaneDetectionFu::intersection(FuPoint &p, double &m, * @return The gradient of the second poly at the intersection point */ double cLaneDetectionFu::nextGradient(double x, NewtonPolynomial &poly1, - std::vector> &points1, std::vector> &points2, - std::vector coeffs1, std::vector coeffs2, double m1) { + vector> &points1, vector> &points2, + vector coeffs1, vector coeffs2, double m1) { FuPoint p = FuPoint(x, poly1.at(x)); double x2 = intersection(p, m1, points2, coeffs2); From e2996755e1704fe973ceabb65f50010f22338e36 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Tue, 19 Sep 2017 11:58:55 +0200 Subject: [PATCH 69/79] changed ROS_ERROR to ROS_INFO --- src/line_detection_fu/launch/line_detection_fu.launch | 2 +- src/line_detection_fu/src/laneDetection.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/line_detection_fu/launch/line_detection_fu.launch b/src/line_detection_fu/launch/line_detection_fu.launch index e765d497..d1447336 100644 --- a/src/line_detection_fu/launch/line_detection_fu.launch +++ b/src/line_detection_fu/launch/line_detection_fu.launch @@ -1,6 +1,6 @@ - + diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index 663bab28..da5ea5ad 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -33,7 +33,7 @@ const double PI = 3.14159265; cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") { string node_name = ros::this_node::getName(); - ROS_ERROR("Node name: %s", node_name.c_str()); + ROS_INFO("Node name: %s", node_name.c_str()); priv_nh_.param(node_name + "/cameraName", cameraName, "/usb_cam/image_raw"); @@ -1476,7 +1476,7 @@ void cLaneDetectionFu::debugWriteImg(cv::Mat &image, string folder) { void cLaneDetectionFu::config_callback(line_detection_fu::LaneDetectionConfig &config, uint32_t level) { - ROS_ERROR("Reconfigure Request"); + ROS_INFO("Reconfigure Request"); interestDistancePoly = config.interestDistancePoly; interestDistanceDefault = config.interestDistanceDefault; From a8f5b59954158cb9bed7d4636ebc85a4b49a2bbb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Tue, 19 Sep 2017 12:16:22 +0200 Subject: [PATCH 70/79] added comments --- src/line_detection_fu/src/laneDetection.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index da5ea5ad..f1bc7de0 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -1058,7 +1058,15 @@ void cLaneDetectionFu::shiftPolynomial(NewtonPolynomial &f, NewtonPolynomial &g, g.addData(shiftedPoint3); } - +/** + * Checks if edge point p is near to lane marking point lanePoint. Enables proper + * assignment of edge points to lanes in first frames when the car starts if car + * starts in front of a turn. + * + * @param lanePoint Point of a lane + * @param p Point to check + * @return True if p is near to lanePoint + */ bool cLaneDetectionFu::isInRange(FuPoint &lanePoint, FuPoint &p) { if (p.getY() < minYDefaultRoi || p.getY() > maxYRoi) { return false; @@ -1218,6 +1226,9 @@ bool cLaneDetectionFu::polyValid(ePosition position, NewtonPolynomial poly, Newt return true; } +/** + * Checks if two polynomials are similar and do not vary too much from each other. + */ bool cLaneDetectionFu::isSimilar(const NewtonPolynomial &poly1, const NewtonPolynomial &poly2) { FuPoint p1 = FuPoint(poly1.at(polyY1), polyY1); FuPoint p2 = FuPoint(poly2.at(polyY1), polyY1); From 031c9a1c67074c416cf5358143ec13d6d733ecae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Tue, 19 Sep 2017 12:34:06 +0200 Subject: [PATCH 71/79] refactoring --- src/line_detection_fu/src/laneDetection.cpp | 16 ++++++++-------- src/line_detection_fu/src/laneDetection.h | 2 +- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index f1bc7de0..07aa2954 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -1038,20 +1038,20 @@ void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, i * @param f the original polynomial * @param g the shifted polynomial * @param offset positive if shifting to the left, negative to the right - * @param f_interpolation the points used for interpolating f + * @param interpolationPoints the points used for interpolating f */ -void cLaneDetectionFu::shiftPolynomial(NewtonPolynomial &f, NewtonPolynomial &g, double offset, vector> &f_interpolation) { +void cLaneDetectionFu::shiftPolynomial(NewtonPolynomial &f, NewtonPolynomial &g, double offset, vector> &interpolationPoints) { FuPoint shiftedPoint1; FuPoint shiftedPoint2; FuPoint shiftedPoint3; - double m1 = gradient(f_interpolation.at(0).getY(), f_interpolation, f.getCoefficients()); - double m2 = gradient(f_interpolation.at(1).getY(), f_interpolation, f.getCoefficients()); - double m3 = gradient(f_interpolation.at(2).getY(), f_interpolation, f.getCoefficients()); + double m1 = gradient(interpolationPoints.at(0).getY(), interpolationPoints, f.getCoefficients()); + double m2 = gradient(interpolationPoints.at(1).getY(), interpolationPoints, f.getCoefficients()); + double m3 = gradient(interpolationPoints.at(2).getY(), interpolationPoints, f.getCoefficients()); - shiftPoint(shiftedPoint1, m1, offset, f_interpolation.at(0)); - shiftPoint(shiftedPoint2, m2, offset, f_interpolation.at(1)); - shiftPoint(shiftedPoint3, m3, offset, f_interpolation.at(2)); + shiftPoint(shiftedPoint1, m1, offset, interpolationPoints.at(0)); + shiftPoint(shiftedPoint2, m2, offset, interpolationPoints.at(1)); + shiftPoint(shiftedPoint3, m3, offset, interpolationPoints.at(2)); g.addData(shiftedPoint1); g.addData(shiftedPoint2); diff --git a/src/line_detection_fu/src/laneDetection.h b/src/line_detection_fu/src/laneDetection.h index 9dd017c3..ec5f76c3 100644 --- a/src/line_detection_fu/src/laneDetection.h +++ b/src/line_detection_fu/src/laneDetection.h @@ -304,7 +304,7 @@ class cLaneDetectionFu { void shiftPoint(FuPoint &p, double m, double offset, FuPoint &origin); - void shiftPolynomial(NewtonPolynomial &f, NewtonPolynomial &g, double offset, vector> &f_interpolation); + void shiftPolynomial(NewtonPolynomial &f, NewtonPolynomial &g, double offset, vector> &interpolationPoints); void generateMovedPolynomials(); From 6b1272e8c93705bebf5f947b69de3e0106f5978e Mon Sep 17 00:00:00 2001 From: lars Date: Tue, 19 Sep 2017 20:45:40 +0200 Subject: [PATCH 72/79] minor tweaking --- src/line_detection_fu/src/laneDetection.cpp | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index 07aa2954..4ffeddb3 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -159,11 +159,16 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") scanlines = getScanlines(); if (SAVE_FRAME_IMAGES) { - mkdir("groupedLaneMarkings", S_IRWXU); - mkdir("ransac", S_IRWXU); + struct stat st; + if (!stat("groupedLaneMarkings",&st)) + system("exec rm -r ./groupedLaneMarkings/*"); + else + mkdir("groupedLaneMarkings", S_IRWXU); - system("exec rm -r ./groupedLaneMarkings/*"); - system("exec rm -r ./ransac/*"); + if (!stat("ransac",&st)) + system("exec rm -r ./ransac/*"); + else + mkdir("ransac", S_IRWXU); } } @@ -1007,7 +1012,7 @@ void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, F * * @param p the shifted point * @param m the gradient of the polynomial at x - * @param offset positive if shifting to the left, negative to the right + * @param offset negative if shifting to the left, positive to the right * @param x the x coordinate of the original point * @param y the y coordinate of the original point */ @@ -1032,12 +1037,12 @@ void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, i } /** - * Shifts a polynomial by a given offset along the horizontal axis + * Shifts a polynomial by a given offset * The sign of the offset dictates the direction of the shift relative to the offset * * @param f the original polynomial * @param g the shifted polynomial - * @param offset positive if shifting to the left, negative to the right + * @param offset negative if shifting to the left, positive to the right * @param interpolationPoints the points used for interpolating f */ void cLaneDetectionFu::shiftPolynomial(NewtonPolynomial &f, NewtonPolynomial &g, double offset, vector> &interpolationPoints) { @@ -1078,7 +1083,7 @@ bool cLaneDetectionFu::isInRange(FuPoint &lanePoint, FuPoint &p) { double distanceX = abs(p.getX() - lanePoint.getX()); double distanceY = abs(p.getY() - lanePoint.getY()); - return distanceX < interestDistancePoly && distanceY < interestDistancePoly; + return ((distanceX < interestDistancePoly) && (distanceY < interestDistancePoly)); } From 1d95e5c5c2738740cbd0902e9cda19e0a32b9254 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Thu, 21 Sep 2017 10:58:05 +0200 Subject: [PATCH 73/79] removed point vectors, now re-usingvalues already stored in NewtonPolynomial class --- src/line_detection_fu/src/laneDetection.cpp | 75 ++++++++----------- src/line_detection_fu/src/laneDetection.h | 7 +- .../src/tools/NewtonPolynomial.cpp | 10 +++ .../src/tools/NewtonPolynomial.h | 3 + 4 files changed, 46 insertions(+), 49 deletions(-) diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index 4ffeddb3..d65f03a2 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -115,15 +115,10 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") prevPolyCenter = NewtonPolynomial(); prevPolyRight = NewtonPolynomial(); - pointsLeft = vector>(); - pointsCenter = vector>(); - pointsRight = vector>(); - movedPolyLeft = NewtonPolynomial(); movedPolyCenter = NewtonPolynomial(); movedPolyRight = NewtonPolynomial(); - movedPointsRight = vector>(); movedPointForAngle = FuPoint(); pointForAngle = FuPoint(); @@ -679,14 +674,13 @@ void cLaneDetectionFu::buildLaneMarkingsLists(const vector> &laneMa */ void cLaneDetectionFu::ransac() { polyDetectedLeft = ransacInternal(LEFT, laneMarkingsLeft, bestPolyLeft, - polyLeft, supportersLeft, prevPolyLeft, pointsLeft); + polyLeft, supportersLeft, prevPolyLeft); polyDetectedCenter = ransacInternal(CENTER, laneMarkingsCenter, - bestPolyCenter, polyCenter, supportersCenter, prevPolyCenter, - pointsCenter); + bestPolyCenter, polyCenter, supportersCenter, prevPolyCenter); polyDetectedRight = ransacInternal(RIGHT, laneMarkingsRight, bestPolyRight, - polyRight, supportersRight, prevPolyRight, pointsRight); + polyRight, supportersRight, prevPolyRight); } /** @@ -702,15 +696,12 @@ void cLaneDetectionFu::ransac() { * polynomial * @param prevPoly A reference to the previous polynomial detected at this * position - * @param points A reference to the points selected for interpolating the - * present best polynomial * @return true if a polynomial could be detected and false when not */ bool cLaneDetectionFu::ransacInternal(ePosition position, vector> &laneMarkings, pair &bestPoly, NewtonPolynomial &poly, - vector> &supporters, NewtonPolynomial &prevPoly, - vector> &points) { + vector> &supporters, NewtonPolynomial &prevPoly) { if (laneMarkings.size() < 7) { prevPoly = poly; @@ -840,11 +831,6 @@ bool cLaneDetectionFu::ransacInternal(ePosition position, if (proportion > bestPoly.second) { bestPoly = make_pair(poly, proportion); supporters = tmpSupporters; - - points.clear(); - points.push_back(p1); - points.push_back(p2); - points.push_back(p3); } } @@ -865,7 +851,6 @@ void cLaneDetectionFu::generateMovedPolynomials() { movedPolyLeft.clear(); movedPolyCenter.clear(); movedPolyRight.clear(); - movedPointsRight.clear(); isPolyMovedLeft = false; isPolyMovedCenter = false; @@ -882,42 +867,42 @@ void cLaneDetectionFu::generateMovedPolynomials() { if (polyDetectedRight && !polyDetectedCenter) { isPolyMovedCenter = true; - shiftPolynomial(polyRight, movedPolyCenter, -laneWidth, pointsRight); + shiftPolynomial(polyRight, movedPolyCenter, -laneWidth); if (!polyDetectedLeft) { isPolyMovedLeft = true; - shiftPolynomial(polyRight, movedPolyLeft, -2 * laneWidth, pointsRight); + shiftPolynomial(polyRight, movedPolyLeft, -2 * laneWidth); } return; } else if (polyDetectedLeft && !polyDetectedCenter) { isPolyMovedCenter = true; - shiftPolynomial(polyLeft, movedPolyCenter, laneWidth, pointsLeft); + shiftPolynomial(polyLeft, movedPolyCenter, laneWidth); if (!polyDetectedRight) { isPolyMovedRight = true; - shiftPolynomial(polyLeft, movedPolyRight, 2 * laneWidth, pointsLeft); + shiftPolynomial(polyLeft, movedPolyRight, 2 * laneWidth); - usedPoly = polyLeft; + /*usedPoly = polyLeft; usedPoints = pointsLeft; - offset = 2 * laneWidth; + offset = 2 * laneWidth;*/ } } else if (polyDetectedCenter) { if (!polyDetectedLeft) { isPolyMovedLeft = true; - shiftPolynomial(polyCenter, movedPolyLeft, -laneWidth, pointsCenter); + shiftPolynomial(polyCenter, movedPolyLeft, -laneWidth); } if (!polyDetectedRight) { isPolyMovedRight = true; - shiftPolynomial(polyCenter, movedPolyRight, laneWidth, pointsCenter); + shiftPolynomial(polyCenter, movedPolyRight, laneWidth); - usedPoly = polyCenter; + /*usedPoly = polyCenter; usedPoints = pointsCenter; - offset = laneWidth; + offset = laneWidth;*/ } } - if (isPolyMovedRight) { + /*if (isPolyMovedRight) { FuPoint pointRight1 = FuPoint(); FuPoint pointRight2 = FuPoint(); FuPoint pointRight3 = FuPoint(); @@ -937,7 +922,7 @@ void cLaneDetectionFu::generateMovedPolynomials() { movedPointsRight.push_back(FuPoint(pointRight1.getY(), pointRight1.getX())); movedPointsRight.push_back(FuPoint(pointRight2.getY(), pointRight2.getX())); movedPointsRight.push_back(FuPoint(pointRight3.getY(), pointRight3.getX())); - } + }*/ } /** @@ -957,10 +942,10 @@ void cLaneDetectionFu::pubAngle() { if (polyDetectedRight) { xRightLane = polyRight.at(y); - m = gradient(y, pointsRight, polyRight.getCoefficients()); + m = gradient(y, polyRight.getInterpolationPointY(0), polyRight.getInterpolationPointY(1), polyRight.getCoefficients()); } else { xRightLane = movedPolyRight.at(y); - m = gradient(y, movedPointsRight, movedPolyRight.getCoefficients()); + m = gradient(y, movedPolyRight.getInterpolationPointY(0), movedPolyRight.getInterpolationPointY(1), movedPolyRight.getCoefficients()); } double offset = -1 * laneWidth / 2; @@ -1045,18 +1030,18 @@ void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, i * @param offset negative if shifting to the left, positive to the right * @param interpolationPoints the points used for interpolating f */ -void cLaneDetectionFu::shiftPolynomial(NewtonPolynomial &f, NewtonPolynomial &g, double offset, vector> &interpolationPoints) { +void cLaneDetectionFu::shiftPolynomial(NewtonPolynomial &f, NewtonPolynomial &g, double offset) { FuPoint shiftedPoint1; FuPoint shiftedPoint2; FuPoint shiftedPoint3; - double m1 = gradient(interpolationPoints.at(0).getY(), interpolationPoints, f.getCoefficients()); - double m2 = gradient(interpolationPoints.at(1).getY(), interpolationPoints, f.getCoefficients()); - double m3 = gradient(interpolationPoints.at(2).getY(), interpolationPoints, f.getCoefficients()); + double m1 = gradient(f.getInterpolationPointX(0), f.getInterpolationPointY(0), f.getInterpolationPointY(1), f.getCoefficients()); + double m2 = gradient(f.getInterpolationPointX(1), f.getInterpolationPointY(0), f.getInterpolationPointY(1), f.getCoefficients()); + double m3 = gradient(f.getInterpolationPointX(2), f.getInterpolationPointY(0), f.getInterpolationPointY(1), f.getCoefficients()); - shiftPoint(shiftedPoint1, m1, offset, interpolationPoints.at(0)); - shiftPoint(shiftedPoint2, m2, offset, interpolationPoints.at(1)); - shiftPoint(shiftedPoint3, m3, offset, interpolationPoints.at(2)); + shiftPoint(shiftedPoint1, m1, offset, f.getInterpolationPointX(0), f.getInterpolationPointY(0)); + shiftPoint(shiftedPoint2, m2, offset, f.getInterpolationPointX(1), f.getInterpolationPointY(1)); + shiftPoint(shiftedPoint3, m3, offset, f.getInterpolationPointX(2), f.getInterpolationPointY(2)); g.addData(shiftedPoint1); g.addData(shiftedPoint2); @@ -1184,10 +1169,10 @@ int cLaneDetectionFu::horizDistance(FuPoint &p1, FuPoint &p2) { * @param coeffs The coefficients under usage of the newton basis * @return The gradient of the polynomial at x */ -double cLaneDetectionFu::gradient(double x, vector> &points, vector coeffs) { +double cLaneDetectionFu::gradient(double x, double interpolationPoint0Y, double interpolationPoint1Y, vector coeffs) { return (2 * coeffs[2] * x + coeffs[1]) - - (coeffs[2] * points[1].getY()) - - (coeffs[2] * points[0].getY()); + - (coeffs[2] * interpolationPoint1Y) + - (coeffs[2] * interpolationPoint0Y); } /** @@ -1604,7 +1589,7 @@ double cLaneDetectionFu::intersection(FuPoint &p, double &m, * @param m1 The gradient of the first poly at x * @return The gradient of the second poly at the intersection point */ -double cLaneDetectionFu::nextGradient(double x, NewtonPolynomial &poly1, +/*double cLaneDetectionFu::nextGradient(double x, NewtonPolynomial &poly1, vector> &points1, vector> &points2, vector coeffs1, vector coeffs2, double m1) { @@ -1612,7 +1597,7 @@ double cLaneDetectionFu::nextGradient(double x, NewtonPolynomial &poly1, double x2 = intersection(p, m1, points2, coeffs2); return gradient(x2, points2, coeffs2); -} +}*/ /** * Check two gradients for similarity. Return true if the difference in degree diff --git a/src/line_detection_fu/src/laneDetection.h b/src/line_detection_fu/src/laneDetection.h index ec5f76c3..58d147b3 100644 --- a/src/line_detection_fu/src/laneDetection.h +++ b/src/line_detection_fu/src/laneDetection.h @@ -304,7 +304,7 @@ class cLaneDetectionFu { void shiftPoint(FuPoint &p, double m, double offset, FuPoint &origin); - void shiftPolynomial(NewtonPolynomial &f, NewtonPolynomial &g, double offset, vector> &interpolationPoints); + void shiftPolynomial(NewtonPolynomial &f, NewtonPolynomial &g, double offset); void generateMovedPolynomials(); @@ -323,8 +323,7 @@ class cLaneDetectionFu { bool ransacInternal(ePosition position, std::vector> &laneMarkings, std::pair &bestPoly, NewtonPolynomial &poly, - std::vector> &supporters, NewtonPolynomial &prevPoly, - std::vector> &points); + std::vector> &supporters, NewtonPolynomial &prevPoly); bool polyValid(ePosition, NewtonPolynomial, NewtonPolynomial); @@ -332,7 +331,7 @@ class cLaneDetectionFu { int horizDistance(FuPoint &p1, FuPoint &p2); - double gradient(double, std::vector> &, std::vector); + double gradient(double, double, double, std::vector); double intersection(FuPoint &, double &, std::vector> &, std::vector &); diff --git a/src/line_detection_fu/src/tools/NewtonPolynomial.cpp b/src/line_detection_fu/src/tools/NewtonPolynomial.cpp index eaa699f0..ce376e62 100644 --- a/src/line_detection_fu/src/tools/NewtonPolynomial.cpp +++ b/src/line_detection_fu/src/tools/NewtonPolynomial.cpp @@ -160,6 +160,16 @@ double NewtonPolynomial::at(double x) const return tmp; } +double NewtonPolynomial::getInterpolationPointX(int pointNumber) +{ + return ys[pointNumber]; +} + +double NewtonPolynomial::getInterpolationPointY(int pointNumber) +{ + return xs[pointNumber]; +} + /** * the degree of the polynomial. * A degree of n means that there were n+1 datapoints added. diff --git a/src/line_detection_fu/src/tools/NewtonPolynomial.h b/src/line_detection_fu/src/tools/NewtonPolynomial.h index e96b84cf..e72643ba 100644 --- a/src/line_detection_fu/src/tools/NewtonPolynomial.h +++ b/src/line_detection_fu/src/tools/NewtonPolynomial.h @@ -21,6 +21,9 @@ class NewtonPolynomial { double at(double x) const; + double getInterpolationPointX(int pointNumber); + double getInterpolationPointY(int pointNumber); + int getDegree() const; bool isInitialized() const; std::vector getCoefficients() const; From ba5d728ba0cd9ec40f8a4d783cb482450166132f Mon Sep 17 00:00:00 2001 From: lars Date: Fri, 22 Sep 2017 17:35:03 +0200 Subject: [PATCH 74/79] simplified storage of the data of polnomials --- .../src/tools/NewtonPolynomial.cpp | 21 ++++++++++++------- .../src/tools/NewtonPolynomial.h | 3 ++- 2 files changed, 15 insertions(+), 9 deletions(-) diff --git a/src/line_detection_fu/src/tools/NewtonPolynomial.cpp b/src/line_detection_fu/src/tools/NewtonPolynomial.cpp index ce376e62..6ce54b55 100644 --- a/src/line_detection_fu/src/tools/NewtonPolynomial.cpp +++ b/src/line_detection_fu/src/tools/NewtonPolynomial.cpp @@ -48,15 +48,20 @@ NewtonPolynomial& NewtonPolynomial::addData(double y, double x) int n = ++deg; // resize structures +/* xs.resize(n+1); ys.resize(n+1); +*/ + points.resize(n + 1); dd.resize(n+1, n+1); //TODO initialize table with zeros? // store given data +/* xs[n] = x; ys[n] = y; - +*/ + points[n] = FuPoint(x, y); // insert function output as diffTable[n][n] dd.insert_element(n, n, y); @@ -70,7 +75,7 @@ NewtonPolynomial& NewtonPolynomial::addData(double y, double x) // << " xn: " << xs[n] << " xi: " << xs[i] // << " dd(i+1,n): " << dd(i+1, n) << " dd(i,n-1): " << dd(i, n-1) << std::endl; - tmp = (dd(i+1, n) - dd(i, n-1)) / (xs[n] - xs[i]); + tmp = (dd(i+1, n) - dd(i, n-1)) / (points[n].getX() - points[i].getX()); // coefficient is stored in diffTable(0, n) dd.insert_element(i, n, tmp); @@ -146,7 +151,7 @@ double NewtonPolynomial::at(double x) const double tmp = dd(0, n); // c_n for (int i = 1; i <= n; ++i) { - tmp *= (x - xs[n-i]); + tmp *= (x - points[n-i].getX()); tmp += dd(0, n-i); } @@ -160,14 +165,14 @@ double NewtonPolynomial::at(double x) const return tmp; } -double NewtonPolynomial::getInterpolationPointX(int pointNumber) +double NewtonPolynomial::getInterpolationPointX(int index) { - return ys[pointNumber]; + return points[index].getY(); } -double NewtonPolynomial::getInterpolationPointY(int pointNumber) +double NewtonPolynomial::getInterpolationPointY(int index) { - return xs[pointNumber]; + return points[index].getX(); } /** @@ -207,7 +212,7 @@ std::vector NewtonPolynomial::getCoefficients() const */ NewtonPolynomial& NewtonPolynomial::clear() { - xs.clear(); + points.clear(); dd.clear(); deg = -1; diff --git a/src/line_detection_fu/src/tools/NewtonPolynomial.h b/src/line_detection_fu/src/tools/NewtonPolynomial.h index e72643ba..e06a23e9 100644 --- a/src/line_detection_fu/src/tools/NewtonPolynomial.h +++ b/src/line_detection_fu/src/tools/NewtonPolynomial.h @@ -34,7 +34,8 @@ class NewtonPolynomial { /** * this stores the given data */ - std::vector xs, ys; + //std::vector xs, ys; + std::vector> points; /** * this is a divided-difference table From 732c9adb28e6f975178245ece46dafa3a1bab193 Mon Sep 17 00:00:00 2001 From: lars Date: Sat, 23 Sep 2017 17:19:16 +0200 Subject: [PATCH 75/79] code cleanup --- src/line_detection_fu/src/laneDetection.cpp | 38 +------------------ .../src/tools/NewtonPolynomial.cpp | 8 ---- .../src/tools/NewtonPolynomial.h | 1 - 3 files changed, 2 insertions(+), 45 deletions(-) diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index d65f03a2..5d32fe14 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -861,10 +861,6 @@ void cLaneDetectionFu::generateMovedPolynomials() { return; } - NewtonPolynomial usedPoly; - vector> usedPoints; - double offset = 0.f; - if (polyDetectedRight && !polyDetectedCenter) { isPolyMovedCenter = true; shiftPolynomial(polyRight, movedPolyCenter, -laneWidth); @@ -881,10 +877,6 @@ void cLaneDetectionFu::generateMovedPolynomials() { if (!polyDetectedRight) { isPolyMovedRight = true; shiftPolynomial(polyLeft, movedPolyRight, 2 * laneWidth); - - /*usedPoly = polyLeft; - usedPoints = pointsLeft; - offset = 2 * laneWidth;*/ } } else if (polyDetectedCenter) { if (!polyDetectedLeft) { @@ -894,35 +886,8 @@ void cLaneDetectionFu::generateMovedPolynomials() { if (!polyDetectedRight) { isPolyMovedRight = true; shiftPolynomial(polyCenter, movedPolyRight, laneWidth); - - /*usedPoly = polyCenter; - usedPoints = pointsCenter; - offset = laneWidth;*/ - } } - - /*if (isPolyMovedRight) { - FuPoint pointRight1 = FuPoint(); - FuPoint pointRight2 = FuPoint(); - FuPoint pointRight3 = FuPoint(); - - double m1 = 0; - double m2 = 0; - double m3 = 0; - - m1 = gradient(usedPoints.at(0).getY(), usedPoints, usedPoly.getCoefficients()); - m2 = gradient(usedPoints.at(1).getY(), usedPoints, usedPoly.getCoefficients()); - m3 = gradient(usedPoints.at(2).getY(), usedPoints, usedPoly.getCoefficients()); - - shiftPoint(pointRight1, m1, offset, usedPoints.at(0)); - shiftPoint(pointRight2, m2, offset, usedPoints.at(1)); - shiftPoint(pointRight3, m3, offset, usedPoints.at(2)); - - movedPointsRight.push_back(FuPoint(pointRight1.getY(), pointRight1.getX())); - movedPointsRight.push_back(FuPoint(pointRight2.getY(), pointRight2.getX())); - movedPointsRight.push_back(FuPoint(pointRight3.getY(), pointRight3.getX())); - }*/ } /** @@ -1165,7 +1130,8 @@ int cLaneDetectionFu::horizDistance(FuPoint &p1, FuPoint &p2) { * Applying the given x value then results in the wanted gradient. * * @param x The given x value - * @param points The data points used for interpolating the polynomial + * @param interpolationPoint0Y The first data point used for interpolating the polynomial + * @param interpolationPoint1Y The second data point used for interpolating the polynomial * @param coeffs The coefficients under usage of the newton basis * @return The gradient of the polynomial at x */ diff --git a/src/line_detection_fu/src/tools/NewtonPolynomial.cpp b/src/line_detection_fu/src/tools/NewtonPolynomial.cpp index 6ce54b55..36b7c395 100644 --- a/src/line_detection_fu/src/tools/NewtonPolynomial.cpp +++ b/src/line_detection_fu/src/tools/NewtonPolynomial.cpp @@ -48,19 +48,11 @@ NewtonPolynomial& NewtonPolynomial::addData(double y, double x) int n = ++deg; // resize structures -/* - xs.resize(n+1); - ys.resize(n+1); -*/ points.resize(n + 1); dd.resize(n+1, n+1); //TODO initialize table with zeros? // store given data -/* - xs[n] = x; - ys[n] = y; -*/ points[n] = FuPoint(x, y); // insert function output as diffTable[n][n] dd.insert_element(n, n, y); diff --git a/src/line_detection_fu/src/tools/NewtonPolynomial.h b/src/line_detection_fu/src/tools/NewtonPolynomial.h index e06a23e9..c4bf70f0 100644 --- a/src/line_detection_fu/src/tools/NewtonPolynomial.h +++ b/src/line_detection_fu/src/tools/NewtonPolynomial.h @@ -34,7 +34,6 @@ class NewtonPolynomial { /** * this stores the given data */ - //std::vector xs, ys; std::vector> points; /** From 6510557a36c16d21badcc9630381de91cf8e20ff Mon Sep 17 00:00:00 2001 From: lars Date: Sat, 23 Sep 2017 17:24:30 +0200 Subject: [PATCH 76/79] refactored shiftPolynomial() --- src/line_detection_fu/src/laneDetection.cpp | 20 +++++++------------- 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index 5d32fe14..b724df8f 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -996,21 +996,15 @@ void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, i * @param interpolationPoints the points used for interpolating f */ void cLaneDetectionFu::shiftPolynomial(NewtonPolynomial &f, NewtonPolynomial &g, double offset) { - FuPoint shiftedPoint1; - FuPoint shiftedPoint2; - FuPoint shiftedPoint3; - - double m1 = gradient(f.getInterpolationPointX(0), f.getInterpolationPointY(0), f.getInterpolationPointY(1), f.getCoefficients()); - double m2 = gradient(f.getInterpolationPointX(1), f.getInterpolationPointY(0), f.getInterpolationPointY(1), f.getCoefficients()); - double m3 = gradient(f.getInterpolationPointX(2), f.getInterpolationPointY(0), f.getInterpolationPointY(1), f.getCoefficients()); + FuPoint shiftedPoint; + double m; - shiftPoint(shiftedPoint1, m1, offset, f.getInterpolationPointX(0), f.getInterpolationPointY(0)); - shiftPoint(shiftedPoint2, m2, offset, f.getInterpolationPointX(1), f.getInterpolationPointY(1)); - shiftPoint(shiftedPoint3, m3, offset, f.getInterpolationPointX(2), f.getInterpolationPointY(2)); + for (int i = 0; i < 3; i++) { + m = gradient(f.getInterpolationPointX(i), f.getInterpolationPointY(0), f.getInterpolationPointY(1), f.getCoefficients()); + shiftPoint(shiftedPoint, m, offset, f.getInterpolationPointX(i), f.getInterpolationPointY(i)); + g.addData(shiftedPoint); + } - g.addData(shiftedPoint1); - g.addData(shiftedPoint2); - g.addData(shiftedPoint3); } /** From fcea2a8e48350d9daff839aa46637a54e4581962 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Mon, 25 Sep 2017 16:05:06 +0200 Subject: [PATCH 77/79] refactoring --- src/line_detection_fu/src/laneDetection.cpp | 42 +++++++++------------ src/line_detection_fu/src/laneDetection.h | 12 +----- 2 files changed, 18 insertions(+), 36 deletions(-) diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index b724df8f..43f99ea9 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -88,7 +88,7 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") ipMapper = IPMapper(camW, camHHalf, f_u, f_v, c_u, c_v, camDeg, camHeight); - proj_image_w_half = projImageW / 2; + projImageWHalf = projImageW / 2; polyDetectedLeft = false; polyDetectedCenter = false; @@ -189,7 +189,7 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr &msg) { Mat cutImage = image(cv::Rect(0, camH * 0.25f, camW, camH * 0.75f)); Mat remappedImage = ipMapper.remap(cutImage); - cv::Mat transformedImage = remappedImage(cv::Rect((camW / 2) - proj_image_w_half + projImageHorizontalOffset, + cv::Mat transformedImage = remappedImage(cv::Rect((camW / 2) - projImageWHalf + projImageHorizontalOffset, projYStart, projImageW, projImageH)).clone(); cv::flip(transformedImage, transformedImage, 0); @@ -235,10 +235,10 @@ vector>> cLaneDetectionFu::getScanlines() { vector>> scanlines; vector checkContour; - checkContour.push_back(cv::Point(proj_image_w_half - (roiBottomW / 2), maxYRoi - 1)); - checkContour.push_back(cv::Point(proj_image_w_half + (roiBottomW / 2), maxYRoi - 1)); - checkContour.push_back(cv::Point(proj_image_w_half + (roiTopW / 2), minYPolyRoi)); - checkContour.push_back(cv::Point(proj_image_w_half - (roiTopW / 2), minYPolyRoi)); + checkContour.push_back(cv::Point(projImageWHalf - (roiBottomW / 2), maxYRoi - 1)); + checkContour.push_back(cv::Point(projImageWHalf + (roiBottomW / 2), maxYRoi - 1)); + checkContour.push_back(cv::Point(projImageWHalf + (roiTopW / 2), minYPolyRoi)); + checkContour.push_back(cv::Point(projImageWHalf - (roiTopW / 2), minYPolyRoi)); int scanlineStart = 0; int scanlineEnd = projImageW; @@ -489,7 +489,7 @@ void cLaneDetectionFu::findLanePositions(vector> &laneMarkings) { } bool isSupporter = false; - if (laneMarking->getX() < proj_image_w_half + projImageHorizontalOffset) { + if (laneMarking->getX() < projImageWHalf + projImageHorizontalOffset) { for (int i = 0; i < centerStart.size(); i++) { if (isInRange(*centerStart.at(i), *laneMarking)) { isSupporter = true; @@ -869,7 +869,6 @@ void cLaneDetectionFu::generateMovedPolynomials() { isPolyMovedLeft = true; shiftPolynomial(polyRight, movedPolyLeft, -2 * laneWidth); } - return; } else if (polyDetectedLeft && !polyDetectedCenter) { isPolyMovedCenter = true; shiftPolynomial(polyLeft, movedPolyCenter, laneWidth); @@ -921,7 +920,7 @@ void cLaneDetectionFu::pubAngle() { gradientForAngle = m; - double oppositeLeg = movedPointForAngle.getX() - proj_image_w_half; + double oppositeLeg = movedPointForAngle.getX() - projImageWHalf; double adjacentLeg = projImageH - movedPointForAngle.getY(); double result = atan(oppositeLeg / adjacentLeg) * 180 / PI; @@ -950,12 +949,6 @@ void cLaneDetectionFu::pubAngle() { * Utils: */ - - -void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, FuPoint &origin) { - shiftPoint(p, m, offset, origin.getX(), origin.getY()); -} - /** * Shifts a point by a given offset along the given gradient. * The sign of the offset dictates the direction of the shift relative to the offset @@ -993,7 +986,6 @@ void cLaneDetectionFu::shiftPoint(FuPoint &p, double m, double offset, i * @param f the original polynomial * @param g the shifted polynomial * @param offset negative if shifting to the left, positive to the right - * @param interpolationPoints the points used for interpolating f */ void cLaneDetectionFu::shiftPolynomial(NewtonPolynomial &f, NewtonPolynomial &g, double offset) { FuPoint shiftedPoint; @@ -1274,10 +1266,10 @@ void cLaneDetectionFu::drawGroupedLaneMarkingsWindow(Mat &img) { cv::Point2d p2r(defaultXRight, maxYRoi - 1); cv::line(transformedImagePaintable, p1r, p2r, cv::Scalar(255, 0, 0)); - cv::Point2d p1(proj_image_w_half - (roiBottomW / 2), maxYRoi - 1); - cv::Point2d p2(proj_image_w_half + (roiBottomW / 2), maxYRoi - 1); - cv::Point2d p3(proj_image_w_half + (roiTopW / 2), minYPolyRoi); - cv::Point2d p4(proj_image_w_half - (roiTopW / 2), minYPolyRoi); + cv::Point2d p1(projImageWHalf - (roiBottomW / 2), maxYRoi - 1); + cv::Point2d p2(projImageWHalf + (roiBottomW / 2), maxYRoi - 1); + cv::Point2d p3(projImageWHalf + (roiTopW / 2), minYPolyRoi); + cv::Point2d p4(projImageWHalf - (roiTopW / 2), minYPolyRoi); cv::line(transformedImagePaintable, p1, p2, cv::Scalar(0, 200, 0)); cv::line(transformedImagePaintable, p2, p3, cv::Scalar(0, 200, 0)); cv::line(transformedImagePaintable, p3, p4, cv::Scalar(0, 200, 0)); @@ -1343,10 +1335,10 @@ void cLaneDetectionFu::drawAngleWindow(Mat &img) { cvtColor(transformedImagePaintableLaneModel, transformedImagePaintableLaneModel, CV_GRAY2BGR); if (polyDetectedRight || isPolyMovedRight) { - cv::Point pointLoc = cv::Point(proj_image_w_half, projImageH); + cv::Point pointLoc = cv::Point(projImageWHalf, projImageH); cv::circle(transformedImagePaintableLaneModel, pointLoc, 2, cv::Scalar(0, 0, 255), -1); - cv::Point anglePointLoc = cv::Point(sin(lastAngle * PI / 180) * angleAdjacentLeg + proj_image_w_half, + cv::Point anglePointLoc = cv::Point(sin(lastAngle * PI / 180) * angleAdjacentLeg + projImageWHalf, projImageH - (cos(lastAngle * PI / 180) * angleAdjacentLeg)); cv::line(transformedImagePaintableLaneModel, pointLoc, anglePointLoc, cv::Scalar(255, 255, 255)); @@ -1549,15 +1541,15 @@ double cLaneDetectionFu::intersection(FuPoint &p, double &m, * @param m1 The gradient of the first poly at x * @return The gradient of the second poly at the intersection point */ -/*double cLaneDetectionFu::nextGradient(double x, NewtonPolynomial &poly1, +double cLaneDetectionFu::nextGradient(double x, NewtonPolynomial &poly1, vector> &points1, vector> &points2, vector coeffs1, vector coeffs2, double m1) { FuPoint p = FuPoint(x, poly1.at(x)); double x2 = intersection(p, m1, points2, coeffs2); - return gradient(x2, points2, coeffs2); -}*/ + return gradient(x2, points2.at(0).getY(), points2.at(1).getY(), coeffs2); +} /** * Check two gradients for similarity. Return true if the difference in degree diff --git a/src/line_detection_fu/src/laneDetection.h b/src/line_detection_fu/src/laneDetection.h index 58d147b3..f4131314 100644 --- a/src/line_detection_fu/src/laneDetection.h +++ b/src/line_detection_fu/src/laneDetection.h @@ -74,7 +74,7 @@ class cLaneDetectionFu { int projYStart; int projImageH; int projImageW; - int proj_image_w_half; + int projImageWHalf; int projImageHorizontalOffset; int roiTopW; @@ -205,11 +205,6 @@ class cLaneDetectionFu { std::vector> laneMarkingsRight; std::vector> laneMarkingsNotUsed; - // Newton interpolation data points selected for the best polynomial - std::vector> pointsLeft; - std::vector> pointsCenter; - std::vector> pointsRight; - // Vectors containing the supporters of the best polynomial std::vector> supportersLeft; std::vector> supportersCenter; @@ -236,9 +231,6 @@ class cLaneDetectionFu { bool isPolyMovedCenter = false; bool isPolyMovedLeft = false; - // Points used to calculate shifted polynomial to right lane - vector> movedPointsRight; - // Published angle in last frame double lastAngle; @@ -302,8 +294,6 @@ class cLaneDetectionFu { void shiftPoint(FuPoint &p, double m, double offset, int x, int y); - void shiftPoint(FuPoint &p, double m, double offset, FuPoint &origin); - void shiftPolynomial(NewtonPolynomial &f, NewtonPolynomial &g, double offset); void generateMovedPolynomials(); From 6aa06c58afd47ccd8e7491aedd639ce6fafbb92c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20Sch=C3=BClke?= Date: Mon, 25 Sep 2017 16:17:02 +0200 Subject: [PATCH 78/79] removed callgraph image --- src/line_detection_fu/callgraph.graphml | 757 ------------------------ src/line_detection_fu/callgraph.pdf | Bin 107097 -> 0 bytes 2 files changed, 757 deletions(-) delete mode 100644 src/line_detection_fu/callgraph.graphml delete mode 100644 src/line_detection_fu/callgraph.pdf diff --git a/src/line_detection_fu/callgraph.graphml b/src/line_detection_fu/callgraph.graphml deleted file mode 100644 index 534879f4..00000000 --- a/src/line_detection_fu/callgraph.graphml +++ /dev/null @@ -1,757 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - ProcessInput() - - - - - - - - - - - - - - - - pubRGBImageMsg() - - - - - - - - - - - - - - - - pubAngle() - - - - - - - - - - - - - - - - pubGradientAngle() - - - - - - - - - - - - - - - - getScanlines() - - - - - - - - - - - - - - - - scanImage() - - - - - - - - - - - - - - - - extractLaneMarkings() - - - - - - - - - - - - - - - - buildLaneMarkingsList() - - - - - - - - - - - - - - - - horizDistanceToDefaultLine() - - - - - - - - - - - - - - - - horizDistanceToPolynomial() - - - - - - - - - - - - - - - - isInDefaultRoi() - - - - - - - - - - - - - - - - isInPolyRoi() - - - - - - - - - - - - - - - - detectLane() - - - - - - - - - - - - - - - - - ransac() - - - - - - - - - - - - - - - - - ransacInternal() - - - - - - - - - - - - - - - - - polyValid() - - - - - - - - - - - - - - - - - horizDistance() - - - - - - - - - - - - - - - - - createLanePoly() - - - - - - - - - - - - - - - - - gradient() - - - - - - - - - - - - - - - - - intersection() - - - - - - - - - - - - - - - - - nextGradient() - - - - - - - - - - - - - - - - - gradientsSimilar() - - - - - - - - - - - - - - - - - maxProportion() - - - - - - - - - - - - - - - - - config_callback() - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/line_detection_fu/callgraph.pdf b/src/line_detection_fu/callgraph.pdf deleted file mode 100644 index 1e2e013ff94dce56f89bf4ef59f68e87bb4f5cbb..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 107097 zcma%?+19$uw%*UVioYU?*nxo9qoSZFB4US#qIA+X>e$&=nS`A{{XN*=%4uI0e}Ad84mya^)LTx9o%nV{ei20 z|DOK7?>PIX4f5~hS;qIDwydpx|F<|ZKmW9e$*kx1uhag0@!yg-`~Jb2rab$ zexE_?{QbaoPsjTF!SfULe{VXu*1uWVF0*VavmztkP5kq2F|nel@y&bmMXZEuh-pJk@J23q5eMpuNJaykhk!U+M3GDYSks@ zmJyrzvt)z(o&KL6fA|0M&Bsy*aL*sB8b}evF4+~n^tmMwymmrI0-GFa@%=y{sF&jR zJ@|E$cf`2xLM00ms4hn;1e$cEgDrB=4-MpEL5- zID}!zX_11NxY^YjmF{KNiH=jWP3GW()*P?LDMT_4k z^{&qAXMVcdawK-6<2@4&H&a;(gBbX9`GixxT-o__=XTV;Ma5CJR~DyKnLwkI9Cdwg z<5n>VRnmRK`uL;*v)roK>-OGDA$=uKg>IM`Uk}L6wzI|a7M9wjROa|(b6dXiEAbo1 z3yrZ(*ojl7FR2~b6Im(JH8WX^nV>yg9xAgBF&~!aP{}Cp`mVm}uZ%l|Emhil08E|e zGkoz3vcZ;|_f)f8Gx6P}n3=1FNuD>(9kiYg)EHqYGUxO6pV;H@%2wH^_LHVbHDEA* z{VZL+aAzcRY!_Hrvv9hh(imKl&#xp)hnt7b`D zwcWUuR`xZ^oK{Er8>yiMI7&Fi?du#5Y?(9E&>^R;XKbazK8Q(ir3*}Obda#_%`t1} zDN<5u9T{zKi6GbOy5)k7ORM|y%{0r7cy5&hY6ucL250YLeUyI(g2i;UU!Z zz3ejRPc7c)R@uj{i<>V{N8WrapZ+o*##wQ~^*7$VJQdw$A>r;=4YNOgY{7!H+I~3BS7FLm5E$2eDrDbJI$@V&EhMRbg}62BAX9fE94`ut<@RugOB=M zb3K63j(HC}eJY&i_mvckEGu|ni^ttuDd7T0SEMq2y4HAv)XqaGyR^=rSNsH*ErnFA z!M9spmvrXctEe}Tbq92{ejIMbM5J#K=g0|4Ra3@2ZivK-?{+ykm4JidPxP?7JwVK! zLO;cTm#4uY7JK192$M4u#861OrNZ?wT4?+pJnm%r_aJ-gnF31DcQbnBA6kR*YU4he zZEGZ8p6b!;mvl#8H4Z)wN@uryrvQ9tjbLUz{LHAg3w?^`chDVe8fv@c_dvP-9SS=#Sv!{{ zeqT|Geu;h_i{U%Fyv!8?zbUej9Z?vH0Pk(IwRGUWew4HKs2yR=VF#(P zBWGPP$}Dby?DjI31-PJV1TEY*!`QtKUAysa?9XoO>(&K6GMAbo7-K7X&49tI=9$*( zloXNLZPK*si7u$jbG56iJ)UgUS$}+)xt_LfZovz7ph>%`7voz;Y3$6p{$isl_kjjaYosNAAlJ5@w9%HCaQ{9Vy{_ekY-U_yyiFc-l3PH&tb~^ zHoI2RS!|W4j+_r?Qem~92x2Elf^q|%$!QXfxSX!W>wTp4=QPsQH{_h6;4i(m zegurZiRYV-Nqxe8iWRSKEK6T-a%+GnV3t3}#|w$eJh1|oo%ZaQity;H;ZOdp8R zRYgg%bz6DUbPj4+0Jomo!Vw#TgUOtdSMOIO0GCWD8qojt0z(%*Nu z9Pt`8(AKEie%=w{J+bQ3Z=11^2urGb+?#8<`oP?dnMV0-eawT@!-)jyZGA^Elc}+i~kbB-0Wi;sCJFYWE4d}<(O{=+O z>Yb5!U*-|}yxG|6KO0_*K1Jwt#pd0rh7#MXE*}!S=DzPywtJiR6@PWuZkyZ|_2{JmL{}KY z_O7gft_|b03nX%l(qUB&7Na9Y^M!Nc!$4q z9CY4~yDN2xivYMZj7p_cHol`msT3lm^$_9HBxEee%=mG3b4Z*nPc z7zs6(n_t-cw7~r{`BllN3+=+%?KbUL0NQc|pPbcJGY}0EL@TJQufsK_ zRJMznD-XV2?a>eV&c{ENxzaz;S3Ni&qob%FNYfhmJek6 zIH*a@F~&IewEJ*$gViSO)p$;n`>p!ZUb8>;X!gjC*70sT*csK1nz7!hm7yf|&S7UB z0m5K#F3!fOi7z-j9Jq+7%CpoS(ercgdCY5md)F7(<~A96wB3gI&eqV4z`)_6=0|si z!_6{)_M4W-FiBST8285n#cM#><$6CT8{c6m z4S&OYSY0sdZua;Fbw2-Ig~1_S7etoZbu1aW+#YILIio4bOUDbR3>seoIj?51Rco?TiIh*q}_tU6ELCx&; zPZ3$UGJ|>Y!ilV3j8xr#oHMTsC+k0z2KoR=y1^ zc>ac#ho@C&00zk?j#Deu3~aPLVf{wby4k5j@AoMzNi+k|(R;!lS|eOKAH+ubQu^Bz zdl-w=OO@W$6BC#8#(Cd>t)yj@$375cto>L>5HnHQ!qUI$?b`tXzm9n5gS~C$5M+6L zl_TM#O%UWzMf&<|5=*?|C}-(OEtU?WTf;-^MPqr)a4$Sy)f+2bm_79_M5r@4Ihaoh z@6{asencrp8@2Pg_?=CS?TXXS))<^L57nw#?hihLUGsXPq@^RH18E0ZtkCS;r$8%K zUPQWX-$0P%iYtgL8kf4y{{XlC<`$O zYvwVn;?K=oKk4kHH0yE)1(R(~_mT3T29TKf4x7yvr4duqzS&98?~7BhNn9~jeoPj{ z`z{gEs1(r2`-|_#D?9tX=k@m~F$*dBv9gmAFr()9(oaU~LBIg|0F+mUIn}zgvc;>F z?eMs$BG}89Av=sGzhYsi>LOnEgWGr582a57x;?+{$f=Ir`RbA!Wa_D%Wn1*d z2VlPKN!rFf=Jk{(OD}|I?c%y8L3Qd^X=!r;`<>ps*m^@ST)+4&@^RPqVNGwB-5)eY z6;X^Hm16z%VOeE>{kC6V>P{Bfi(OjpD0T{wX_!drrFdmZeZQudU670BSNh zcorLzW7VqMtQmZq49+LSnWDpO|7sFPF7;ZE(k7wt+s{cLJwq3j;R=AZYPdQ!c(LCd z4I3I7l;`MVg{O_{LUob>F>dp#{qJz(ifmCm>-EBHIUm+%t1D5xMy@?nTw;c+oN@Hk zGh$#HfNG6FJM5SUc_pSAT-gab$ZKh9CRcFE)PXpXjVU_>ufsH`>Gy629KY1I<{E^c z7bXE2O&9pPXFq0>6?-fcN*J)JI{t>YU4_n5QWy<`uTzYUv#lhAD+Jl|U~Dbrh-%lF>PP0SL_Q^hCH}lv$lP#RDvLpp^ znaPj)zIJtlH5BcZGFlaBTc_*E?p?Wc?j+NoRQE74+np|WoLBcy|HWar0bDJWg;-(H zl(pOCM!HH6;M>?O7>-gcf+crVYE~ZFaZ@%rX&tsmst3Qb@Ww9u7TDCe)un@dIFdd? ze*HbGY#e|@YUQq7ju23994f=*y1R7uCVsR-QeGf0HdeWZw31^O8ued8b2!nC)|s=}-cc2Mz9uYo-_sKW4N9t?Qrz zl9NdyXwjxSE14DQ_;8o-|~d>VE4fKg`&<%@}b4qv!_*V^gtjOF?83P;koQbGt?o~-LLGt&)m(E zItuyY()q*s)TLvyW0!2&YcI!wy)Ha3;1JZxuuxj~6{CaYx{s)Aw@tQ(w7gcczhB&b z%v?>)R+((6$bw%s-dcGFo0qM`ut{{hJ&~tskgI)GAq!ihpb5aW+Z`1+AIDqhxzK_( z82h7y#ZYJu~f z0DZ;rir7>nd0pzf1rm>u)+ z)Q=DKJ|5d@lfxg$C3UogK3)O-qFZ>fD{s8XN7?*>w)W1(MIv*UtfPh4lj?Pbh8l5* z_3)%AT)^5^7rE(K@2_4r8(&tor`Jo`Tzo9)Rl4BN0j^SI8cz`|&lfrUwVoe)hXM>q z^Gb2sI*XhzzsRj=^UC#COF=aJuqipv=EF5l^E^38wgh*DPXmFM)hD&flP47*K<0iO zqm!tb+*`w>xq?b9uEDL1d58I4HrRz8_n!7ea5~Hn%3rB5USLM4m(=ghqS0B8*Q$a; zyG_YDoP(0zUjVmy!RC$f>D0ky=5dEQ-eOp7iNS{ygKEK&w`W1uE@brp6`~ulKY z_PF@dD^Ym(EAb;1WKBo>(jIO2sdV1~b)v#WU}1MoHCA~zK726`>p91*!d-`~T-v8) z52ugivnU2(*-T2`w^c1r>=LhCUnGYRlzMC68OR!&`U7f0MRG*e={jQxvfvO&AR<6hQ6B(V%2eG}^M}=q2 z2kzvAp^`WDL!(`G%N0YYdU_QL;2Cel2L}?qSsE1L>S}Kq<6owBy_645g}68FM6<*v zx>oxq4SH$O*!d1(9Nz%AMiM-nu|B@~Q!M5$kn@YjmV3~N04*z6@REW-* zD{jmF`HL6p?G9fP<|>wIQ}B2gl_9HZgT@O7R*!iyKUCz=ZRA-9Mz7_?V^g+wh`(pr za+kMS z5j;q0z<4J$t%bk}XM5@0n`|fH@A2^0JiU|T&4ujsvbp)Yt-Jp(4%voqkM$@Tvz39((|i1d?jq(+4f6xYQvI9kf-sMI;`(NV=|z2 z7g-`YF7pyPqpirs?8R=q47l+ma}DrSpELroxf`3cW`F72s>w6A7N;?aH6tD@)nC0=TshHF5iZp))Axgq>-Q4hZkS-xI*;qexVCd! z^Y1exS4Pz@3;63)x-a45H=nVk)tT9;Bj8Me2f^&yRD0n4lKx@Sf%{E!R48^ZZ-(Ey zjY9hh>%D&XYcy?i;Z*wNi~p13An8UPv>HsJ`IRm;uKg&1OS2sgaChdTDkyfJ^KE=o zT#XObI2?Au1#2d;g^+ulem@oo|8{O=&EQMF8;ACsmgx|8_5ywH+_m{;^FYo+JH;*P z7%&32HC=?H&fpewN;PTU=)|i2a9*8K<#J$8&C!ZheKhE=tooicd9XN&#cM-Z%=+T> zQmKN^ImVIQv^2_$?rk~;D#ma{OTF@Aht}4_TD^>8<;|B05pj7qc4axa6nED?t?tVB ze%_j#G?yl@19}#(Eqa#mhC2z+bD6z0(3|jqbq?6J<>DZBKE@D_O^7rjxbahY9a_kt zb<6s*Zk;9N@W9rJZ>S)p!DR zG%-Ebb}CR(f~NWr~Ufw04CK)|8jtin2@AIp9Iuv5FknKvxHFJ))%ei{_RI9bd7`ncJd7Y zA=j9SS8~=W7n}R?uy+x<=QwDWZ0<4tEpKzJPgrrop1fz(eJoU}kM(dYT*3QBqR-*m zZAAxUq=MB>E4~A8mwYb^N$b`hhg8#W?>{N`2o!3QiXgV0z6uaAEe*pC^(++Dvx||; zAFzrw==x1q_1`{DB7@B-PxrbD1|6UA2k>lLPS0<0ZHjvqUJB(E&Lu(X(C3Gc`!rdC zIB}y{Z^nHu^y=Do_zl!X38!HzrSH*oY>vwPy4IEZs@FU~H$4KDuvC=E{*07e=7FN- zj|#}!sa*V;)^GnANYhVY#6SC!E|mtOUBW#rw9sp3P_ThNo2U2d1*DnOwK6tD*KawU z0L&0;_jISOYAbhrE0s&Hz5i)75trQHVzTWeqiyltXWQl|I^!(JCwymu#?PvLjP+?g z(K2gE>zVI^(G;8CHW&hT20nEu`E&VQIgR>kg_kS@kK$tI*9*TXR>4q_z0;J*TDiSd zj^)u3O7O(5$w}qQwyPu1_>^-HqGN9nZk|1#03A+W-c>i4JumhK{J6C+Dvcn-7f`L= ziODt(Qs*5bzc&44Z!?ltP`7Yfy-$T!6Q8EKD()Eh5I4WvKvr|?K|WaB+E zkp|XT?|aoti=6MG-m+ONs4Jl4*lOohK#r^Dl=+J14Tvqby|3NF2X!?<7sqp2Fe+R{ z?UY-62W!0;ch;DB$BLcP>m7B;n!lIt{#5o;O?W+`>iLJ9oj_-+9t}2!eyz7DT{r}Zq_Q5rg@}a-tST%=Ap2q=1qP6 zn!Wrzh_y#lxntHleCZaM=<{(w=F}DF%fn(TkiKm4~tU{ z<1KbMo#v)#Wk4SkHtc}>VB}|qrLIyRvQ|FHGby8cpJuuR0lhm3{qd?;^uiyr71}>4 zw~S6uK+K+#s8kisV&(aUu04>y(3ZD7zMxjFSDPgDAgjggLR)u{B@Oz0efpdi9Tx-i z85ld){^Z(DCV|&7W3PdfN1swR#;Z`_eIDPfVreS%E#-Y^4@G4-C_;m{e#F?bC+N}d8kVO{aOW`FN=>%sM*}Esy?>c8u%e$ReY>l zq~GzZL@8ZpOgDD8d@5VR0K(0Z5;RL!&I*QagFu!l9DuW~D7oPgsMgH$zP8(Q~5Wk*gB?x+yO^MJ%=4qVG*cUN?SZxsDxtmuO0Op>2d z^@GPkx0Q@Z%!0}1jhtK=9;fmgiyoFY3u(bp#)(SJ`Oupg^}!BQ9sRVKU9|49D{8D? zbZJ>0kw6^2x?Z;^B{K&E5B29FkeSWbXr@goPs104vUc>1QBtqvZnZ>wvyPExZTH_vLhdc|UGDTy8O1hKfJjk_q{m~k_p8_4P zorPO(JFkH&TesQfR}p;QOKCRR*>wUHJ9}!e6IF{m0RBE~K&77Xo{BmF%_0u19jYv| zDeMk&xFh;iU+Gk!S>N);i-B6e?%d%$n|1Wl0SnEozBXc+A=-rXEXYFIcrd_XoljQy z{P%gAI8`m3+x1(uG-ymlRQTbdXl{MNmbW}E+hkOm=Vj_x%gK`T;5@NoyFHnYw#|A$ z#%4_kJ(x{+z5e)EEjVJAqmB|V z&E5s`UA+{4X1#<+%8v42=4F}7Tdq8vn0JKHB`ZEv=B%+jnA{F=nk;BkM&;j$7>plB z5}Y;WkJrf-Oey)eZ6I))Q&iV)k=&PVtm*d*ZE41Rtp|>*tP8XtWM4cOJ zZO!{(H?M5$i`JIvOkS!|@Y<)% zHv@LEGSJ!hs)#=B#w7m|c8$u>m`e8dz17_Di?G_dZu?WPcKE$DTz(XR$UEkyVPh;!a!|?o?%8^DT4~S|o;Qo$U9?OGtv){C zCA!n^NtaeelsJnH^ZKjQ#?F(bNN2idR9`aXOxY$~T1YQkcRq(iZ-=n5 zvl_fH$Q&^FWV+gi?UBqji>KP+C9#Ho?IYzp z+-LRuvW?%+5F9USX|Te%jgmmna|~pL=uGyl>zv+mZ@A#6)>SG(NzzZq1|~qe`)QwZ zT#7f?Fttq|4QI7`!t=e^_sD9mn^%|zSt_?R)9ghz+A1soDjxKH`Pd58<%gx&T0|BW zJ>jD0HqEs@wwz-VdA?wB4^-+NPWt&|@m0d=IiKWQr3=?i*`Y57d68FhWm&NKHBi)J zr0t+Fv}cmTTNpLp*k(ZzBd3PN;T5Z!M_^PSgqETF8mf{9_=Zm=91?{Rioicqp zpF?x34Q|GE*g*#*xWx*Pt=wX9FzdePT*hvWHCG>NMCVY(Sgc*pA~ zAae0Et!x`e4po{Q7Ss4dG4+?N%$QN{&;;ZhU=^pO(fRRA4OpxzPa+;kQ|T)U{!kfg z$F;gv7`;m|{Yntyv5MPJVLte^=J($)TqsPuZ&SoIU1B+BW~WaI;FWD2)|XNWU1!y| zKQF#N<@+_keMEcg+Ch6?<2-*MbE|^J*Q?Td1?OphEI|&vV9EFSC2a}wSS+=x-)F1<~E>)YWCRxncirVZ8 z)v)clVI6wzmG92-qQFC8U^iiH=VEOl=kgFDl{)S_rhr{DIk0NPH}V^;((1Y?YTMoM z_5llnda(qZhv`Ne3Qt~rXuU-vn}5*owt3uX4m5$*@CyBKnBMNa1quyFML#Kp!uf<# zAIHOf;cX01Qr+bHp)q?)wpfwfk zWE4DALX8BVVUwco430u?VmC7j^E6Hpiprru2LV6VA9g>;Iy90 z`z5A!stp^8QYZ`g9H?E7(nwgcB;egH!}PeQt*Y|WTsiegG->EpBak@%Ty}LV)r-^f z_h!gRdqXZ-hfVJ>dfaye{*tP4dSWVk_0pzD5qVyy`EsoT1ZrND80>Fp*SH19{_IQ^ z{B!6FO(x<)b}jTn(fhiXeX5j7!MT#k$5yi3Qdic}?;^g!4im|L>v^KwvV#Vn*Ls;y zuU0_c?bAtjHL+VxH003LSQ&@!`PLrE1X%YRmYaT`h4Xv9DX+;Bd0=5h4Z419p&!{_ zqbH=scrTvK%Ki)#!bkbgEr9X%WTX0$>USs0%e6$v|V%Z~e)Tz34VNYG2a=_-XcQ<_Jp_g(PltHgLZOW8nqqNsT3L_e}EAi zi|asA`mAY_$juc|wTHT<<-9*Uyy)%H_t45stLAYE5yJ0Of)`m+9Z#;69Dn9-&03vO zKXpPEiQAI7Q;e<3=G7%N;N-*NN~!f%{i)GnmKW|;V2d^LaoRo?dwW1t-?-Gi7>+G7 zhu6ViK_osVbS{!HZ!a8?Q zTgGdze2>6wpg>sIwDe25xbVcbyr*IxOnT+od9|wLf*%PhcE|=OJt7ym+v8KES&O(1 zf?8wG5hLR=8-552EN2X3#!1ZtkTQYD-bp`{gQF>#)m8<`faxqemkygX^O%<UB`h=!3#T`OMnq0k$A!sqQ0+0DOPsXP?|I_3_eAs|?)+hL>fd%1 zy~a3(*;bq7c<+|>eO$CI14}tE%`}w+Z4OMOM762aaI)W2i>xLVE9UsHq@Fo48GFCw zU%u)f9ez&vN`N~rji+z*jG{SDMI?4Qrs)r8f;PQnAgSCMovh*$=w*lEQmUe6&f z1TTblcci8r!q;mh5MJDKCLL^PId#B1?1vjgJ-I;0S?M-C7Dv#!un3~Mz_d=>mmgKy z&o6L;Pf>B^jgz=QqvKT9VfI)he#h36HPG_)m@dK-OkWXBedL*se%U_s#* zz%WHE#gYf+EieYmdR@SQhhJ0=-V9yZwS}F%S &Re?!uez2Cp##|!cC{dwZ5!d@ z$nX&FD91M`fMuI|Jq9nt_|(2wt&kg4!@|WKlmLAiLl~yc;C%9b;dg`W=*O}eP_TL0 z$6o1?IE2vu%(-bFvij%JooZBW;>Xo|^K|chCGsrRo5K{Gi=FXZSh+{w8LOAZV74#)Rd$u89k~4|`_Q^y+i_D(`c#KC4Uz zIljkyXyEysKof;fLoeq$N{R!84wH3!a7#SlTh3RG-~6g?ZB=1lLy*(bm>V(pAO>$TTwE2VPP`4$6>5X@)pN#(&?F|+8N3aw_Q zqT|wj31;V6e>DJ=VdWm6wq9>nnw>meOD_J>qtW*ckh6;G{4eT?Q2<6Cg(bCx0mcnvwg1HeQmuF*z`xe z3o~tOcN;;ofq|bMxE|OjMZUSWhxoCjYfya#FBNtC+pfi<)KXX>D4i~%Dg{%xA~{jz zR=BqL>2WYX!nHWM{4^-btvoF1nK?^LC>M_y(+er*92Rxsn2~GurB-?D(*d;?2ixOm zPFp%-5*kQDQ5v{Bg4%$~<`bmcmu-8X9IITYh9RoV}IYl;E z(XYm3mpT0=JgpnQl%>JvW2$zMb2s68($$_JiK)SRtxbPi%DD(ewS&CxOQF?kC)XSo zY{7}l^LleotrN&B47&DmkeZ zu~r1>S9Kl{F&5%&p?jbQjFXj?+Gs8yi!d5sH z15-d3l51S7TDQvBh=#Wdei!6LMS*9Hk+~S8M0bCzZ^KR&*5Fl14fB;_#1*!^YR8x2 zhV$I+=%gS+$&kH$>degvnzt|-8MH0dcXWx;sk8KuLa5|TQ}wwwS-?`viKGKIAqO-@05Hga)T*X#Yf$eX|~kGpn2Kvh3;M3te! zqJQYOl|`^_%^YSqko(u$oyItEJI7QnRR+lmB`b*3Es~9WYO?!L4JmPwW=Mzoq{gjZ zw`TL30|0=B{sw*G30B8BLb%+?zc_g|-u+p|RpXJeI#;$V@sxB!F>a+{?(8k?eq*W@ z*pKDMX`#uH)PVLVxcp=W|Hw{(F0@U~bg3L4I@N`Ja=YZ=Z50^fO`x}a_QB52&0>n< za1JGn@9zBWb1iNp)~7$T!|Kn`^{V8w2`D@#68v!e7Y2Z7=V5K}v5F7rL#LJ-9B$Hj z)kWhN|BeGEfb^f$?SC_2w2C*?bQd)mNTgIvJujvC(Pgv9DK}1vE)Z7gH}={D5b^~M ztY7$bPv&kWH?Za7$xPO{RWtM%oYU^+?GBr(nA{dh^o(h2F87h-JS@)iHdQ(>GuMR+ z;^SGfE>jx%1S^2o1l34d_rSbOs9I)e_`SdsZnpzNBMs+O!=;O{mv5Te820$K`{;1> zn3`+n<9;<7RsFfU9DKH9Wu8{I`d$AOftY1jc+5;dnqaz(|T^X1sikJJbc;JUFRKm-=&y)LCg2mi<}xC4NqVH$$@(U zm<7k~`aJbVq;Vo9aeE8mGn_###P&bfcDnC$)=kM{JnGy5 zr^T==?VTc_JFDS>Nc_c-AfnxEc}%XW{Rhi}hn~fspAQRB5pEAq9a!R9cXzEns2eFD zU&z=`w>Jad=T79y z!0VsdvD=Z_X5sQcb&JPu-J%V!%#)NhoXaH{Nua}w2C4v#- zc^>z2jlCpKIM^sB_fk5Tw!FWDn{NcOYwUv3>pQKKoo6~+F_9|_nQi5RSLPIuCS=*$ zrtkg&T)Me#(&j$DNqR`~rGQG3TxylVw7q%J)2M5tC{rKj``tr zn>sP>)fsLRznh6Nhi3cjyZdWThJFdffl0O|U){o_Iiro z2HuatRalzJ45eZ5yXhdzJ{q=9Hw3z59S=>)&DNxrR}QZ>zNoCcOU0|7a(>>?qT$vQ zpGRO<+MyTp^PFFbGifYg*WQs|+pc|ENvo1yt(BDC^fIsOvMpZMZ!>5#F>OtE&&WhI z=*H@S{%@krqfJpLjlz5Wg;@*;3W@|3#ee}pL~>FT5W!Ub|G7Q0m`y*u=&tT6x!f5b;+|F}iy36>uFttHF}tfTRIkmR8wO3Qm-r04zy z8@lH1Xe2cl{yk=PXOt^BNGpO9rEHR`sEkEWulZeTFg20x2~j`iPf!rjZs7--v-7;d zc|GrbR!{n3f8{qgvoe-UeHS5#x*^UGI~T&yEr3fk|4_E}gV z;jYnYW`~ZvqXwa7mQg1?=r>43!V`LVi3n5RW^?u1J$8O(^?3X!@`86UhMIR)zY=ot zo3A%A8IN%b_xeZhnUK>v(m%}BXb!iM4JgD4PKz;Y$RMv7IcF({`NiMFishu93qv zyvCK+de~Kgmu>RjKhjPwDPmlZ_FZBNTkx3;sw_VzQ$9Xj8rNe}5x?FsxQE7Zi)ojZ zdQvX}QUg}m5)gur{bbV#^-cM{G-gYoAN79nz4BqpUv1JYJiC}rzOu4vKdY15q)_XG zsEo1W-ZSrMN_%~pU>&pH{sQ93HIN~F!T-I5ncG$xi-XZ0edSm63{(>ez6@@duoX5g zqIVsq>Uf{pyVEZ3R)OjMJzAQc-P}i>U&L$mJ*v+uBW`1&9F&V^?)RK5+m%x+!1B8h zcL%k>6HoJx0;b!M$TE(yW-ffC7W<^j__2d0vuO8Fy=>ImHTX)U6$GAN47)b-hHa=n z5VkKMZT=vSJMe^7E!-^CsQ%rs_Qz_68Ef1;#-piU&N^llU9MG7eXY&?s`j#+j3~d~ z`fg|((irTa!MuEThS2VnOoM}C^K@T}+-_&2bd&;+YO2U;qR!rf5i+c~+tw^nN5rVG zc7B69wYhl}b7l07$LXWg^8dWHvTa&N*GV&|KrHd!J?|6>nNi^=qn6+kCvJ$V7ZG#} zCIarCE+>WTln#*DrP~nN)TZ&MGrxqrbuyNd9yRn{dXOt|eD^M11R2L<1?;?9a*Jss z($Uh(AK#~hTPQv(!7)c9hF-OuqY19m;d90@$TPbnR)-oTrynfJ$NZ@*JMY1w57!2| zK7i&qb?Mc1gWv=#ZPkB+)ZDq4cJ-1NBdzFJH-p4##jlEG+uaVUDErI(o1EpDaQ^KR zRu!s^9tuy|DNYx2){0Qb_m*K*x9YfTAOGI+u!MW!xA_!vxN-aynaFZjQX?%ZovyrE z$ND3t(#@hv9U3c4D^Be@bfb1>YuQydZTgRe2pR3=&D`D#Vi4)N)m;{mmT(H&bEM)D zM|p)e!0T&ykb*kR&S`O-wLKE6@Cua-c6q$yiII>gOP{Q_s#mz``8{YjRP#SfZ!^F9 z_uIY~k+=iK|9y#U2^YbRuWIye!{)6rsy^R5mE`=r9sJqUo3DQ#?AWOwg>WqKVvAFzH~Dp?VI|eO z1lW#x1CM+(#k|(`rrx}4fbNBbx}M`fc^)GWDt#Z5MNiv$A?@AI!6tlrIg z_14Sna~v23mq7<}L?7tMwACur#BSrcu|s%?ATNq<9=JneK&Tvx_kzaKUHrA=z=w$@ z*YsCr)4DF1uKOpZ0MQX)WJ3aPy>i5nr^t&rX=_CcJX)eZndoJNO^@n0{~1bp4VPI_x&x`FC}sE zZAA|DVbO@5xx-GWeTp7n^H^}h1l)rPRTzausyd8|(*Q>1y-s`kR}i@e7Cnh`OvpT^ z8HUs6E6GX}tM>77Ko5KcW|Pk8d9jMUl32N~PCMxElt1@$Do$7VYG93aVYvuqp4zdc z`*vzBG28;L_;z7)YXW4C;&g@>eE(a+`e7ZOt!86DmgKPSphT~$DK|jdP=sP_u56j>S9s3Y++H#= zsa#gwzwasjsFqh~dTmx>wdFkDSZnA_R>?|72K$a;j7BlAq2?VRaXz z(=y5RcJ0{#{H|GivOtHqO3QbbmVnV!>;_|1rh=of1^EE3@?g189Zm7s_plm zeenh=Anvmbc(@9)FYg_!YB~6&u0Hc`3bqvF#9iBWpPnAE5K5bZq=Rq=kzK!H!$CCZb5HD6l%;8~4<|FVMf=93P+jr>1|VP8}Iiqbo(b zEophYJoC>Zcu6V@=rJ$s6O+vfipXr?1mwySUfZHsew7Ob4!VXq+#5$dc>&y$#%NU| z9Oh%&cOw~&^G``CIK8yTf7>0tRa?v{3V?9BGR&`v14xa%m6-n;S6h^b z*3H4@N8;57`X;z_NDqemhZbAHJY2;KM;!ULkHZ0pwsv$M%x*pVGsW48Fx3lV3LIQaV?ci}te9Y<||AnLH;(go))V=b-k~PoXr# zkk}M?uwy}-6Gacyiybcx+7fEnq^upS{-xF>U6-sEGFa^_+7sy|L)Oq5tlH&IpY>Pbwbds(!&@QF zXfOIZv=m__&xjK^b?9)IPOdFaGKSTP%kNIe_OJZxwiP|9$2pnr@d}I7q>b^aTS0|*6{33Rl6KTQF+4dvZb@`UupKUF?jWiZ6gLcnSZKVRZ)a4j5{@_<^ zd2FE>PEH>`f$k~WurQzyJGK52i#eC+lNM2F?G4xO3pr&`aMS$xM-_b~FJavLwi71b zWW>|IkinffKkj>^fF8HE_NwD8jzs}FwWgOV;oYYBZuPCb7d8B0PbIx8y~L%MUi1o| zIZY8yxxgE27Ew&S&{FfYegf0JPvOt*;CQmWLvB`UcN5}WJwXZ+xHopt7f{0j|4}f6o_&UZX0ZzeSV#0h_^!;r7c73<41qg?Mk^5q%3rvu zVr4FE@+|WvhU@)Vi!6g@6_VPMm0JS<5bQa0VBF&KWp{Np!KGz+y2xiftz5R>rnU<` z7O@G|jS83rw_)H1_)K1rvvzu|GIQzjdg06w)y-dMdwMe2v1fgrLl;Mohz^drS5r;v zVDJC_NNcyz`Dx$+mJOh11y}zsP+yBz_m&+=uZ25PS~2)et=joJeZhF=2-tDnj6UPd zId+7I+cga2e0+Kf&W>!$ql#;ee-C$_MD$M?w6}-O=#jrqdDno2i7@Hm*0=iCG0LM? zc{I--ujaT+EI3j4k#Ka_w;<`Ae)qWnyv?1GJNr9z+W})TF8_#D5-^m1!mmE~voSudWC@h>k4)q+zqp>hHK34(i2io)RjhhN&KOP+38 zce~TrJb;*)>r|>Rat30aYKLF)78nX7RlbnU8cI-W-yaOM86teAB`oGHhVfUZv>cC6 z=4W?nE`9y>jti2Jl$jKlMqdOB*JlG`TAE>8w|OIAHa9e(V8E)$u;KI^Ed0`AZhh<` zr%spCOS}6$*oV>Z3ClAmuQaSdfKN)rJMTTW$C4VRb1=4$ST|4Kbue93_Pi12J z@Ua#^{!#8f>YeSJ74ehn?mbFt%ikYlIiX%Hsrb@bU!@Y|lk!YQslXQu_wXsdH@ngr zo}hZ8QsL|$F61k9t$mEY0}R{1mt=L2Wyg*rk+W~Yf%vpW&~1n%mf1|yMWm#eUq4qC zd9Pd|!X^_i)r1l>a2!xBcp9lZ)Ewo(ayb$?pP-fiZ;>xmgBAGxn;Nlv?v~`-f{^E- z2S(saZ7sEKNg?Ws3+FQ5XM3({4dBb)-YT){Pjt;DuK^fschoUpwghE`1;e&_4)d8@ zZ&Yf*ckFI6H#}9Lpd}Bx_Xt$kb%0p3tUz_m15(h^Vm^7OtIe@@#n*7xD+Ng%mDOJ| zYL}WL26pH1SlnLy39^Z9gF!wZ8^#_PG@!XYuC#7vo%lP6jY~G1B%Po>B+5wclyIUGg*E5JmT?bRqZ9+ET$M z*MFxv22F5+C1LB-Z2!_Tm3LY(<=;o}4q4Gu*1)9u9}Bdb<+CUzt*m_ zA43Yq_qfZbJJk+zxZX1dSybAg2cm@jn3qgy(+!C(GIK^4+tls~{+ST!CuD4mF2ZK; z+3P*MnxWH;wfg|H(eIN(y*j36%Wb}&UyFb+m^9N^FLDSHj#M|MXX0E3F525Ak1pG| z>wSNNA!7zNB|LZOh_3_PK;3KmQ#?MBU3$b4xaK*np&a0bQ)coa{XwnD@9d|y{lolmb2^13bF!jvYu zpd&s%fU+dLmugLwA0kcSu;^5B$N<77BEYuXhS90sGWUn(W^xh@w6!U@s|no!yIorNCelaZ8tccR@_LUJ$*D3ZXEiNo(9-MK zb`B9%?2=1k^`v8{iIog0-|g?SXYJd1tPDS)l^&Y`p$w=u_9H_=g32MTufBa?=&X-t zH38PPqHoO{fcNMZfrds>doQA1ypFN?#Ds)XMt8$+OM96xfpiWv32)#0tY2Mir@3^0 z+i?rnUd}?dKOt51@7=bwgvBpx`LzK8%f{~%eq8{HO0T@a)=(`6Cq;hW*Xr~_n(H6; zwHxwr8SiT+P-cbz$?*0g&y!zU-_u68DavR#?1*LuPWD9an+^!@D3o?SpM}Wo)3y3B zpvTw~7p_PnN&q(ER9=p(_tZ@)2^+4C{NnoFchWJDx3T+kG%u z!qKE#R};InKV5+Xd&7Src5b1@!sfzcbBYrtQ>s8D4YZZL(_$?`H0lP_dUPS3x4DO&AIW3Wf5iq+=Ayj_a3 zfBzEKNNW+5g{3PGhCoMWNFHv;?z<_Ju4R2vS%MktpHK!?d)(;QB!8mH1IT`@S5EYD z!6h*HZ81O#7%CmnRdzyF)Fz)lq$sVr%K^LUQXx+j6=xuffnHW7-hVTsBNhkKsxaU5 z+`&^i!~K^6P4dMwyQ{t)$=|dzymi-up>Hl*4BS&X6C2l~xKlG0T>ze9bEl>zz=o1# z#ZZxcO@wZ>>V5mnqvU8r5U_AvWRS1}(^8kkXPf;1{y0hA8!?3KYt%=UCKzF zkzd{TdMUt_-dtMsp%cnPLSNyk<`-=|VxIkWIWm9$P5j}ta;%#PR;p{SLLP%odM+dR zY83*l=$TYnHHE^z=D^ai4*KazP2{#Ry%vRbZQfS1A(QfrSO4b5*HQI81#79Ixy7FJ zr9nYY-kyCOqnDj~{&%+{T3<7~qHi|@V`l4QrB<)9j_dnI|0S2^rc{TEMA|#@x`iRd+Y(->wC{d$aE5o`zrWSn{K8zcQKBjVIA& zsAKg52%jN)4MwE-T@0A+qg9nF2v*d~_d!44Xb7uN50 zF!iDX)0JJE`b=R8@Mg2atn#R>Sq$6s7~s@I{@7mT_ovTBV^h^(iF^t8=6KzWSk;h` zWrbk`dJukm=xEioakU&MC-{9%hi~U_xt0$AQnU94;^=*n3j#8*vLzp|P1@P{BB@t_y3cVY?Pwi=J?$0iC8Fu}G{CGD0&m?f8GdOIIn8-LolzI%I!P03Z` zj0Wta-(OKRY~5I}8{i%|0194+f94T@eTcuYX{^dAM;L{5dk#fuqG^}y{q=cr>EXS6 zzdwKraa4_e+H`!uA{AW@yLIaS1gzrSng4y;TL(Lzt7%&6Hxu@iOHzS)mRDcsQE4m5 zzOapJ>&s%*-$lUhC8R15wEpB~sy@zvRK!2~${6bv$Bmy7Yr$@L^!KXv2&c|2d>pS! zpdCMqqkTAAibJUB5k8z-z#CLYUWq=rtMIMetg_XHAPr}E|22k1l~I<9y;)<{9uu*{ zwo5b$HJ3;_n;L@p7vOpV{a1jjkyQ#%d;BCH(l-%J{GO1pL*~|tBsK4~Q8nSs^U~>E zt|@%U=p|mrkH-zWZZW$pA1Fg+P2R@IMt&ia4)V}`tA0Ojn^1#o`*o%m41Qz&pMpb9 z!r5Y^QYD{14$IS&P+kRY`rOZTVg}SJwVYDZ_2~Mx2x5qCvs=U!J5pJ}oZb}ZCEynX zI`U6~O+*fcyYGKgzma|bz7*atG{B+QG#}43mfV*6wHfi}_4VN?$&l=rZ{Z#Z`y9;` zAV~5}FcLa(Q7ly`=vZp@DtY@m&sz@IC)6YVxdUHgZRZGL)H^L21ICcYYoK=Xb^Q1< zfrzg%LlxkX2k;u>8Vi>w^*8AIdlD=CaWaSY`@Ss;uh&uvkR9E*G4)s|F*QMQ?&iQD24YbuzKlghWxd7-rImoj5BR z%(v9$@&#D_jTi8+L*m>`){s>XTpto}b8QC0dud0ELOY!qvhju&e!W4`S7EL7XToxJ z)m4bF-Re^JUobct!h_#eg41P|%Q;y0Pz%cbw-_~~!J;~`Z<5%( zf7oDyO=m^7@>hyW*8~)FQ2yQ&H)jq|`0i-?;#}Q!GIGoN4FM3p-Mq!Uj$Llkn{|3y z{=76s^64RP#aml)wictg&2%8ns}1&|mdh-_TfHjaIrM;`VrzhdGC%!2)K_~Sa=nU0 zFaD61JNRsHtyDOSOZ)Fdrq2F>UR2>xEHeJJ?_e!M**=vP_fG83@kv? zNNU@E-w8TocQfO5fC=XWNU77oM_zy9IypW!i(P&^=>Wuj`qeDTC{y00SSCA8+1tO# z=z_eqJ(&?0h#l@Ve$Um|CTB$N;fL3oPB#l5jsz%;jYAbI3^02^Z7zd1U4CRQoGrDO za&bJMw$s7L1e2iKZgn)P_mVzW7aIo>(~PfOhi`GbI4jRD^7rMx4PQs3%xt%%OT9|j zbbysTejlpZXekU|SI*G@OG(c(|I@AtfN96%bv>#8{PI#OKyfOTZu`;G6Yhs{zLCK) zrG!3Ln|=C1B=a_$F~f3ON{Z1??%tZU{WV*lO{zo<2Lp?KCDa4e6Cw|C|s-HiBoxQ0ACv2VdEI zcn>K#ngn`U{Ki=s!Un5YH7|{u6NR(;d**E%YuPV6ar7}{2b9H37ha;+KMcHT{uJ2i zesV_N(c2#){Z*D%@M+#3H%I57yXz$R09V5&&vu*7MQd zZY4tR?X(9B1IOXu#U85Qg;G`al@p%R6A3&e5{gixv%9LEAw-e^I~hRBDC*JX3eIyIkzgo`>9yXDNlpd>lb>GKy1@ETrP%koE&#ru_72ecT3hzga)g?Dg*^E zz`!T6&=oJ8ElL<-OIfDZ6=O7!8#Zgpd;J5US;!mwJO?#?ceBy?d}-!YBxS>j`@y( zfjxSks(s^xzLf!b3v%s{IkS8fd$@vgF!$!6Yv0ED^Vj=cdTEQpn;uDlbQa6N1j#QZ z=oyJB)#b1CZC!Wmk}MuqcD1L`E8w_X*VCltCgl6?`ddW#>mdhc84mk+Q-5 zS)Fll4b30f6&NoAj-2nqCAov2rQUAPys7M!OqTGGXlfy+iQXt%9=v9tub8pUsjsp* z^b)P#6u(_pu!g}2$VQAd`bR=*e`kUO6>IZk15*x{?MN>P>Y-)nyJ@y#Ylx-Y6?w5&k`jFQC%SfyAia<(K#H)$u}|RAhMf+Pb%< zRv=FaV2IX^V2nQi$N}jg>`$bG|K!brtxm-FB@Bn`i@=reC!$D-#A*7BR)-tZBo9>~ zluw((VRe+{Xg4v;!#K${_V{22L{C}~t{;-ad)h6@)%Mu@3%~Sfe;IE2TKE(Mtw$nk z7a-uY)*Za~4XBRhKR|#X;^|mhNSSnki;ZOPtE@q!iPy;5r@qn>tQgHs>l7JnhkcFU zfatMDF1hYg-!Y7Y;*DS(!`jA()N>6YCzon}6?sRWs1GLRc01o)z3yUzD3Z7aYq9ZA zS$LP*GC9PLUPm?M`dq47G~$^*e>5#3k6jkap>WMk>9n0>1ql4qOhgXj-Kjn}mnvtn zt%09Wz%={}`@Y*@K7MpmB%Sx$<)XYIP~I?z|<5gUFMuJYH`bqqsE$ zT~j+6-o-p!PonkY`i9aave*=jY9H%JwQqxO*a;0n3znOcO65)?GOi@Q`~d>Heg8KP zJF)UCoYyAwpVF1~AmgBmodAj-ew)5{M?YyFg8e=p_kDf1qlT;TeEvuDs>;d^(_lJZ z{i!`5^fkY$YfYrcX@{zOuh9t@Ms7Go2Zp&%Q8!Px$L-j0b%Fod_zvO+p(?3t-V@3l?uYf>TzF{Sr^AEs zsj-s~dd`nP*ti?dF8XO2`tE1Ewx3^ptI${M>(?e!nE>lj;0Ni4%x}rQ&+Bk~RdIUw z?8FlQlmf;hdq`pCxKgKV=jcFX|I3}5^>=Q_9xbGpFkNd)qflvl_$KdU#q65L!9ZR& zv1_q#3aX`j|F|FS&Tw4BPMk-<0g@FY1<#CBUgf7@+?9^JlBykX^b z?VhUh%jvq}Z{8vVL%E>Rc^s~vr&P1mm8t@*_!+NA0RRTO-N&TZRO!vRQ;|2sV=^|9 zk1paS>Vor7y+Jl&Rqtw>nGWV>0k)V|0bVOd;h_h;@lT%idIOxpvPviOGtLLBb zhUg|t8-ASkN10H`H?DaLz zt^w=U`cQzGi$XDkufvcy$u|)|rTpFIeQklDCChASPl@Jrna$)mSS{LnuG@Yf>++%X z9{X5NdfD>2#eTNq?sbq-i;C-tVQ5i7{}&yJXrt)ffOcXxH?y?(c+ zf@&D($y(Rs0Fl>{0>9|b?#0wz#puxZYd zi>)_{yK}8xUnAwv0+s$fqEG}kq-!wyl#0*n{wtl$O=*5)a~$gZnzI`SwAgnB-NLDm z9t-55()bP(`U5~7K;>oB10T3i-^YIc+HAg<&g9Q?Xm{Pen@S_7{#4lg5R2*alNGjS zON92nmbkpQ(dGF*kLbVr{;;ZxgxpvFy2*ElZgRafg8neOs=I3H@|_Qm5gjn&?r=r$ zT~}m`vi};@uA7lH*p5{n+R(_R()+7E7gO}EF!N=36}Z5XI9(SNf0y0z&3WIr*lXuL z^A&_!7T^PhUK)*w>VZM$)hmF(*nl_G-A`EZ<5mbchi2=!pF9eS0_8hccu?~buL4iU z#|3~@udG^Kb^6M>7C@iB`4#?Lt;;z%yp#_!;P9|S@2zqG6;U-e1F2Rd%8*o)+ymX3 z&MRT?@!u=Gb(&&teXXk8`pr*E2tx$)RZ=ZSuRKP#O3TGRba*`#&!Abp{DGGFDa`G- z-!ds?4Z^da-J@8Pjc0lHN67mrmpr^>4Wv1N;TBTuD<}4=T%ta}IRG_h@5i+0HtCzh z`S4#}64A=QQKdi?B^U20(AA`81sck|*~fZd>j)_fG61DagY0zQ!;;WZB<7tew?#vQ zfd{H2k@GEF>V0qWEVjKw+-atZJ}-vUVCCi@ih;&~4s zc%LQtBk|hnHKf4a38Pzn?fmdu7=z_O7+ZH$3>v6Wap3ZAqfXY%6G+zU7>QaY9*_WV zU!T60fQ5Q(Umx|+iu_7$CqRhg@Fljdbj?F|+S&V)enhwCM`gOLF-XZ*5BPIa+-Y5s zH$V5bOZ7#(B-RlU^4r5s`P(YKA99@5=i_KdD6W3oU&u=ml_uxr8^OL`*@&s(m}ziC$l|^VJy5f{dw$~KZZ++F z%G1KMT-AiWik@LW+y4A~_F63c4)Z2*_}~pSQEDzD;%BvxOswPUJKsLWTI{l)A79H? z^B{-t!=M)HZ$uv=RQW0#9@7up>d0l!J{EgPB)u?@Z9<%@t>x@;9DgL3Yh(4ZbYn`S zhG?a|Qw`k82cYi=66R;!8T8rQ*+0IQgB3_0OaRpdCaAfj``bsuO$HOQL+YhiK2Fgq zfL;P!V<6-u8TN$1EE@;g-;*bP<$3Izy>0xW?X3irU&%I6rh0u$G225hCrM+w+sh}d zmO5wj1(fcTRzel2_4WCL^1!lZe!bobfjPtP;JK|0j0PUet#)-NP>CAR(91LU*<}T; zDjb|-XSV9iw}hDPbY?;#)VV5gAMLsz4gh%lDr3Df_jHwo5DVu*cp*!+DR=&|veG~Z z$0Wq%6NbFb6&4#E7q543Ajpfkugo|q*2Pym?5|J!eg}U9ZT})Tqu`hM!g94~z^^mw zULH!jqSRNFX?YYaqKXp1b5tNMkZygvZ{)VD@6o|{)G2*h0}w4m*YCHll~)u^>j4+$ zY#IaNz#yhp)s=E6m~X{UpUIr$Jiu~Ks(!YLzko0dvM_)wp+THiUKe~UWDHE&qoxc~ zr{c{ojne1{5G-%_k%KY&Fk9gGK(S$;AjaifYffEiU!H^q-~uea-m$eI8GHf4H_dEX zE=<4;-UpOes%IO)5ZY)(Y)odTB~!qKWS50sHUDkz!%3~2 z+oek>y9+r{=`^n0hq;^&YytS>0P66aff{w#r`fGVGL5=Nfs0O{e=7_yL<-YKp#PZ| zxep-l_c9~Zp5=vfq&AcCm7z2GBdHN+hF@ExA#DoFZ|e{2PS-Txxx0_%jB!m4A0sg4 z?DTPW*`1+o#fAMD7Ay}lDDIuMslF#`w@*JA&PQbhEsnW(Xd_;8MU8Oge5B4~kVajE zCNZ-s8|gXeDP6MqrB3LMrY_DR77jf1__ATKl0clrhgurNRj$5%rBe|5INXC0l}_X{ z(!7*|ehK9%b94)nf)T^DUOqd))KVqcNw0#nkCw2EK=(j-ljAGKM$I;Kzb?xt-vZE@ zYRlJ2vQ#nutV^+xQn8A!dY#J=X@E8Ons4|!*|fLK2RAgq#7>@OfD)*vK%~3499+1u zc=kG4m4dWbX^`xKSlpkJ>-ET>rRh|&$NkQyDNBXyvguwW6fA;{&-?!K1k{#(`J9&J z(LO$YmK)0Rfw1Vc7Fy2g;e4G63wb+KuSC?geeV`QUB69hP7_-+^HL55w_UwCE7Fm- zef-@DjofaHE9$-8D2=h#oux+WkG==BNa=j=dX3+E-VmrIgzBxE+XQUV;Uu35rJNYK z{!y+A0WlwYNvYnOMUPTHob?j_KFVDE6+ZxDkm%!O6t9k9Ns{W z9XxMfZZNqEHaF40n&fp9Xc>f&iqHb%<7!p17kiPH|X`N!r&AGqn+hkm1*hd4p4e^-((U0 z1zzU%F0f#os4jqYx8@%6;MVI4`yoU~jWCnud^+qY9I*}~sfgHP_C>~y*uX2sHtt~~)fY*9OYLEulv z{FO>@`V0{dq}{ZkG(Zj@Bm>a$Ydk5T2V?kQXeXiuI`(uoA+ysT+!Cx-?L}$1|M>9j zfd`Tdtj)V^v(s5VCW+zo{7ZYYXHXD~r$eW|YcQ;pwvo>j6;dS&RS?UJA;T@1(dXAG z2pa{Aegc|h7}4w9b9a|nTI?F1=26y<7)cDRsW#1t_AAhuy|ltZm*ZeTnTIoMB@jjMJ`j_255tSfx)awwu?wutN;xT+ zV3@XsPg?(8!(8=|5W6V3?}@f{%Go9bk~oFv(_Ka9QoFH*XZP#J>K8D$+K<`?8fQa zWske#?K=xhl9%Fsnw#I^dU^+&g8$Rbz)Zw@XPX$?QTsyla7)xmD zdyxq;+J@;XB-?tobowr@#rhOAQ_or~qYE`0iTA|f1Sw2`BMR)RI&QfoibFsFSY4R< zX9Lz`q9Agmq9z-UN_A_+6o>~@dK}aW(Pc(~GHZ(NRAL4W^^?vDR>1Zr_0<*F{wg30 z`9yBK4=Y&L^iTDwC3YfC0WpTPxpk{JOo}5O*a_6-2?aJ1=ngDB{gIXwrtMCPV%LC0 z^6@HKN!dT@QBQZnt3N1?O6U;8UgakT2Z7Q<>e!tLT%+pAN__*+)RKR@idnN^wNV=S zU+b^sCcQGiSaZE1`y>2FomI&JzqAiuYhOf!ut!D1T&1|+`Scs~ssTCGN)D`0jtX)u!g z-@q;&gkaWj1`BiUEYb0NV5%Ci{YzwXA7MiRh8r8SQ*-abMtd7t48{^e?F%ZuZJ%r$ z#R8%PyvjCWUhBlY=RBvuJaois_#BvWWEw6oC!~Sj>yak^P zkfmALH5yNPT=t93tt1zht@lY|zn|-I#H0H7z`JERBJbW;!$UNF2Ysr4Yu)y8y}0HU zHk_U777~mF=phHg%X|MWd`INIWvf3z+w_*N{76P0p>yr&t3$~VzH4`s^%c{r5fk}R z{=PlC*n26+Vinv@Yo8F~O{V1dY~v5xSEW)tz@C-i z*H5=hpkGAt=$$GT-6a*}Iz=xdKQ1M~GW(E^!;II*owC$IYtR-s8*n#u0A~Ed&6=(= z{ukJ(8N+sAJYC+{GFsSczx(s=XzKOqV6PaUIRxX#Qr%bNRo3qc~ zhl4=j6VInI^AVL2>EeQfyOikd`MYlOkS-Xtd=Bz;1H($*6RIU|8G6&xm z>g|N@dQov=sLorQ|Joo&v_S^#R{F)ii1%%fYmme4Kf0$OX%Ab+_@qOl4uj&K;!?u; zh0Ydm*gQmixzYKQ8g(IY%o}AB^(T3^ooeJIj`+5lUG4P<0`7N}oK;U`%acU(cIU42 z4{-2-xa`u+)(^Pozy!1pc+NI~S|R~_^K^wR-BBF!^e4w6mYbbzO1-Mnhm zqp?{B5l9YyO$VC;K~B5Ei^QO2R~hV2GI?1n1I24DmyHzz(${y*_)|bSvnyTOUQW*O zs8dZJ9^L9d9l2(kn0>EiTz8(e;?DcbNMtuR=A{37X%I#+jBAUzL84e?T3S`p=&ybp z-)~bZ)!D_?-MpV5M|3vm_``1Y$JZ0PPfuG-mVI<<;PZaxd>1ot6QrC0bzyS}(TCUl zt@u6X+eXyd)^Ro^YJRFq@0I`Y{m~MmN4$LhFq^+`)_k2Pw`sO_4d(#F4&y;}i#O07 z{kTds`f$!n&P5>d+W3)yf;|_&NfZz7ME1Z(Dd@F(<(TWDeGJcZkWl5q(~352I{r)q znv}pkO-lq4|J66+%W#-9#@hF{dsy;f8&?^jDzIds$pqG64uL zm(o?~>JM+>*?5(ZsrV17bY}xhcvlGL0V3q%eYP8{KObW`Zq8vIpbF`M$m45(31{o^ z)#$9$CIObmA625-r@_WaYT%$qY~_CJ7bgOjSrE)2c%iYl81er2%E<1r#U}pyv41+H zm(uPOi*-E6V{d0IMUbKmy14yfQQ1GptFk7(P;5- z#BgTIGX|RL&vO<34hLpB{S>jHXLqh`xI1)#$k3Vro-T(Mg?YMbz8%25Euux}P$d#l z2MEkq@dkhCEvPx)=(eqe7cJP*^=V)AMm5fN3Lx=XmN2yKlj5U6{FRNw+h?9UeGa9Z z16#-(CP!0atq}&D9C;gAeYevYyN|BI{51J=MtgSE+}5v^`m*-wey5+bxZIvD0+Eg^ zqJniV*_bL7+&`9Y&I;`MYV>r|82ps*eQGj`kKBjy8d>QY3bk%>g~{PJOX%Y`xL^iI zNu$&E-lqHqUUh6q#Hab)_|DAO_FHqtjl&$*nM|uP4-STYc|lqr(>A)V4OAi6fn*Hj z?WUA%bJ>Q;JH=`!(fjcwSuLvBRp>jDPPjiB8?YyC>tMJfj(rHqzM_RQVe)!Qzevo& z=&JP($CD_MRZEp2-5mAaDf{;PC2?K}RQMXhK5A3HUy4CTzJTE26YL9Wc8BXZOq$); zzMl7vQ~se~*{0MMHcV-k$ru7k=aJEh#=GTgu#&sKrB)A)MM_qaEce%>*RP_X4D`*q zF=`ZG-dQ8|v*X)n`Q+tHi2FQR3d7a4U(<~BvDE)wHSB%3A^vJSe|u2A$bs*#QGE;+ zDEfx8_9V)Lzw?ys=5T(zm6T-m7X*jW6OJAQ8n1nnZz0B{XrSY#c%o`|VlX-~15ZLu zLwB>8vYD1%x;SR;wLcGo?S=<@^FrvEM@x?h=fEb}x}@!sO$m2A$$uWAg+7<8Rn6lU z!sz?#-ze5vKPnth&_B9~#{QqU#HK?n81A{Z5j%vKqbzTIrnTADMTiYxrM>^(jq(y zdzsRtIV|fzb6;Mz8E6jsr+X7UOWn=#_ENcAj=Wu90X3VnLAitRS4T-(8dLm^1cnuI z>uEee`ci9^l<4vLecqtW`?55(FO)Sp*ZHf-g{MJU@xu1maopKc>?GZx7vIcjI-vJb z=oSCu{)av-F%=50iRf{<;v;z-jneVich8$e8NMjx-4?&o^L(coNqMXGsXa|^NpR2k z;~jIOfg@9EA%74Y@uv_Tp8EucCfcB`ujgOQ&A~GlIZK|teRSq()Uh?1=-Xvua;&bM zvbkQ2(|R*|;+$Eh!PgJl;;HmBRljjPFvE&Fo~pw){FOksd1#E9jXZmA0Wjx<4WCu6 zrHEzUkN`gDSxpLHrz256bXMZ%Gn||YoL1B2T7T7wnV(YH-=Y*k#?jw<0U3aD?r&7M z)?0i|oB!A>7yz%IG6-m(cwb_qmVwE0cwQ0X3q;+1QXb-WbI2S^`RGG!pL~|NLE>hkW~K2nf9js?tl)R!1{$!U-+| zSUFls)#GvEtHK_P=pwuV!Z-IVrS5SzUi=216rQhuB^Y)Eet11jW+q`&$CG^xG!A&PEqR+gA>Ti1n}#g!uP=%>^pGq&el#_#9MiAda9UPboCcf(zinHRpYQNyBD)A zs#KJ`#daVqo2}0e&jpwDVsJ4CmkCVT=SHP5!Jmcxl$j@Xelq|A?ht_U}JCjrAvP_?ma8}7x9|Q#p=t= z6!Q|m0E=8HN{$jS?|lBsvd$nhiW z`5JU4gKE1zpFLwc=|9@WC3x?2+M?p(@eVJ@o)UiZNA;L8FC@Jy^(jzHz7Q8dt5lkH z+t<6Z^-6{$rA2s_y=8piDWm+vno=Y=$1nJEbSO1R_B*%WC$2MD@2K8M_*Klr_esco zrL-$I%%~C*6kcK5>nbkMFu|L>NB%iOlXf@+6^xUGsTA#Afta#80AT%4bAKbA|3zRz z>C9jET}12(HP=v|NbjhX_*1<#bp7w>=BqrEK?$|oBO_G`VbRrmJu@kc$}=fSR|=nI zU-5#s1Ot|O{dfO%BDHyi)8QLLD!H6ahgF2{${p4a>PwM|Z%05iJQ6s)Xv-ZQlNCK3 zt#&W(mP=2iWS4L)Ie+~-s9Pv^eO4a+mJ3+_^QV>#oWMi%b|klF#GkD1*@nyS*6*dn z>kiDWGZ%pb=6(JWR|)0&^&0SF-DS{0))TF+e|G%PoS%r-~slInO z3b=F6Z|wpx*=F6#6hY{!b&ZkcD2C8fDXKcY=%YgWlXjXs(89K>T=@A>^~2$ssS2as zA$Gd8?-AIf*dfWy{unV1Vq}t^nEmI*1%l3Ru~O6SwS0`73!v*OW$1(EiTe^Y$Wgi; zys35m3H*Js-;!^^ywJ?uEt-4}rIDFWt7IuHV~H6fJGmHloH*>1G~Wy_?W4!{{kt=r zb&gz5U#}-D<|fWA-mzbp_9w2gObXG@O^XaxV)TnDcgss@KkmY=ptkU-GvXHgpWee< zw29uTNvFiWCvROFKuviZ)?T&ICyAaQWptYGJ-->c$oA`A}EKY+Q>emw;wKR=CYjJ$7rSmt#ar^i(0f9oD{j7XeH7V*F z`CZT0OLl0#7`1nn;RmeGT@z{`BC$$?eq_dQ%ca0??1KH5R#Dt_YW=o4K@D_0sK`n& zpz=Hx{-X10qF_UA(i{)}7Qfn#?#Cy&s4v_qR<%=5nV<;F{}aalz6ZVZo0%7LTWyH$ z{kScB>?vuK`-Jbn;`PaPvN@p6VsVBwaElIwQ9&8SyRdb;em-x4^XFCUOG-uHA6n>) zeQAH(iZ0CCmvSjr5UQ1f+&Im`9a%Af@>zGLe{?#j{3Dquo zUC44of7x^3YW`jY1tLJ>>b))GYlpt-9+ajj$INVTtTKc<-67j~U%ov}sKtJY6A>QA za_05EdG+#k$x#=!&nJln4%Ac|2Gs3qmBkFP&dJh35JGOxGi6YK zB(^uNT^l+Cr6SqN73oSd!-ILKIjSuaTzHptvuid7AbdVwHt9B&*XWis#m@-(c zxYsrl_xYYzzCRk$^$V=~4x0`?8vhts1wNNq45td#_YHNg?mL+1J*)~dLAUEDz>B+V zKB+yJ=Ck&>nAo%9%yPB6F+I4{)NCc1+<8s>99_*!y0HG=|3>56u>eZDF`P2Ka6;(k{#t<$73u zOj7>ci$vt$pqJ?Qcc`Gf(!+g>>l1YI5Xfn z!|y?rUAX9y&S5 z0ltUmMbI|=8|Q4*mJ3qIgJ@I5zN&bI>NekI@cZWb=-e`Q#LQNSfwI&mluA|({ynQi zQM3TzsXBSq2KhqSUeD@Ie~@n1Lhin=m28KzsiePEP0pVZC;Zk+0|4)Ub5YG`*#3n* zx8G>mSp5w%NxuL)s8*09`JDF5S2EH=92}-dI(pyC)`|Uo(muv@(jSphJ`bfie7d=% zG32vJt#F)8dE#9CEGVIhN9TsBhTNU{$1=^l}o%F z-8ip&+6Ai+p#RUA32dS7;a9!hP@NQu5SM|0(XtTvv_Q$fHCj+l`yv{N!hJ@#y!4)x%r8AAd>?O^Z=TB9a4=UQ5oc+-7>>fNHM4;{pY+T&N3H|9C@ z*V%FAreNJ=yI+CYP27P^XzuZqf3t40(0>FsdIA=KT(r#w*U@4h#y$U_J9%X7w2pQ^ zv#sq=vzEEg!KFRi7p>u7yW;%=ZjZ;NByrjH)y(I8G}+Zx`OQkJ3|@Qclyovy5iNWd z)Y2ao`fkQ3w2`-7g;YSv%+}q^0>$?vTCnE?$W&(1Pd~vOq7Rg}F}+^*c6facwVtk) zT`{T~mu-3wG1y*IL;90`)w_ISvm_A7R%Uu+55mD52)TAUEDXxu&TN*Q*D<~*Ot0Ot z7rhy2Au{FQZ51rFx&=tc7WHypgRSKcdMJeXeEXe}vekwy76KcuVuaU2I2=;hzdStr zoiZ@O28jI!MAgTtf|<L%uI&cWwQ~Ao5rpR5@A>KN`xUC?C4J0FowW5g8!9utLBDF9q4A~%l-1i^f#ZOX=L304pL<-V zIsM&Z@Fa-grH9w;zvq3_L5bczc;|33V&c89SrvT7n-Ua6)cl}3eYU&qH#7ay7Tf52 zltF3thd_cdfcECy=#S5Uc#@g+->#Hi^{N@@C%wPkoVq777r~lva>X{4aqKUMHmeAV zka@mm`tjU@p-!fM+^G5ck?Kv5Rr}(b4=`WoW*x26pNg9s+qGR>J#&_apLshqp4A$D z&bUV|Du1l}W=j!6x|P|D3V3$>&D551Ry(%r_1L68u7%vSQLOLG^ib|LAC9k9wy-nO zRoDWRhKi`-8Mx2m{p$Y3P7k}n)EtvXScXBr5)wt~nfu6Bzf=^L3P8}6>ejd;`oJUF zNv2R{2Q?@(r~`&@h6-p&x|z{<52*o>eI9QE7tSM#d$h_bSi9siwds3U*uH6jojo7d zdC@B#@yBsnH+l%|d3tm7qvwx{;e}RKtwTA*68BMGnSc0tw@s~wlliYL7m`bdB3Jzs z4tWo5A*Zm$71tH!$k!K@-cxTbUAbh@P`J)i5o8x}3l&Q&&NWxh#SkMJ~#oXUa9;M0NP^ua?B<>$L43^X}pJNe?$* zs+_huPafv!x6C)9ldX-+>K=nx9i9fMguw-IR zkmHv6TIlTcDb(|}&Gw6BxiYZ#FFed|iraVVw{a z8T{3Ezh(5jbPqL$755u~o%c%k)~M!MFYKJ~jkl@XbV|A;8SJbNOnO+WrmdO)%X#_L zjVo^`o2w1Qdp0;Jqh$~UTe15>ZUCw}AwtWu5R}yOUFGJ~mif)!{TQ546#90L)(g%K z$IdUF1P+JFr-Iz!2PY>#&lk+v$|P#Tcdz8!ehAI;W=`xqp)Hr~zox5#UeEvnCDq~m zlc@%#lEXOA5M(cDwMKLJbkyDXyH`=lN$Tm<-#Gnl;T;s+y0g%aeJ*=PtbvSinjfi! zif{S6FkbgU`z`MYeE%uO$Cn-b4LxQJ|M_Oif2`)(bd{MsZ%;pg_j+2EVrb z*_zr;a|1-FZYG7t5yDS#@fE&gTLYPNd`}2r&uKSU%QhC7@|R0`b4Io~7ETmUfu!ZD zygca(rJ$ioPq^<7jZQl7SL8TF?!_MkPCze)9q!sCeepm<7M)dQXYRbF?v0#(c$7~z zTU(WcXLL~D;a~it<7^X?!%KfimRDV3GfsqRu4E_6n}!Ut;?=O|;$U!m{>Z`6X-|M2 zfF4Duc&l~t!O74$ag{Mn#-CU0+v({^Gb|cG$Bn5Dx^}j88lrH$?GJWclc;_g$aqWd ztI=U%&sK1`AFA@}hIjk@KBSiBondKcsy8vm>>Nn%$8+$&L9SpdvQ6v3{-y)x_4dI* z=|1_tODeyY5+|~J>xrQD%0o-h71`P5u6g8b}qm#wH~03 z9UQ6I0+c$xL&yIlEXd&6bz2fj$89aHG={mBvi9nJEI03dzCEfp1pantxP(0(Znh`5 z41Xq&=8D-WIX0}>_d_pv*D)aq-(e`UGh;aT8>LPA%EG?l+r`&s&Kdfo zaABd&{i{S{dknb1SH68iX2EED38eF{daAVY+WzmvaN6WosKj%y1&r3&IgGSY+mecm zzME3$=WaF4Jl39WDfvOXU9a&->*q4Y;k%8BPc%eoXYR3eh&I3gqkmP!M~_J&+$PTB zP`Ulqge7+t*YEuR3j4HS2_McTiR4s?-n6aGji)ehlxHjgXZS`wwabV@;y+_@xCQqh zwhA-$xXMsfgKIx)W%kSnYA<`EgL|1p|6-77-esz|9m_q!=CtlDaxA_V|TIb;oY$wNWQ3xl@RHoA5}hHU-(gE*p4y`x`0q z9=Ba_Fc`1bGi-_Ld|v}&8TbBn2q$V$bkerEu?tiig`)932v*g5yA$urhgbC&j0Ir- zNj+dg^WIj*G!IP2Yi>X_-bZuSF@gH;Cc$?aRV!_vMIv&`SItUkoIY=N4L9cbG;S$> zlTRCC?^*uN_^DSC;S7=kIr=x@IKQL*)rB&on}B^zOsucZeiq;@zX>Q&d7u&Ut!LMp z7ep_HaHwt)?SUKxE{H^ZH1C#`f=R)Xm*`y}xs2|x_h;V>$FDT(EZdz|FrsIeQ!Djy z>%O~PgM|!ypUA_YZ(By|nw}68i|c_Z?W{X@v8q5&=f1i$bY`CkP;=NP84+^vcRU<~ z?zKO+fKdl6WNgF6-2r>0rpY#Sjr9%5_1?J&**ON(tO@8qqG#52R<^+QN9r&%M#g*P z2Giz@8g7ccS$#6C`qyjHdDP1He$9K<%Wn_bY{JzcteM;z#tgE>1ReNnSg-*FZ4s_z zQ}c9k!1QYr>tIWrvAi+KXV!w~bxT;`4xsYoZS@=^>a>$BS0rz{^hnS;47D?sJ3n42 z!W2~09tOsG3qvyJ_zEkgwQa4RhADjF(Qf)(!Sw~$a#iV1s6MkT9`5@k^qY$8O=Rz@ z?e_SaJRYxiwmPlHJdr^P1(oX@%~Z%2cBSG?Dxdt;z{l&Aap7-Ruk`MC=KkAWxRU|< z>%;54YPgicnslSyI*^{0ZDpBg&zzcCtB{?>D;5FRAtm2_>$?w_EQ zIPt2L`eSRhxK;w+`r}DKhrvE+cZF}KIlp}@+95t~|C1{mdi&NB`wpPY>)w9MjnW+N zIBP^^`O!o7KoipnSfTFha${%At_Od! z3Zn9^7AT}SjfKH=o~@IVM|FSrfAQgOEjlqt?=aj(> z|LZZ0#AlCS2Zwz41#CU*9JohhI2{W{86&7*JF%xYmBER2qhNl2xlYZgHM$kRl!{0o zFT51+!_W`92IMkZCwG-QK4cz%*8UmwvHo}GSL~p%ZF{SK)YZRv?U9#>khz<|JRieU zb4X7LZBqr$>dz)d$VuIByxL05^6qX`YRPd8HtDB*F9P@a92KG64-Fsk4N$SlurxDm zJX)OctQ8#xOKV@Lm2%2q_6jMFZW~nj(r+S}uf1J%L7`LFtZ5X_L2Ys48FAs6Xv)|= zOGEkW=uePJ<^?@%W3e&@de!s&R_Ia1CsI6S?9D#Q*6+WiawXf@KR}@6cgI(6 zSYeO_x6AaDY2mDQU_8O5XP{}Xnm+@*|K{ZnJe=;8*KYn9tba^i7{2U$f26deL%MSo z9kbm%*V@NH&AcAA|P0sZoEXE=k_XIxkuMCEAc0 z>Yes@EY>#b=B73u@A!Rw8`B9KIrJ6APA(-{I{UlQ*U)ijZ7f4&KI9gjHbm@oQ~+@6 z0$LKYRwb_07l5$nW^blN)lam2c~}XETc%NOH##f;H=Up1oV0^CYq~IAomc+7Y`j*h z6EylO_AqI5kr&m=i9j5`26ic@q7|Uq=WWxOS5A~NxmQn=`&ow5exo$G?Mp9ehPTJc zSky+3N!FP+qxrbTT&^RO0?#3l?FThr-3$E}>F=2JIRV4pV3ol)-u&ihV+#1SjiJ`l zkCRfg61(9oD}LzO-V0%D&fEBL{k9dzN?acTA`fUr47(is$X=&l!oIY8`#g zJ~CvzHp;8r9UWYMVwH=%&3fma|9%*l6kluL--3|rz27wtA6!U`!8$15me$1Lw+{d_ zBfH}sdp;}kuK`^I&L7`onr^R;cLnArx{F1cxw0Znkoww9lj`8Xy&Cs_E76AO| zJR2Gt!{unCBgLC~p+w8$gP8(`>)L`W^|cMwq%ivEqj2uq3zviV_M9hZiU}LjFAF&d zOMF(V5mZndV3?og8N;Z|yT;(TLKH7bz4Wh$-l$XN+_(aC?a0s1D}E4+{!+;ayK|pv*Pccr92v?ctrUufFp4nmIA#w)qtRV_P62g2x&`%z z;*I-*_=xs!f9>cm1diIk?6rc^_kG&TPa1$0I`y0`ReMwJ?&Wso_tT#jE5~_G7w~+c z%imuWXS^JtwYQdLH~v&e0DE=B@+Uls=A#T!()~SANU0@g+(woL(>uU#h4mlX>eiQ^ zcXa`zdf#g2fZb}Nr}sMrh$5%A#oKu`g%!;(@4^xU zyTLGNRf{K%9sT9p?Xo;mif^8-B%>zGIL+T|R1ebqDSWX`^2}|QDLngoLp2nf-|z>d zx5G6;>{wh@hlgU!ifd0XBiBYh&r`*U?n|8E)%(ka1J9!L?d8Vwc;0>t(8hE5^vJh} z>W|Fxa1z?LOk1wb;y8E8UPPpS==Qgy`!amu*dmp4A;pVzz_xK;Fa??YS+)3g$_|~w z`#T*j-}XWd=(s>UQ95T$-}TNEgkVzj#BMg2dfgT$WyEvbf#TJE3RRGYuoaGtPm)(k zBH(J*S~ID1dbD+2ZqWB)AEbWoK07Aubv~P%0mKy4pVAd59&Ca&{&qR>%Vj(y(`hi( z>ZhcKoiLEx68_$};8;-5IXd|~rzEzvt?Ra`JQn5Qva@?bEi7%)z}A3`(hbc(kdwr= zo7%~3Waq3tXd$dQ`_{SQsTmDF_Ur46do=#C8g##kE-UXD{?b?DMV?K)A1gao2gCR0 zg5G?_*}Ob;(D?Ob$kItD*Bc}shvVWJu?vlHVE{J^F+)5up~ua*=TXUDb0&p{!Az~M z#r8b}B-D3uY@E>Zzc=%F-Z)ujPklAJXV7n-$nRpbn;dt*&*?!SS~F(;zI1#@O^U_m zLmH~1G*=l#6JdT>?1ogvnzXY^df4jeaZQ6lRJX11)b;!-#>d-wlx)QS|B$ufe0i;) zG4fh7$Ku^YuiBOPKpK8EPu^_+yB-~Q`|KWoyljwvtbZtYsCB?LB&@QHnXspz5IRbF zgR!_Q44;YQPRHHi?75pFe!NjKr9#_Ux1fh{;rrP}jIGz^=!UP3PU7a5K4vq<{7`2O z`p}9Xo-#QJC-zgg?W50-8=AeFr5-v34u#{oNT_1l%)uW$*cmX@qc3F!@IBFz^In(setS+sbIK@s{pav1ZLTMgfOfwj%>V}Ew5F`LTAJy1fqqfIVO z$%C$Hsp>6?kj8%(=P*eEoE)AaSx}XW2S6tu`^oCU>@*f%LDyb8mwC;!{6PVk&(mZ`8nxbU+VXyUnYwzIfp)Uk_tS6O)GaV ztJdIOtuk^Q(^UsvqHq^b%3QxB@xeOY1idfjQElSrY^_(LiPs?J4j%z)CahN9*-z58 zuXAf)&h**ML9)~R8zr>zpco944K@8fTCRHUO*6ZOp^H!74fo5f+((Z@cf@nM)8sM; z!4pczzsvR$1jW779}%Ob6*}W?wKozGOmxiiY_4ZEoujbp7G3(&Y85`8cv$~S^0-T! z2YfSg9!kWAJl&sesknbtruoZgHry(BS={#2>U%uv&vxm5`3^+y72Bg=v2V1UXEVR! z1|N{AHcRM~{E??2fjL=p**IS2f}c-EGjdrW(%biakxqY8ZiS&yGp%_+BrGiTa;m#- z7P;|I+t>=Vg!_NA15{PFdJAazA>N>KQa8lCd@C{u)A^vJaVI}(sc@B_X`qx3wH{tx zs$u6=>Dp42QA{8eA=_CR=EFC1U!Rb>V5*T^RJ1$g)t`;p(L8>sL^&Vdz1i7+Ox}xj z6A+mI&%ShyAc<6g{zq-r;(OM7&eM%>$8j0C1J`9*$|}Q+M(X*Ua=WW2xK36_k*;?i zON{APAB5=r#n1JZ7*e@S*X^`V{P1F5W@RV{|J~}3eOfqI2eKu#7Y#(12N*V|@0kM$ z!_MWYv~f$f_wV{#vpo$WCj1PWlmjZ_zn6SvHqS11#oy)oay5~2gmt||(Ah;aJu&IA z(gPR!9a(H5maFB*?E9=9$L-Uj;jt}FxD-r{t3IF-epG?gfcE@re|S@7n%bn^=fB6l;ZvYn1+pLTs~yX| zHw%VRIE6-)mMII<$+)m1r*Pcq1KtkT?{Clt3`1#|Lp~FD(H|#`(-J4ETw_2rOZQVi zlzoyTf43c-@07w#i>s_}c~YVIehF)U)*^fAZ+EqOTg6MIK4c22W=rg|*YR$Ls6b8@ z&n(L14PY~1tW_h!fazSe{grLs`_7b`Z0V@_((6C!rS{eVOd~H#W|dbu;GwUbm8Nn# z9Ae!XUz}Q*{vpcvdpvb2LzCnbaP`o3e3VDP=pA}kZPUQ;#&a}@6E5R~`n{u4aXEVq zuc@s_!Cx`f9XVRN?5C~I>vc2jt>LW~8WNu<#c@;3*(WcRKpB6$mV3@fv(v%F)f(Jm z!#Av)|6A&d>LICKNB2#l-21U;G}rh|(5>Rt${)kLxYNE6{S^wSZB8aqw8MPhk_-ioA!aW zT4pB8cDu8=d|*la_*7e4`IWOseB~|ne;XwW!3JsFj(1JGbzJN#`ed%mb${ICg#FdJ zxMa6QlqYkxpu>oxFr|>&r*w@f_c>F)UVqa~yegbbj|=*8sdoDEMwkc0Cb!mbX-pTd zqs9wdGVZ0d@O~|rOYLOwiQyjq!s**BEJiIbYm^#0OQHYnR8?6Vaz02n*NAKRwE5SxfJxwdu-cRHq5D56cMCo4K{TCSH%~Ee>9+Wyd z-e&+E8ZyT&crM)l2sdJNxPQX6N%QXuJSa~MQp-^FKTF|JtVRZo-Txzv?KKz7LgR0F zZ8U%g@J0Tm;q>G4@782^VF);4FKUmt3K2&3xsGZ1EXG)Rh!(P+e{=vYkF)v+a$Xk) z@1OP`rvLqSTOY9SZ?_hGqbnf7%4wdZibH@<1OxEt=AMm5vTG3pm-Dx|Z8T|UhjJNo z94qk{I^;8h4IHXvhtyxz2*Ph}K6M3t$68Cy?i5-Jg#Pz>jOvSB^LBu;d1W(Pi37pq z#2}}3p#=g-XAR8PqxPlV{|yN}$=?0zA$iF16kG(0)@_Cwa&rODcKFon8zf>HUyUJ` zyyjMKx3`XWr}JKBu;=UMaJ4dlKo9OzfE?7RdLQwBR~uA8Dh;B z6i;4f@*!sAeu`Rd)jyP|I z7X9k6nKsB^$m4uTj4zx8WPVR><_Gh*Sp}HSot(KRvRriC#f*qDMV%fpeeRnr6x+yr zdl9Gs6CJ1{1n})zfQyYb@BW$D?%}*6zvl~WZ70}^j(*QkmSH9*ZF6pBa9?I0C%z|U zpLCDOoUV=YTnNfv5@8gW98Kj;u{NwF^OD#g25R*$k`L_t?{+NN_ZiRKuzP!Pc@8e! zQe)68E`RC1nL^~ecvfw&eTnmGX2722MgP*^km7S&x(V48{e5*(Qv*ody#!;#oIfLh zE7o;Czb{lD^qesNMX5*(!|HvaPKnX?Q-8GT=V$^z3EP&;R`4seY9|uBX4g}tZI^kP zp6TSmu}{3)ekM)Kj5F8Q=dfo3tXsP`R&hRVMfrB+JSe02viCtvyFGwt zFvjc;F;Xp6#*_9t#_r9H)Na?g$Eh#dIq8+2j(@ksv*FG@na#7;WC-`C_^Nw0pE+YEG9=Switl;FbDDI6 z!!t>2T+~=kc3bU)6!LHDI%&6aKl`&y9?{?Ux-2fwgmHY7byOX4AWeWyTWsn zkml}`pO4T2V1v=U(ii(Vc9(5iz<((Z4U^T8WxmINcXr*xDzznbIu4>+?Or*zX7=#M zoj*3n4&b91Vv#a^bU;T=rK|7ciDjX1i(M{kzQY?UT_iQqzkAqeyhVsye&aMZ;vutq znY&&Nl&Wt6K%LD-CmZI7Q~pk{F=gKuIdQO^_PTBC8JDWfI)rr8E0AK@%KB&pojMtP zr*P8SAa&WL)7;;gy+gX`S21cUDeTYSJ01nf{eAm=9zE*vD77b_061!wdCZsUyYjHV zi#&EVh#xu0*nI)Mt3%dL)cqBr(w`*}4($r7x6ewcvly|rVRBv9_b?+KcP5kCd-}L3 zA7MZw<)BGr8Z4N2Rco-$tvaS+OFY>5Mz><`@mG&iLPK_9XEM`NVzGSmhMO_98XQ?Z zBLf2(m`k^0#^cw~?jh~X*|W4S0EJ;k`-SP60N&K{kR+M!k1S*wDd!gZ+@m%x=(Yxx-3No9W9cqWHm7hBA z-t+U~PV>{YnHb}u`!)}rY43|H66rjq1p-y2|MC4__9pd4SkM31``yIQCf2^%n0>S>nF=6s{FH%o*7_@u*0X3& zx5yn^PuoHC1XEd50%G6#(|ZkH1;6p%sj7d^grneT{8W;8m5o~2`Z3>fwmX?sb*lE^ z^4SS39Z|H3mMrAh`aD}d&NQ9@&<7IV|M+?97!!+fZ63mL&&bik1VZ{rtd6d*jGRcDUBen8G2 zE^*XcwI?NJi4FrR%t_=}mG?FKH_V*~mc8BJW|=hqvOlO4Z(9eg&PH2Axam)A615+T z)naXyz{lkai@)hOtLq0RgBjIr`S$8o{H3|K`F#b3$Apnts>-oFZ|z28W8Y0>?xgOV zdJ!cHY7Nj=x>ed2bsT89@*^~;LqSE4U3fD(8`@YNDkus{G^ zRb86%zuiUrGC4uc7z1yaV5kXp5-ngeERoFFYuR5-({A6$!<6AU;rqN~pSJVg#Td-@ z>xrHEunS_*;W)dx^Ec88K#84uMdU@E2%#tTzwI5{Y#_;Kl+&;hM3-B`w0%B}p3kyI zw4G~wyi#Yi#&m*`@Y!c0UZ+}C<*;R58)Y(sn=n5txk>xW)Xw_q;`}Y^qBf0qgb)4h zJ;(p>rsSgJb*bszfnL=&OEHP1<8e!w;bADVl&u)vrx`yQrb8+QWIub zLMv=n*eK;~DUW=y<=h66+_!jmue-F-RD+O7tF7`3R<-1f&-CtraW-fD`vYp5<`cpV zrE&A}E?F z*1x4vTl~(I`_r2r|B=6ufwt&mu5#5@lJ~dRE|87j`*EtRoP4$PQcmh!Fg86DyHYx8 zR9%J;TDD8~GkvwXp7FOyZeA1}RUO4CKORg!hdV->>>s?>3sBUW)iqp)8eYIxzI&m( z9g#xEv>WNgMT4(!s^b$5EB=z=?Vc+`SZPBA*_wT=?bb1k*^U;w*kS?C!xm>xFXIX! zLtS{$YDDuWyS?qE+z01hVXZ%r$|9SYwsU_P{jSw-srCd1udVRuVfzM(J+UTG4>;t< zY~2cP(H=3r@#Z}5tUG>TUK)Cq=mFc2Wt&tVFi2D&RGc3LDag)(7$*=;u`BY*1%K;< zn1!sNWt7}76+|ukH6K8P?QM&QH18{%SPv7$j)10GWD7iYc`U2#Qn&*dmcnL&>#XU* zW&71y?K%dYJ&gmoVTi`DRGkX7Mm$>mtK9j`zs)Wa@W^0SbG(NCko!xS^wE(w2HLCS zHr9*Zr-HuYVeJNDB{a9cns=b~H9?((N&m1YudnZj?bU0v6d8C+?GtLDpt)VQD90z?fRNb4p}~Dq zD5fo&DJH`^bdGHdwD1# zE>(I|4jrs3#Zs?ho!j>zv>B^I_AGFvcOa;ReXaSy4l64Tpcvr|4J%8Hez<+U{t=yS zslH&dqx|(F|8+*4VO~KVBD-@X$m}xp$)N&}qHrS3bnQ>9>%B^`RmBQ+?Qww9oqYbZ zJNSXTwGO-R3FH!KPd*}}a`Iv#tFDUWdCzr62GTfImYo{bt_I-4V&c{Cgsy%6v3MQH z_FkD%&ROk3DDpQGGtYPbt(|P-eynjcl zKB;5eY(a#|tJ4znJUdZPO{fG~*m5YB4*C_lB+Q#l9v=-Q<+un|i`1WUBznyY%AQuA z_V+exBg($n=rK|WT2L^G4RV0Dp3N4kkBe5G<>c|yX4m&&n;Uw$wEE7sIhcVw(!slT zdx_TCs{DLAT$5XyI6{|AdK5yW5w3t5tN*s^+^f9%#k|yu_Ii8s{A@3j8~fYx)FX-z z{*mU_Ja*}rY|Go(*p!6qc?9F_6u3HvxCRo+=vryl_DBob&2hcCrKg{tcRA9_bLAIY z^CSE|{QZUAf{HGWzx{$9%Zn6!{27^TlyB_{QY}os=QM5a8FU&BNO3AJ$EV6bMfSdz z`{d8jr+v!VU4Ju}*RsBdHNK{7D9u^6@4cmGN@F4Ld#vqE%4Lg2#@vkIGM4MFvCK{> zjK5uFkahOs$%v5<_agE|z~?bzha_6HsJb7$H#^}@bpU>{=vwQ~q|hm%+tA@^WvIRK z6PfJyFkkck^7D$f(Q3a<@?Ks<7Fw z7m8iA8r5PRU@SWUyG;5&d0_R6?ye5xSc(~MZum@}!2E(7O~)1hBE+)f#x}$E#qz_S z_<}+h=;AegFkIiZ+95D+Aa5=1d!1W_az|G(y`s+aq8+iu4l zV68dF=w0QMin^hZ;AXv#E*`zybzhjT7n5(ul$rr?tLsZIF?&9xVuPWM+KdK-0n`&HqJ-3!&|FQW9RT~32eEO=xe_0|4<#zfV5 zUs-l8lOrtXV$r!}`YYnJME!8{A>#f!w2#K6?{Oo&6={<2dX~rtD?Y=h+0VH_b3Hix zpvls>R94!d`s8zBmvLBl*F#(C{ilD~S(@>e@epfX763)L{Gp<-Ft!}ks7HW|I(!FX za7Bzpzw=*Lz`vU{7qMQ5(G@9oXmRo=EIs%S-2*7sxulGrm)*S-WQ5U0cU6hg83md> zHmg7-eDU5Kl3|_2hUFa$HD0`3zXHMJ+;if>NvggaTsBF=5HD^_)9n2zVjDGGm+`#;F55H4w zltxzmv1@hyuqUmjGaVjtQ=NI;m zjx~9~SOoB)tGX(k7=`ZP7H$8lZ17)==PajV0O_Lv1wh|dwvyFG>O(Ziycy`8yczaa z-RliIzZPn_cm|L7UEKZM4PTYB%~74wJ!CUrUS0pt-k&_qTeUj*!23F^E^QfjUi0 zi2>l@S;#M|$_s?ed({JXeOn6_6)Oxkh(nw&zlLlA1 zwH|4`+<9XfWrm2H8+@9%A+Syke>B|p{a){NU}g{@q^B#<$?a5 zyM2rA?tlU4jMepakoqRRU;)yk4(BQW(l`4SVYSC$5%vHO;N|P79Q3T#T022IZSyXJ ziF+qJMj6Oij=KQ(sfl~i*fKn|nHMfs9EJDfZ$BBvOB)Xfp$4L)3*CGJtzfM^UP>F? zZm;asyXBH%t(~1D-R<$}^-jV&Q||rQ{SdEA3SgQyK8Ne;u}PsAXUdD{amSd&@`a)E z*~H=V0b;KT9#fVQ79CF+^0ns=a&}Vcz~79_az+zdAAx0hyTGTa0uH{olLl^Sh$IdVR@_qX zduwx$?{(c}qemh&w#p0uLHy8c<3a4?`{IOMf1PD#6x%l@dR#K|+E0eH_G^jnV2o_v zo5`O#Db?l4sKCC^@Ee{R%m6>F(s8TC6^?Wp&rVrg~G?9L2#KfL0R{VKB4 zPbTRRsyv*mDYf0`sY?J+1tDA$wbHshaDBnTaJ6C)4F6^YdNE^?`Fq!@!F;x~vN0C= z;p@un!Xq)Dn_NB1)SH=cUEfzu*$Zq{)=aUVT=Qilua328T%Ap6O79vqw0ORME9?k# zj5lMGTN|$RH>WP3*n5h0Szby%7RXQtf6rs4lX0D8x!f|a%2T47o)t{LiqU!m-$u_) z;O-}e2!F7%x$>p(^z3^MrF$gan@9K-9Lx6H#q1mI;8RMEc(un}8?8)QtjT}N#;>;> z9;J16S7T(Q7V&*f^2VDS=Mm{+PE_{Q<~ejg{;wR0v z9>__jlj*(woE7ov4t~|;3K$>BdJL4y`>Fn@l*)b$_Yu7~DBztJSYr}?Dq_g6oW1LI z%PI-U3_q|p*0-qqXI6i}g##lp{aTBy34?qaFzw}L`}Quw{&%4WDoQ@ac6E6i-LF+F zdCxz8DVocUkLjcQt1|+{Q?ewNxJBZfkjGKz8&iL?HLjU>sso6ce0L!DF*yGE2z(xT zUS>ClA$70%ffMV*avn@=^gA)pKO2TE$)K~56UcSU%5Qy8&lAKim_|Cgm#)=o1~Rk( zaAZNG7L0FDUK)AJ>AC;R2TdCQkJ4|55*%BxqubniMHY7c@P21CAS<@6j~ph*(w+yN z;HeJhO7?L60-FVY7OGXZEX^gFUdtm|c*d#e)}=~Ok{cHtYJzqCFk23_M+{Q?AEi_` zn+tu|V%JYW0ejni)I6=7+y=Fr=0^laA%<_P-DOiw%%!EphsuiKyZuye*^2P49yXo( z%=N~$7W8I+k}&Px$y%PrnB6x zWG5bMI~=LbN>TCDyAyOinUML-_($I8!r0ryn*@y;Xe&$KKtql>R8s%$r?FF%D@R?+ zoaPNL%PxRuFk$Z4!5#pxHJ4sf2N_+=)D3eeV2~6Wc_U^Ju^w)zKh$i^SpXoc$Q@=m?Srwrya@jwm)gOa=7^S;mC zX3IKK&b&M{R2fZw0->|<%e7-JcS^Xy)hf>km2B}UQAw@sEfS8}Zui}@*hHgquNq{*~Snvrq!D`1NvF+JduD?9pdIS(oFaU}Rya5b&x2^rY9P;MlcHTsXt} zaAb>xLOhu6*8a{T{mM^$QcmDHyO>*YG{G$7M+1V%ZaJX%JDkS~<0$^kU&VFV3HNj{ zobPT|s((PbpUg|z_R?ozIMU%KlYN(D^HuHpH)_afr?>YGr$z#=Wn~X6gjVTc#NFH` zhr;ZXs%^e9EUDOH{U2^GxL*ZV^Y#AysgfJ~z1S1@lQq>InyBLZqKeD z^Hh*~vZvbh^Q)TQ9nu!)2&xS1BmUxj9Lo~uKPpmhrURD`@jZvM@7atd7iS|4m60Mt z5E~6o^Ao13xae*;(2eZ{na~b7C~*576AgU@4drh#{5|gRb9P_^u=m^Z!c4zthq|I|-0`zCDT(y(e z>dqB$@wg#07eS_W$m2=nL1YAmbvJuSK0FP;SAH7mV?N^+EJMP#4^L{;_A+7f+1Dq= z)98eKio&BOou4}JlM8)=3@S8w$Lg|)#*5&pr1L?h^_7fAPrt>7*EB3p`Ne{2o?5YX z_RW}HEdYDy-`dH3SgYNHvYH#+w(0j4tW8fn_b;<{SBNgXV0R$zw~F%)>6!^5-w@s_%2D1t$1- zqnJ+$426C}mcf=7;0F|D?PU{F&qjvtI}#t?ae&LH?G)g1~H z>A+os`MW3U-Boq*aU}M0Oqrt9M<{o?t(Gz`^UMZ^omyN%;@lKBs~6+x`Qpo~td*dX zHWHB;Jm;nA{xBB~(7#w_0X5se@ z-OxGYTHhL;Xf8Sl8{2L{KEpu@xRo5E%-Gyyj9C(;z#)ebSBThD+y*;(K|6->Q2$zW zx;ckzN*jA_AlKQF;`-r(Q>D!yz z%m?ackhMwC-Y~I!V&vrbZei%gIfAX!-S^{OF>^=O>E!w*kQ;8p#a3!yp7@=e|*)LCX&su|ec(C6irg&J?x-pU45 z#e}T6>&|=`zB1b|i+R3WBen5F=w60I@1OvYVP?-})%d0ibm@Prx zc!TAV0Re@Q)E+4)qc*%FarZx}ZG@fqynY}syDeoD%|6O4Mx^%b)X0zqEVM-d-!1s?o zv^YN!Z5+PfZ!EUHyIQj6(3Q8Aw&|VRwY2AKp0}Mm(8us(eA;CoZP4NGji*@GeveHS zHSF$v^a13ufsG1|OSj{dJY`U;#q&sySB4DE5ISR_r0-U?9W3pGOzNdB_HHQ~^gHoi zMyTdDsYx@9=eBugqg?T`tI?Hh|K{Jkn^X|nOt+F>k5*h=q`L8+dY;K!L9u7I<(tE% zpUcNCsP(e0;Kh-BzmNG0XkZ|}F7J%aaJ*$F{=7P$YzULWUW_@)84c5{UYyYr>dv<6!}d2gcTv8w;N9xDeHN)`_NFbi{wXim`ymo z7Ks`VWF37(BbJbcP4(}Z75 z01!i4X);*(g=rB3Y^L!Y1{ChhaG@S8!s_OgW9>U`7Co;tZ>|Co>{QqehC`Lc5<-Q( z)}?K?9)g2eynITs5e=H;^Q?NeYdtyB5Q1`6u~EVJS46a)@9p^dy7R)i385+!0M3Hi zGuQdkY2R5Rt;_Q8hhMUFJ16WZh@*INucJs6MyC41!{1+pa-(*vxZ0~LK=iF(mV zNelOK_(?h(I!0z13wUan*G$}>djdc+9|Cw0~C&R zF`6lCD&6z>#lC|bzT~tkXtbSrcGGQJ&p&d-ww>D}Zr=v$1E~h9^0k`kvkliQ*iT)V z_+05JnccTsesII2NHC>I1YD6m97}+U$Yp2M(dM!1P~Y}2r2=R5(T`64VZv%w59m(8A`7pZ{e(;eZU-F{^2ktWl4*!n~p1UQ%3} z9xQ@7@|=1tg!j+Et~UlG`nZqEPlkqmS$X2D%9B1(EvsJEg7S^74V}VL&op-4JB5>j z{t4G$9-Wc_zUd~XNpy$$Oy?1#Q${L@!@&$-`udxlMfq-V ztL=Plk3m9GK?mzO>?r%aCCL-^SMFTyp>vKQj=EE3q=L6PcW37g`*(E22-d5%@FH_X z>-G|rFb%m^r^vM2E!DO#Ibu0AL!Ya{Rs^y%!XvYU^wyot?5Oy9Ezs`Zn!*O&QjL)s z)!3er`Mp~J!Q69I_s7cV3cLH0*&KmlXY_)gywqy{d8JJLZ+`v|A|55gwEM8%lSl1jLV#>gv~@boZ{$R4HrV$lPCjNc2iVOcIF!b3SC=XW^&T{KEHh zTm1vvpnet0>2^tH!ZGt#>~DpZe;CfKDX3+Bx6MJTba$W29}c|#lyIOVKWR64yfGQ@1By5GrH^5PVpgZ9v0)( znXSV3zQ%vE(3x@-JooUP&Y}r7kbCi~kG-RSKcE)!9R!bk7p+F8KG6%M$L0^vrBiCr zO3zw`j6db_W2AGBUiBq@5G!s*y=M?%u?{eaO)T=mw>!uBoiTDub_QU%IrcYu8;ejZ zcZ4igYl4y12RE!vC%J9)nITWD;vRJ;Q@JjWD&%k)H!l4&zD*S;w>v2d@FBW%ypQVo z{gUmP!Kp6XgcCg%>siP#y0LVy_&{)00UjY!p(bQ@pPf|33XAl&8*^`w6lihhqhCYrNKp`&L0eKj>lZdTsQEe-f0Bk2J$Bcq=_st2w zLuhe7=SHPUtyMWcDs?y>SBkC;ITs5Hvrhc{I!X?^>pn@WM1Ew4Lf*J-l zndYv^@kV4W^dNgQtRUZmoq9tFDduwVX4th)F5z;GWkxEc7=oY`+d1Sz&Xzbx^U1XQi2#lg#FM z3uzv8Z)hXrp%_o?q?xNnZ$2ySP=CiyrP?75V?-N#x!fyt)fN z={WOU`?^r=FwI~Jv8tZem-6Ky1KRiXb=n2B;mz(Z;uHA<*Ajc?v$6eWE^B&{L|J#$ z_$@q*Tg`?*&{~wb%87-l{E=Mht0A(5+B-OkIhZ81A<(C?qibLcHprS<)h58m)Y^8B z35jZBnf(!cbTQ69OZ11Xcwyumi;tBzx<7A~`%DkdKm9@DBPjDeC>aLo*REW7E%q_p z|FP?Oy*TeLZe?ifd=1&xN^}_4m`cMyqWmPObf#Gn3Rj68r_&$q zWx~p24i$iLE~et;tOL%bv;&bQcg!n7AJwfAQw}TrEnQbn8f{c)Jr=^5 zF!)?7E~cUW?X@E6YKaxAqHBB`t^*ScK;c&Bi&P(ZQ01yFc*bo5AE&ZNrQ>t0bzJR)j z%Sh7)BqVFz^yoq!F@gAF)sGlSOs;O?G>4Zj0w>cv1HOWpkeaeMK$}f+cidM7=T-cc zt7_T~fURFY{dI;`a;-v#=%}JJm!ei&|ExQ{(ic$ zsMFKTPPPE(SW4c^qwY(PmOrn9oOD?<&8Cz9o8koM&PjfqeWou^LWGLTkDl8MF9f;2 zJbuW2M#tWnnTkC+^e7oTaRuhAL(bPKmwDNZ(x9@AKlX>RnAKz>+-&-emm(NBH7cEd8-u~KMiqj%+ zfCbQBwtMA_Nq~bs)$DwoY8zl@z;XS2R@+7F;^Hz<_Hn$MQ`?TiLJn6|d=Me?D zarN)K5P^b1UwRey_a++Er&TH9Ie6?VBf(60<$t;7n%_cu#+(n7!Xlq+KQ_ypp0T{h zP}MM&z}v{~-@ie)QJ&T^1%@KJCTX$TSQN*lg(V|Xx^Z{JNWhQVA7Mk;lXOG#+VYD8M6IlD2>JZ_^rP}O>JaIS#lcdQ92#O61xM#+I| zB&b4}KDhIYI55ofxQ)%qK1No?^ar0%i`}DFXST*_wi^$MTz0fTE+JL|u)P=(s&F2o?S8=eq>QJ-#mUs^R))4vN!J;nKoe7v4K&&Q>S-1f&h zhr51s(dSjdS{|frp=owqpfGaArFcZ;GgZ>h+iW#$ffW0WmLJ#Vb&CJ4f8P0z>C`%p z4ojg%_X<^?8YsKXrEnhS&bfP1yq9m|GcLvIX8SlbM0+~D*1|$Fsg=^Z=eHL~Ml%bX#8X`}C`B3lA@SUBD{lb-I4l9k>hXLLPspq22g4F{4fi~-)G(9=aM z2=) zQb9fuATAr*i}p;qo{ZHxF5fo?2T^XB1ag#+x8D!W#c3@z8)4-*4Eleifi4en?PB%U zp5JbZPbe?GXQc#EH$2pfrJ4LCEb+avRo#^|T)t+SDiT0f0-jCSGTST7? zZTGTQuw^e8a4yrWm>RBJ^YAHA*t>EPFW$x}9C+Up>0X9#`;o%@M7~;wZddviHKeRxlAYbY(Tl zr=<{x98Bg<;AKU_dDQgI>1G&Mb&eNWLXG^EGq{vfOaNoXe^K={Zie4Vd)1-yg~>)4 z<{}+yT*XM8Ze|RtFTL!1&}N)`GcIUZX4u)B7Ty*g&5QXe?!e1ZYUNIu+*=-|fy^1z zIuuVH1${c3^|nFicF|viqXW^IZGmLO0~o!oJG1$O-=e1x-~LEdIv2Sfm#H zRfKF6?Kt1Tuwk=(SJr34Ac~7Ir10BcqE@i0ACtfu4qeO0>Oh>-_tG69k~11rdsaI# zALWAM+xDSK^xHxG0A0IWd2>I%=fWkM#|pVm?7m$AJQb7fb&2Wy)Qx4j zc<&s_VLzu;2KK&`2?i2U&GGA^Ac*f)T)0%lnR&)PM-`=|1e`!l>G_^4qrct$hgMHJ z;arR-3sNCeGy5>V>~8kb&zB)MPx|A}cK??-E46cre{g?l;e|=U!YVt`yVns5 zM3cPK>4%b~f>r_NFAhf0Nd+!P93%#(qoK(bl4r_*M07$x<9FHKs|PUTy(iit&MoST z7nhN+*=-RArX@Uvx8HREm2SW66N|FIE$>4!XWxqA8XG@HrO^0-PXMZAu+9CU1JB$X zR^4S1PFo`wo-PYS8M;ksh8zrREv>_J;L4P$)~=HsUOMBX>q;Pd@S~2BotRLy(f#Va zsG-ERxSru30&CyKOg5d0)u5X?uwU4!15P0rQu=kNa}cfCJiJ zJexknX3W{*?lT_T`MIdAHcfwnbZ^;lE8ot-gZ$jf3jrHH$F2=^0OC>bwi(1sdL=Bl z)qdMc@ps?Qpz75yrtphAkVNv>PmA%5!QbIe5X4=ya~yZ#<;EfTJTb18Yg3YiZ{{DK zbY5fsjlg7SwB`W7=^MrGy7;Buv*D~)S$pC7+p!;9z{1W)Wye)L^axtp7S3mQtzq9psHzhg@0%@S$WImocm!JM8KBU8uiHZ~F zO`g$&X;X96kwPdet)V8#?&bh%4z1=#QhQ(=E=;^ zF2|L!gftiX%Fh)mCmHXh=8*!E@9&x(&0G9Q$-G>#lwbx_%T42QoyC#>=jjkB{Iuic zB7UxuqqG~dT1S9--97ts)KWNDk6L$VMeww*ZM3z|Pvh47&x_8(l@Mpeq(P-U-n~`N z_4@qq`k3gQAu;95#UPSP|I!B$tz7nRpRH%Q>;rrFhcG{Af@W_b_~>QZaV+E)hs^F# zEY(Q95kMNMmK2}OPD( zY7vdTq19kSffG`ddy<#H=mRri<68cWTC0I9ZLK>~zHR^l{wgZ@!CwVa2PJY?4D(^c zA6}U)L)KK&Y&|dZBj#G>PH2$ha!P%*7vv9Tr&5v}@5e47FV*E=+vw%`3o%(JdcE8< za*Z82ob`3p*;`n?RyZf?QijdZ-fl2#;!y|lCeRpepdu{i%gVF9@-U@@e`OfTct)4- zO2vZKenT{$o*p5xHQRH^yj6R4wg(&go2*yEG*|3q)?{~TNH?WFF4IN{U=HoA0}(Nb z@0ay*@&A^%Q@itDOPs3plxY75?>p$K7Eqw}ZHr!Ad-SjSu3wFGomDar7ai{yU8EjTdt`TIziRO(Amw5g+UPLZ~m!RZS#St4^E#yR{EVi zb7TEm$gCQZ1v123trZj@_uq8M)$sK`R2%otPqd1V0&RPAgUKU}q_if_FWWbr__uXt zrRZ<%Kj3ct<_%iquCG!t{&u*G2Hu;*cYk|%YQeNB{v6W3GF1}_>tojB$Dl0Kuam6# zy~!X$LP{N^ZyrDx)AedWy?Lbe-(sSYXUn%4pC~K0LT0N-vlryt6_c;?P<$8-O`1}1N|5;0={MhbW7xhllICpmV(f4-P^Ia}cpHR)ueUt`$@=+MR zovy;oD-Qi~#xi&U9c)@SF_X=Pro!fb6kUl3X?9DX5N<_I`MG(Fe%GOwzWB%0tHFOp z-8glLB?!keL_ z*mIlvL9|1Z^=k?hh%v==R*XGOaRtSESW3NPrdcxHzdHwtg#Pv%Twz$eSlrn_dfIOj zZF;h}e4>9wNnea9DmiYQfl;D%CkDz$>2VjchjRb+B3hNg4UY?+Sre*pUdK?8$gq%- z*`9*-J<*S@os$K?>1+-Fzq^CTb8`4&)WGlJ8|g@OVuK~!TkIko2zY+; z3}^*0R-AK~$7G}yp5_9j*RjPi`tiXe)!?^s7~r(|)AQdYTX6`V(zaJ5lyCT; z*LsycTK9Na zxSm=9VbwqWpT=CwS)DceSfMx6g;a z43PTyK3R|Nbw35X6_I>DwIwwD=Xc7mTGHtDn``IO^qh;NbISh#K09b%0xPNJE(4dh z>6i?hN+EJ4%x=)62enM`10%Rf8*n8btRYf(Sz()t)P!u$V!S}NIbppYEx*v+%we0| z6F(;B+4pQMf@tBa!W(HlgF)ZQsrv5P45R>&GVLr?DEIeYYk%^Zpa4tW>jhrB1o0h= zz0J93eVI<9F|~9;Ym(q$n9Nbb=>p)OBHQw>|3l~{*y;?}dQRP*qeL0%5zNXbWj9Qy z1JPWRHXb|R-TX68;HqqeKI%GcLZray<- zh3EN6rW(sz7ss_D^FM0?RexVs^*n!H3T5OZSnp0{7G5>h(dF&AJfjsg%7anzLKhLM zJR6;*#$sMW02Dti@UE~;GHPV@yo28EL&v-hsFI`kn{AD~4Fwy^Ygn}~us<+OdSb~FC-c|B}T z{zK~wC*9xwPgO`VJf;hcGqjR+FTvgCZ{!r)*z0tshV7%-DjXI4x775T4+wu1REqJp zI{woeSd>A_!E5%Z(D=dejmc=Rj1xS5N{<}TTS6@H8gmxvhOfIA&X;byYX`1s+BT?< zH~ISOt`_pVpTorWbcfE9v#c~Su~R?P;UJAR*x_r!@`5=sDd4@IwA}h4QS`$RH}mzk z7MveYI=al0RO*m}ruW>)1By5ixsEeg_=TZ3ci9~~Xk`|y7d4T=G=o&$h25&TTwWZq zzHD^Z56Ehp>-VBjLefLoIvt0#U~%aOV&wL5t~DGg7WHX?I+gtE+RaXJRHtW}l4*;V zdU)}`S;^kOHs1te;+S6M-)K8dC&?CarC4Ys4Z_8*J4(v-pP>8E+rr?1lWW%8ah}&Q zLMd&cUoD(&q4qw`qCj(hu4?mO=l^zG4PVLBZ;G({zP#!d-Wwx1xAWEh8aS(LfAgru zQ@42LlYCq}yTO>)Z}$mc1lWXU;p2Q5P)>)HN+I~WFz|#X;D~}tnk@e4*-aOAll%c> zCpF}LM(2|s_*o}Fo;vMR(8btoH>ohal;|gHG%Wqq}X4(D3 zn5^Jssp!uaUD28c`dD|3rxfL?CG^Ep`!}@)p@H{q3|0}?qu~WMNROkw>))56n3uOi zF{46Y{nYH63-Dw$gLi%F*5@>edrv7pAxe||19Pwa!>dp1Zvo1ah4Yl5(;N_g^&!8b z(eQL@Qsd=5d~7x{5z>oHj$`I8d+c{;apOID8a-4vrW7XItHe^ zeSx&ExDWP?s5KarA=6VS`mK+H3;q7|SP;}f-8@lvn@A@9WTXAn#hT#v^B^y50(kBn zby{k}bfwH(APPBD+coJO{pzO=98h$kU|p_?`WF9jbz>v^44;TE3Xy+1}~g zD6zBZ4pKbOcYCdyW7=%h?#w>zzB21?McWL)gNFK(+pi!ygHmts?pyrEWL=O=70O45 zOo7n8hRfH{A87~KT{$XI{zS%N&;e>fU4+_5vD9lZcd5;1l!;Y%P{DP)Eg_X*?b7|~ zbhg5@Iz7p1zCC%<>_IV!fJ*aUl6`8M+pLeo*DjhK2Di4U*u`b#{ny!qQk0aH zOY6pT&Xili$F$3z!R*+#Za8vJhHulvX|a{TR*JqF0euT6+jjQm>5)k>73q8z0(EHZ zyTik($W10jvb;;>dfyzvL%d^^#L&pjudSt37_+&}&YcPhlyq9C2R6^0lHV5F?ol3K zpAz)9KDakmoceZL_u4q1f$sBHjr@4>HFWu3E&jBszhn>Hj^qZaBicozmEsDm0ZHDm z?CFy^N%OTp8r22$d~oe?)L=_!a~{_zwa<>r=H(&mXRQyn2lW5@dw6C$_Nn5b>H<~Z z^z;J)6g{j|MMyXgq%{5-6$p}j{nmKwZ@e(5CbOBDW|AAMHv8^G07?CVG0NAk7PDMD>NGnQ=u@f3YA7irCNXO{fjO=E?gr~xXEVAVFI}n zcBhW3BHx3RfJQ(i%!qhR!Zy2DaL%W!_g3;|^LLVShxZ!5PKjCZITL~Vb0&f0VLsjF zPaM%dhZWxL736B=>K0P9v>l(%jUIlpzuhYM9;aAK<~iV2LF((Fws-hY{KH#RtkXdp zHHmIB3%%6LdOue{Y)>;TIi02P-}3f`gz{$4dQ=)zkiL^;k@Cv!rSuaITY2%$)wfIZHAlsihvGF3c*|J-DI* zKUOtGSJj{KP+O%IifqW)LaaM2(EiblWpxXGXX}0~ImcKbWBnGpj>W_^mAbHpxa=d; zV{5(V;tsF9vlHXWcV@@iyS;XoXy?)IAGhzKKMee)V*Lu}5<|pygPhEn{k&%RI}MueSvc4zOG#L%n9h&5hA7Oh;7($uO%0HnRH z0RWi8n#UvyS3J#Jy))Vod*5mxKjs_otFSBgH^2UdxIV%5 zvZe;s^!u@qa(>P8dpBJX(!kovwYbLjP1lP-yMtDHTip}u{qpK#A1qkr9N$5=rS7JK z>6uN6>?m(>PvBf#Y+mX@b6$12UxR84s-1V~=Yvmke2w4^Qn00SuQ!JVjhvH*{z1Jh zYz_94ztAWjwZ->3nZk5RBYqDud6_;N8qJa-#@waYH9e~;cY^rWwO7!Yo(3kJepPZJ z7d;5pLO!LvhP9deF6v%aVKs5Fu&YB{ZcM8usysE0MJ;)Yt#_ZsLRaY<$SHOl%ywA? zA%}f%;)dbk*#KwDp)t7lM5fB0P8zYU&sKD&mtP{&zxt}je%Z~Wv09;dIT8jf+l+vg z1^eqYV%3+Ftreu_Gd#3sz0$k)C=AEsJ}(Y^>afk0GZ3TR+`M!o|WpSByLf=+ukTI=4sd{d3hSV;7F!i=mL3B z*mF=riViefP_MV?@ zuK0@CgARn1=~zj-gp9Qq@zQ;hy-BJha`jum)~xFhyo#;CFR0(V*PIL%OxI0%eELO@ z8o$gYsddMx=p?(1#qxoqui9>D2bp*>`m!faDlVwMm}uEIv1S}zhEv*H9!YabpK5Z8 zFRJrTe=J^UhH?z!gCf_^?u)Epna_IIQ9?M$N_&Cn$%COPOZ(#dJKECCmc;%mLyZ6mw z>Q#1j4_Z^xggEL{26AUq?#Jo+J*kLTWzmoi5Sk0SMSn%Q zQhVO@x@b1*u}6$|Hyb8bqaZaB^}KaIbm)5eCXe}Mx65u?!`>|W{L667igPaC^Xu}Q zxpZ1k)moQ35az$A>-E+k#%rWGmbYqmTJ_h3@N>l;fIyz6m*-q&I>S|T$P3FEQW4(Z zcc$w@=%=U+Ae#O8<9AgUuzk4>3MB+JeQfUBup*8DZL_(rP;Ox-_RYfwtC{O=ts9oOOENm@>6<1OJMy z%34!AO9FE%>y_}LL2Pj~s$PuMQz#qj$h-e(*@v=z?=wu8&Q8{ZYpJQM6<|SD*}UM| zAn*q_cS$K_m@gVMIlr!wwV;l(H0r*S_nCVSTLrY<#+kAB;9?bxoJXsjypS-nw&97k zfAW)y{G-L~wzRvU9P5NS$JAchu>R8Z^WVr03e#?Vlm5M9?7?urTc}H6O@WI0K=C_xHfETMOuRuQuz zM}@|<^;ROnSzC)_dQxmk4qwqD-kLj1Dm!U^3rLqt7!e$39Zc~5P zilZq}WD|g7%qFYf41$p7O|R^W2xy9XH^@mb%D2Th#(G~el@@u|tBIzKU)Fp7I~&6c z7`Ju#dW^Y>IDZm127rBLI#g=UGGekQbxddX|v z8cg95QMH0ojgeBgbJ)naTOBH`na8OK4Du*rhCz?3*yF#!^h#*Pj`-_kJgw2j~jesYx{>wXZ&>BMV86>4}hbC zxlkUp+T#wfoK7qK_h@=Q9)`;`%`rc{n?rL~gW%)hM~slugV4uU16SbTK|xup?vm@k zEQwJje<~34=+A^#(8_7zfw%{I>Z$PPlT+BEjy@y%5PdI_A3GH^H(WtoLg}1vvW5Qu zq&At_i`k!Oz?HMy?p(*u@hG!sOn1oS3p~ zPV)A*xYvf4Hu9E$(M@+&9ll{}V)*wTmd>@^RB#K!-}@ItgoKi_C{d({P9!PE&;j%R z@3HpYpVzgnYpyk^_Z{PT?i-qo!D$^#LLpwRuN_NK4Od zoa>#^WgoGJRQrcg~ycBXslV_WIhPRoCqZLTa>W*RWruAxd;WI5xT($WZWbbRA z$(*A*p$8mliLw*i@~UcZz0m=ySBms)-Vn9fb~P34A!5r|slyH$+3xvV;miTh?W=|5 zu(^W+K*@e=2FY5_Rm#;Mn=A?$tF4wG{9U<>HE$4+r?JsutThMjPe@)&)m=PB5~o7_ zbjeT;+sc>U(-F)Grd6THRrbweQ>=i6>2$wvJL-Jj{x#T6$18wq^3aP`LQPiKe^LVA zmjf7}3tSAKm?7~VbIPZUo|~c|A9z-wnG3Uds=lG96*#t^-J+*(esR0QN|*I%d8hk! zX5M)G_b}#%zajZrNvCT(iYi2J0A73Ke#;+cX}UPPYV=v0r-{Gf!w3Foti0$K)ZqeC z&L#KLm4?H2IWnXOo^F=?QWhp}66J?H=&GW-$Sqzk^Yi6SEu;elr~HyfNvg6Qq%}h^nx73Tr#`X6BU+(7?3#aNb3VknxtB~NA6HE{! z*yUeCwG*#)E1XLJ=JN5&z&h&MKGs-^MYa4;Jb=;)~3EUMBe z9*;HVj7$ovYd(<7?OBf0Q*W@oW*9`_89;JIF9usYO# zX1Jusc}K4uMnZeL#L98ipF{-G<*5a~FhOKeo}-61219)=S9!ere;B;!sjd#UFSp@W zJ%vg`tvPB7Ie>?|YEbbo49}%R1;ff#-4D)A)v`+lV1C76vh9=<%{M2T#jb%sHBL#N z<-jm?&?m9}VqL#FQAK)8Ys7t(Da*{;bi^0ie3M}?j||~>O&5!BEO49Bq<^Z}MXcT7 z&s#Xo(%qg~MxKLp?lv;IggNNm2AY~_165d>8E*naS@9km*q}W44r!)8PSlVJc# zs7@v4L?vcffzl6#9f}{F!&gI!e|kt=V(K6W!Bo$AT}tk_);y()5)^6Xg~JXQ!&#}3 zZh(O_xl_Bau0lkBxnxU4tpCXzHi|w=pDFM*&fnR}=r-szjwFO#w{885<;#z7Tq8y` zP;b?xkB&L@Y|}Iw2Ybje!ka5NZeQrHzihtE!<#2uUaJbh4Rfz>y5|a$dR^WaH|2`N zGMwka&TrGz4C^+jGs)Ur3};N?ikg3iUb+fioFsn?-uW7T>h;#fWgM5YH-YVTh7UUb zXUG+WQSWg)CcuSAf;h58Qj4N3Y=EZs+@YMZSVzALRxp%Irjs9To(Aw6IZcS|huAsX zgsJ9^DtQL$Ra&3Wb;T>7Ke%tre;w@mRM@@79H8haQv;ac2U*;Tf!VAbGt@$x zRaClN{84$gG%-s|D?ULuD^|CUu%oO%?Jyz>bHXU8b3-U-8Eg(BlXuI_?chUfT@rJ8 zd5CL->q~@MK3r3Kii#%aXoBYYrPlGOFME^4&S9ENM$BPs|C{EiN;zltc8)Z9P(*3t zL0$iK%%yt8)IYO5_E6SWV5aTvEEJ#~P6jY(#Yk!R^lb*{EjT6x2TwD_)%qxP=a;M8 zSteTU%kCPA1z^+J784Yeg6v{XeLgcgIn14hb!&tej8Jof=E4~$qTZ&pR;R^H%lPuw z1k#8yDmBa<9t_?Ry}5PU%5vXdPSXt3Bg(!#YB_hT#lEj&YARvmFZ(x>en@G0-xq~K zwiTfvMh?^LZWUauTXkB`@}XNw6Ts0xLbDq#iiN|#`;5{XgVj+0Ymn1r^Lhe!JF5Hq zeIl~lpSy^V%ZaA?*`xn$$(JBgnlf`PtIxBx3Cr_gY8*T5`gGst=GV>(I9j-sQ~5XT z_O`g=-^o!Uar1nul{JD_{u)88`-_Bw5YRBvC zruoJ<+b$E*zIYTG=h2^7SUr2+_Bi^D3u^|X3Q5%H&mVUFw?UToQTK2J1}rtvk3(4M zchB>2YuG?%<}5CQbq^%*Dp4L*H+EWgsZv}o)nA>~{o>q3YJ0um>KLf|xnjq(GIkIz zk;9k<`Z&zZnZ^E%*7wld#&qACR`7h7nU}d2Vb~dU{GOFz!T>9)?c!D%?SI$RA32ym z2IYm1Wl*vs0_@m&%ek*0L@jA!=2u5Vo?p zBTZfEZ}iN(_)i}W`|EY@ujV+TO4gtJ^47c~-9XkcSD+;tz`e=B%j;eA5#LUZB;aVGM4`yh-((H5_oJ!cXB9<{3GTG%goexUE5239= z%V($4^-$Fq;T!||>U*g7UTHhish${PGskK@t4!=>hAT#Y-->C5uW+fw^_5Pd1?Gm@ zH6{~p>=E}m3|}_lwK8m}x&q@hrsp8I^e$O`?>q}o?fH0Q)5i!qo^$$fqLea(dR|oO z?0zws_46E*gR_+vR&+<1om8ION0(1p-M0{PTJ7TOaI!Sw!NM&CV#Na2u;~q*_f>}N zS3&({2RoM*zR?q;_Zac3em|(?F7lEHj20~}<=>@!e~OYbw7UEB5N$>^Wx8B80Kdf^ zp8P{ls(8}yx{dqjJ{WQGV=U$Ol}+)uVqe5WItZ+oQ>*=-@wX_5-^J~@!!Wn;7ogXE zquNt}SgWqr+iG@7-L~~usOKS@N^J!}T7^4r=aJPu@#vw2dYijluHAg^<|*gd@z{pU zIyJx5j(Du^%YVE#)30O6A*;w@D;!4$^w*13dmM2<`RUy{!FqXXewW**+=TtuyR^H% zfR#hp*RH)2KC#uDa<%Yj?~1Dv7^%ybmaw6g1b}T11cDhaNN(iW=HiQfdXI6q8nAB| zUA(32CkNVPv0~cI4K&sHxMgna*tMp!oD`8qxuqV50}nGtBw)e9+h20$7HWhLIbq)~ zU>Z>_v81KnMxDPyXR~dc>%%}k40m4zRz%$oTRGtuw78Bp?5J4AZN2N1^(Y}KBK?h_aUKlUN{b|0F`B_W{OVy~Y-(3nU7~Agyv>B^Lx+9$VmJP@|C+7Uxr(heUJD%5s$MBcJhVo#vJSg zt1vm8*=}w}#&>CFU4PEcZI-j#NTZiQJFoNEVr@yJcj-cPhQA|{h;4k785B9W7&|rq zAP;@}?9GS5UM$A^l0<;`YK}xt3F0sHTEe?eM<&*iFN<>kJpq9XNw{6s

z<7ww|c%PYXyLA8y#mhjH26KhS%L^ptGkchd(JcWubAXDud3_ z>wk`VVZGjruh~v`S3|aO?l4K!{VuAMLR0G*WkYXt?)4#c>=+#BTtHA(9Nw#9S9&d< zK_~S8=JH^4>1WcU&3Mszn18XwFPNj#52eq<)ZtTUpDmS(Oby=)`e4&VfR<(8!-Kj^p(bonX?Q0du4rj@3vIAV$rdS zE3;K|T17VZTV$S#r9GgEcDFu|!Zf1S+HduQolI$~e3(X^S`GA=8>hSRy7M*E;1Ata zF`U&)o18l6Hp6niTcD~Rlqai6L2ntN+43fK2v?<>Px@Vt7e+Iunc$$8sI z$6hVxD=p)$zujKatG`}KHvUx8)_1+#(1F`w&yS_752SF4R6Ckt36!{rJZS~i!xEG^ z2fccaefWp1c==PJaY%&kEcvKiyXagJjRP^XN?XgjNJK9_HLo8SZjeK%#b#d2Ekf9p zxCE*A{(5f6hu|&bm1PLl?t#30{_VS~Vt7lv+_}`B#5J3d1`0WD8221EEJkNLt9RRL z56kg?$t}B3ylh&WAhXN&bQreH*bY7`9J=b4t12n6@tIWa5_^3oPeaXDCvPuutJpzY zyiVZ;pA&%Mg4d-i@=EiH$H$Ja=@0HfBKkoBX6V;eA< zD1LiNs!;xog5uFSzR^F$BQC$s0bG`LT0P!t^uG2DVQZc&2>BG25d8F*pp6TAI8%|< zxpq50(8HdX*guX7DAX@^A6I_NIC`7zUHYVI3=G_;3EU%vU_%PC%;wP}DG85Wb zmE0%*3yK@l&n3@?t?q$XAcAsQw7lKm3R_SmE>;_jOhpWszf(#4+bFKW48>znUW3En8U|wF@t#Z zJ`-6ps{`eq?Od+YMQ%H!L<5z-kHJHq3h8o9Y?A|~cH2WTjNikeBznVhovR?IE@9JN z9zYHt1QDEZr`Yxa$oUovedF$E&P)6|U|Izp@)uC6e~s<|Bi@u+RwlwW-aCJtO`V@Q zcXFE+j9O%75oVaV0m!DzPq5A_VvQ^3&#CU9^LP8hh?6Zvv=<)p41*@r(Kk(MA z1UGEwOY6HE-7utPb6l^!s5W@kQAsnk0KKqbLWa)s#*6{Ufnw_C|e!TXQr zrg#F27JZHu)~Q+kwy}kBa^%>U8Qp#T%dAf8HtShO+Ke9hdW*DK(9rc+^6;+Ra$ioV zf#GMNsH=7#!c^|!9;(iVEI)Ds;jzN|U<`Q71i?se(fX5lFG;H1?m50IfecRZ$(?{A zt<;UqPOFxg=IljO5EK`U@LPdkx^+7kM%14y-VV>&-0JqrPDpWEGFjVf2oCY!Ivetr zcPhP;mvyr)&Ft}q&XBDJ&H2R^NH_|g{}!!}kv zV1Ky`_7P#h;c5Z41sm6Kx@`}X-}KSmUo6!I4O!Uk4&}ER&uWi^UF=Ge=DSrWN8x1r zyW=;`VZxc*mR6h5onDORg5(s2>JuLB&GDVH8ay@5tomhQM&&clY6Bs!kS$BCl_udS4P$W?IJ=C*A0MlB@jG5^#rp^u z>+)0Br^0?@kJ)AaiDjY|IlWggZ7k6ad;c5PZ*4Wnyp4)HDW=0wPt1%jCs|dOetkns z?(UBKbLiaoyh!)+=L1>0LT2^YW>!CXzPb{##>mEb?Ve#}ZQEhot=66j6! zKv=n&k~uo(0|h@p*Ya}EYWPC~{$%0U>hI}p2y zS0%LWE1A5?Hz~dA``01Qr+{v(l~z{g&8_#mu+qi6o2&jUc;kV@22j3!^=x9IOarN%&M{aA-MJ@UM zMFgCeJ2y$w3cR&%!9%fl*sjCIa`o`_+4mH$N-r|5z9+D<_qvD%uzsCgQaQHj&E{p- z`i0qMYI0IiFZB6^s-3_k{jQ7#&HHJ)gcC|ZT=2x@nkkOM@ND=0F0?T{SxuW+it=G= zAhu*0J-v{5=3wizZ~IEQF9H=I{f)!?YMLtK(>vOg&f;)SK1Cbgzl3tJ0k!dAkIM7n z?A2gj3=2j&|K%XAw+oPpFcl-w;;2rEI~uQebl9O@Tj}c4brFrp>wcdp^a_QqEWBoK z=q8o=Q|KER5xkl^1O1n3Y4fVX9C@=8L}ifuB0a6w!Y+b4KCS0u<9W!`kI*CQKkl7k z*K`~A-W3w352za#jyR1KXA@y2maFpqaNMY`UCmw&dzsRZ0>G?D(|?wX%sTx2F*NP< zQoRl?PPAoov4wuV-IrHbBsx!ngwK{`cm%+vt}Exz$i6@G1((%-n%fOO#qPB_t~1i8 ze_>9?-4x+Zar^55m`zyDO0z>@N^SDh1*CU5VN&Xz&F8h=Xx8W9aCEar&6Xw?pQOJ3 z2lcW!`fBj4Q(Ebuk9@lj+Kmf-!*_u$WU{66haSu;4qux+e)8<8PcHB1RR^w%GtD9F z9Gqb}{`k1tc%gq0u{h-X{qgTGR-uB(uetPZLAZJv4z`0MoIWzG zIN6-iiD_TIO?ME2RaP3jdvaA%Osg&D!mMu3>-C6oKgO80sKV_v*)A6o{(er*dHNi) zTxSxkxmID=m|C_ySVH6!mEH@znk++YD$}HE*9ST zk~hrVw0(dkkId+6S2Ipt9<7j9M}~@@H_K;-2PdM4+T<->MH2Qc`_fFT7K!%YhEjjG zscT^8cYzz#9@Py#pz!^cO=jn)wZ8n8zdm=iNVj0+4oH*xM;V^rrytpq+7?L&^wzpW8~ z-SmrAjo90@`OP7-^nB`>Qdn!v(lgBp1qk{+X^N!8{ecpH`pMyafpioaxQIYx@dB*Z%vY?XU^b7@$do&%9J2xylb~ zg@B)Ase<0Ec2qJ4Jm9|{ zqa)S#?0(+Jfq=8@MG!->zdK^p>55H8uW|LD?(enmsw9x)OFIQ0i%~5Hje$3?lx=@l-EG{ChU@6^ z0NBaILg-H))D?)3R8lv$FZuGdqs;vY9dl1Q*MGVHlV(Rc`g~o@Y;QUb4Q8A8uWySEZC45Pu{%)Y)!nmYnoV zdr{@B5-8{@X`C0;XYC*k%73jMigd7gFCgA-=W!O=>99gPG96V|Vy37nMEt7D-Icuf ze1gLWzbU;?0bIi1+?P7s5oO%r`0|;=4QV^dPyXl@lh2)yv5XfU9wmRQG@q3=XL*-j z0i&`aUT0cR=7&9@g-1arT|Q9oK@KZV?|IqU=Iw%w-rem}JPvhP-A=y+;&rN}To$qMir$|1 zTfftvtNDgJpO3nTzBhJJ=?yetvl+t2Ku2E>n{+C-rJDfRP1l`*^%t5n7;Kew>s8i$HQ7SRNVKc_HU}I8vE%@ukFH4dRCQ3>dl``+Req2|FtsRBG-p=btmG3iuHE|(91@E1*HTnPy(DHI8iS=g{ zJq`K**xYg47_0t{qIS0yqYUJn7k)kD1Os088zsSe^K8g_SE7 zKWU*rJV*A#Bu;c!)9?DdzPmo}4M5ULW*4?jEeqf`?@g(}7maVn&n&klIpv7>&)VGc z1A?q$FPUi(Pw9gn*@geTRV4S0+`RIUEDqbBa%FT}v^B!M82p;(FDKp8E{-$jJjdNu zF4NL#d*TM**;_I@@+1~Ap?hWe;Gw<|z=mcX1Uq}RN&}={+hw_*yrQ7J;?!^#JzBnYwl=d~P+vyMXGyVd{R$MrIMb5EqHpiOgw+vd9+^~EpPm2D| z$j^4>d0hGat!I`IptM-RP9`+?=0O>SHYg%2)z=VqG;D|PuauR-6y!K?|C|{6P-AL4 z`2!a*FdjYg`+y35&j2Lk#bd5JEG$j_g$u0riyKRG-@eI2gE~7%`T2eCV1Y1?wL?0} z=o5F(^q=xO7Bb~8Jxae+)IOGl?09Q0gC_s}@|9kRDiy@xco0>!Ax*uhgut<4JU!vX z@*dIXW;mdgiFg5P_#09u+sa~pA^Q6V3ioIycB4Ehpdk#B{}i(HQGUf{z6jnl38rVl zdoF@?>!W@fgh|8>cgMyE$JMx2hve8Ce>0uM^(Vdf5-_UUvR%G-m%KLmlzL;T(g*5yk;a!9_UP2AiEy_&FFy!pV|H+!*#Cr_TO7qnkR@Ll#Lo+nSO12 zSNOA;Vy0sN;!@vCL-?y0B{T}Pyfx=xa3=|c{42*Ab-Ir*^|SE~WR=M7zSF<>x$a`E zV}o@?R+mJYS@aoWeCX~v5I$Wg#YBt75Tq7c%gKo#=RJFWKJ_0*^259+JZ`*u7cin8 z-QDw>;DO>0m%F#!Ic}WNu)ee;J5Pn}I0(|N14sHV{`~jKW$Flyr9@ogLTs|-L-U&KjxW%Xhyf;2@x*|m*pI>#cA0jua=0;DZe|;Q@WycZ zID7Z1+a*6HTlKt|$(NkX!aFqc&4U{LAlbxl4o^MiP0A-X0%YyYtPlgI~BSj zvi{q>(V5$PJh;-V*Uh^{xx7uq<+Qp#jjvX{(ITpw=P)p796gKXa5kOiV)x{R=(=wG zfRC_8e>F~8UUH{(GT8o*232ZaJHog*>)iU=(r5n+0xgf~2LiaU?xhB}mHAboM~bHs zS0GP`KedG7(`)dnn5}!Yb?XtmV|;Z&I_ST6dyI67jr;{Z|J6+%D3$8)MYd6ZjKA48 zUMaWMqw%)0+F92K}J9O}o>6F-?ZAO|=w|1;5gMBic9e@#dWR zLn)QZH2%Lv?#WtAimptVbAE*X#T4)m&QGYPbc%=G(42;A@3S2y7{2k@2Y38Si=RU0 zystsG!fEdGKV>+V?ei*ulF2+Xe&v6XaFB7y-yXM{jO)z3Y}ciFd$I4l;Sm z`R?!=ix!D!zZK@@Z1M((`n-oc(~uSEN~1HsHU)^-_-RnXtI9IS9l2?{{KEm#hr1le zNHd|ra}Jv?)S@-bV8NjCPK+(eX}io@nDsq-*}}VZlGGdPcN(Iauy*z8Sej z`}7v=DOB1g&A#CLE%UNM9g68Y?i`m`ZrKQ3kU^rh3uv2uJ0F)%LdIcDsV4rn7iZlJkSgTYv8y~6^f0U#%k-dF$D4}%OqD)tKKN_0 zS;|>C`eURt}@#-yFU((awpg@6Cip2 zz{mN_u5xbijxSC&mL1ZHT~KyK5nNn|EjsO(dieEt+d1DJHGdooM<>S7x7}0pgR{Iw z&<&JG&d#<#c^_W)3^Gn4nx`_g>J6tnfnu-nQOno(Q~lVlA@;AWB`@gD_Q8gXllgI- z=+)?J`JLpLYQn8rHs>r|sLMI~jD%N02ar!I*LG_CrCRI6^9N?ProXAb>x$Vw@{b3C z_a>GYt(^1HduvzPDbvwhVd5Vo=cm228h5lOPMp7CYY9T<@$g&?wti@phWc8M@Adtx z1(kj~Y%cSOWN#|<5>j$j`^bThI9vUb&!q4YRn;A^RWv0=w`%d&dcskv3#X-AOT6L! z^6-k{T6S~hAzm5mw)7+<<2+le)d9%lpfDI8Z0Q_vZSFtv?p=-cqxu}wR9AMeoesqE z_zfJjbuRnXo7s*1N{ex@-SYy~WZ8$86ws$7%=e?8S;*Y=kaejc;Ulxqe}2Z+@mY0C zg=1soG(Yv%E;ggmWn%B2g3&7a&#T07`!@SGFwH6p5hPoZPG`R|wV<6n83lG|>%juv zjtcul7SBm#?K}L$sU7_H3kR*6kzrqJ`ddz*VQ|+5ClCUa?8;?3lA(_`LV-cIJNHM= zpTmJ^G5;!l?%8{(8n^NEGT0!4%Z_fvRs_-XePn`D68X0`HD;<^R(tthz^tsxjAldYrSc~!6l2ct2!l}ABAx7>y z63qH>%oUz;c|872PLLOYOe)qtNi%zQ{#&D+RnnVpRyevs-6E}h&<^o2f!mM^m`9;% z+!p$w=r;qUveC0h(m5|jGqCTbJUHz{-5bs?fwuXnQM303Dn-8swf^9;&cDaT3tgoi zw;NKKYtsG=&E2#;sb|W*)ajqdY5rcn+=j?zPkW>aw9UedfbGQ4wZhXwPj+G}dzE#b zt*exQ(uFp2e4iz!mo*)uL_SGU{TsD7IVWSU@8x`0uiqGPpJH1V+0K*O?hrQ)gIT@5 zo)oAOy#u1>qsIB_fom=o=u`f_HH^!A`3laTywGiTHpxq$ z@KUaQ%+Yv>ZsE@k|JuK5@rP^6^?O*fwAqO%Ao=`dsla=mTJx8koL7I~MAEY;iOLnP zT%M~?jzZ=3Pn*O9u}^Z<47eaRl}&Hi^I>C`4+;wZoDsWLUwsRe`M+FMU!`SpQ~Xutu(PwciLNQUhOh2> z8r+EYMMQGlMI?})X5pzxm%BbXK%%9?|1m?*XVSd(SCqKh_uL|i^$WFXyu#*oM>b?( zXmu{haT(>T@HYnU@t8Hl$oU>#dtYXnmuHEWAFZ$5%kvIG>ARree^wS+&IDCHnAoZC zu3eKfz5S^ana-$lB-HA^kbcgZpMo?`KKGpiwfi3fDS&ZUv7wyb|AE#H}{(c*Q(a{fhbJ9*N$(L zSlBeGQti{uFK2i+hsjW?e$PH7nbbb#qwD;cJ%6}Qqoeo^%0(j~8oX;Yq+@W$C77Oo_VH_dMbP(cxH`^>J*srj@SGw$780LnZosxRf z4Q|oZW~0leY;6&SlN9LEJ1Q6magN*ydG!M&ZiFeUZxP4zYmx2yk%tYg_gJuDu+qj`p?NJqmiSZ~+<( zMsI8c=JrExvEBMDZPAogyJ)mT22|s8W}fixd95?0w^|b5=>0ep;}uoh+(7TJYC^p2 zK!76}5Pb4Gn`Qb^4ZG~Ro5IA~fjeHx#@yJxSC>_;PHD*e$SKNe@pVU=!LOy^dOs43 zoI}_6tZ?WH6h5uI>sDv5d-YmT($DtPq`ArDFWiGn^5S;Ppsv1c(N9j-x07!EheYel zskWvQ_^ZE-zRCQ+?4g`Tsc|0^AZ`B{edTAh1l z)f=EVeh+fl&nYG3s?n7Up=A3jTYAoiePe@VE6x6U0kbrh++DxfhC4WW*i^vI%(&m) zws8l2@M~s781J7XnYU+FwzmM3#c=!m=Ofa+Uf55Z>VR@foenTTiWW2vbg)8t-Xbrb zGvC`w5$?MU(sMi&9d-_}2PN zB;+*28l7)-{MNbOx8C4$DfI9*k0{Hp4&ROqR$;z5seKPg#D?o{O@h8gRv}?fT)Wrw zzD9Ijjb;YZhNF7xIP4QV!s50hF3$x5ursaEnkX>!fdl~PVhV#M7Ix$Fz9+Bnm!6*z zs<#-^%_;94)}kU_(kTk3rRw`51)MKA&pgj$y9lm7jL^rrvD$?G=Ofx&fPmaoAq1Gb zCaGa8&3mbWclR1*NIjOXy8e+Qz3?sR7V*AzmIi4B+!RLBb*EO&O#r{n0nG1%4p zczNt>4Pp9WLpU$oWq3M$RVvuGwqRyl{#`z@a5k#@$-Cc|VbbF^eTlj6zG_6j2erBH zWLBt&mKWEq<&b~N0)CY5hwByl{o)_ug}$8{pVR{Oh&r#FFdpAjf&06!dx3{Gb?WjZ0s`D>A%wXQWfB;YD5}WuXE6;T+?tIq_qit~tV7C)ebIirJ3(WZ6T67xi zT6->8^E?nsvn&mtJQMZ@&Txx4=?)DR`p4Y7G^Y1=fB!|P}RhuN|KjWT2%9KmFNvTu= zSfbLK=5l;g2*Ih-uQB6T@n7?dqZ(@al7yvH{21Nl#$uC6i64O*;Pl$!J3sSjESfeo z(=coiy}~kL+_k^tJUzaK`>&k2@x_{S*GsdSh7xARIg8MC^6x&hUmJ3%U2F?_QQq$* zbTZ7L7eFwzak7=qyPvfY)RRSn-6Hw7Z>r9I+s)+$pUZLQ+spxd%ID$P?}i7lM*yN} zRSve{2P=1$&T@U_`X2NQ%C8u%*h{-ku5&@AZaw~T+4ChlAj0*SS)`fP@{*RM^J4x$ zZ!c@mq;myn{fnbVklt`d9F=Rs307Xq`OV=r`2p8-9**!+{v=4%`YffFxxbB_t?!gB z@+g|S%K^OaRGc3b;5%=;%GMD#NPB(Q{xrPRhZbB*W2Xi9A>QrGUcCK0a@xAR!6A%Z zN1MQ~H!Bd*hxb@pZ>LD7KO3fNv~M+J-f(`E+*b589&g?^5AeRV$_q-BcC(^AUxmQj zxz&al33SxndNA6l)uZ3wN9aCT;&V*N^~l(`BVa^St$o3} zbNM#9-|R6M<+Ii@>o<&2b-LVloooK8Rrz6D$l(5oGEBnV13&F(A3svx*<7prWMXDd zk`T)i1PTvbx{#^5N*tO#7hM6O>)Bp#_lBAZn`{9p%=A0zM!m%O8q9)Wpua~ZVeS0o*WW-_i_ zM)8IODAD|W>N~BMw^~j(^f09`I`-DB*Bs;6CSXhq9Q{C1M zPLPZmDz@>E>(|9rfhT|Me8fB{nrlw*jpF~ZT=Knq%_3as-XLp0r&V$5xBIo^Yc$(3 zC5@A0l;_;y_41kD@bQooFH#sab)?YTg457qU**=k35M5ljqwhn7q+?7dxI6%r&-+m zaigs79*8^U5+y;SvpU&i#69cTY0f#kx}V{tTmlb#sLpN(s?zDS_d$uzE;FnDH9Ehk zEmYL*UZ&5b`ug0d`MYn}HIjSTdD6-)<%8AcD-FkUoiFD}M^9cGD;#8Q$8;^s3knh? zH*si{h^lw1Lf9=#zLjU#pot_U`=u&MFb~vV6^uB3vEY&{>P*UCC6Z(!XRC*9 zazf48*vPz_S-D= zYq4J;nk_yBqq#2HVWU#`BdIRmgyR+wz+?Gs+8}t+By2LgFO?OpV!tdCVPK%`Sp4Y z?-_ZmqdG%tn!4Q<nm#Y&t%l>uiI}^ z_#{A1k!kbZ9T=W|R4riiE3e$LGH*Zo{%uiQ*62phObgXUsS8Uw?>pW#kzJ4lxCVcD zjgg<|?8eS;bXQ?<4wi27BJVRUK54H&Qa5yMa`o{3;=y-$*#`()^1hPsNRM?AyUP=3bVPPr0?paHz%K6iPd<0Xa^E&m(a8Ji#WcL zE3N@NIGznu*%MH^AAdF(nRnce3^prObY|<#vmsHVpMrSpf}N1pdBCu@lmRQ(`DrR( zgfhpUDQwpdG?%%S#`Nimr?%ZBtc83SRfs@*G}k+JCv+=|=RfNA_cAMh@g_}>g6SL| zzFFO(eo8_HWOp1*gXss~@T;q*m3PbXBGqsShuLmVeOG5>?>I~HvrT^^mLNbMb=cu+ zgzc(A?5|v-)J4*+Z~rc_lGiO$d}Fi5f7-jU?!jZ7ukSVoJIAQy@sHD@Y~b6&IR38(pbZyYXx!pU~mcHF~76wb-qo(aU(DmyS`Qnwaw&pe*?WIHDny+g{P zg@;v_ZNf!1D(sUVgiOWe=I&V3e~n7E7LLD7pMVn%X*BO0fsSuvLw>*Kp4mlh-wEurlQo&`yL~ckHP+)&f;O=kPZN z!pJc%gLfyXQ+o}pw0fN>I^jZZyH#o44P84c+LJQ=Ciq%>EubK$G6^)%~iLWZ&~Xu41#guj^?@6?KYR)xljp><%Y#I z4JBJGJw54^*=hNC<`fGrPaS^7Iwn-Eudvf)XEMYOQqq~a?|W?a=))|?t#kq0l|{hj zQMJo}m%?T&&qfcoH|w}P|InT1g<&0TSFV`XiQxm7gmXgl#TlC3j9T-FZKQ$HxH)gd z?AkLA)n^gwYtG?xH%2TEuBBSHVtNZ_4fnO1(j9KstBve!xLpXhdW;z0oBkIoxaF!j z!I-8h_`03yc$6FXUfd7S?)Z^%fm2$KkPOW9oVgwIHVYYLnxUr%T$#CE z4OR|uGJa}XI<3(%`>Iv)>6j&0da+t3V6N9_-LFG5-M_8t&T`EazA0~l&Q}k&@}|Ot zwQ06^d2%i{;hSbrPe-3@TsA@H{0Z}V0sioU!~1o-U)c@q(`Bh#=33s`E)2HjWAWvV zlODMeXP(~Kv(s`XTF*8S3bZr_bl zmWaLHbrsT%RMwllx^b{AMQj_JdV zJ7cZF~zJ zITnj~dlHGv6B|7~k=wms33s~@ldl6;QQRe@?WR{WS7u=)=G)aHJ5Mia=cC}vKO*|- z8)sCJ-HmNem$-1N{OzMA(&)`fkfMoF z6!yy&aeUW~IGX0F-Lr5N<9ds!F9X=^tTnihq;)4Xf6rmb)bTVsP}{9}mv-aGwA_9! zJ&smSb62c{?+T5f(;)f zt||-ipVph2L_{OeBLf2&L`D>NM2V7wIYddI z_(Q*E$RmcdB4p4_sFW{KWl-ZQD%-89FAvS$2Tv)@DO-K=1GU~rJhXdHjCUW)@T3Ef zNR#;Ywp7N-V6q(TtN-Xz(yjZnlXsE$m^KHNX{Gd41h#VjnDo_hD!Q-XEnU+G8)yzk zu?&yAEQ~UmLzosQUPc@be}zK>Nd7j06WHu35ejI4=6G8bz%t= zyD6SWEJXuvi5Qw-2{D+2kGkcmphV^tm|O&S#3LGy5p{y0!?-_;_qS>9Z5B8ye;Hr@ z0m_)`M47-dlq?}$q-2?amB$D%A_*S#;5W+Nm(%^Kx`*CT9}$%P*C@l+i86+vI7vVP zEm0gsSP&>IBS9jeqaM5}l>PE$k-bhRNjyzq0eLY`F|3FfM!=MeWR5ywNXbyVCKl^= zUKhFRgxP*wWN5My9@a&Szt=^wquTGY_CG&^`{J(?Wtang3nNa#6pLY_%94;klhC!U zi@)<+CR{bjSoRoY9RH27Uo)2-N%}s7I==sT*%u2!FA*3riJ>G3Ng{~CXvESa^gWmT z{1x-E-1vVUWf(YpKpEbn3@`^M1Ksg&l>HLE>`LYMEQnb_LczkNlMFCqf=n?WBcn;^ zdoKIwa~Zx;2|o*>3zk6~D^hTBX0ZJX;z1NE@ZTu=`6#aSos@RiE8_iTs3b-c43_?zv3 zsp=O99YKxdT_xw5HtHOaFER*DPYkw$GeY>A?f69~yHe3MhdHu^!Yoe-91~30Mu7_Tm1p4_y3pnZ`$%-xf>1@wqviz+5TJM2h8j;9)_X zA7I9?M0=6tN$4A9Kb?n##p5brw&!7u^%0Ck_B<@0?3;)E1yUc-!@5e8iK0aD67q@y zXv%^;2?Jq38%r_V)$(7z^O|`PWkp3-ms_;l&F6cT}XuNY4v zaH;xZi+q`dGE!r35zErBU}P^#vSNSq=jz^wk3obZeQ&~X5(X2euhLk6>sM(U*hGDm z2KG`XBp<&|Kn$VlVp~9q$>^6uR~?s=5JO}s7t6`8yZI^&r~@5>FA4&Az|MY^2o@7x zb`21Szw8^J&^VC0*jOOZI7hjhC?G@peK`^IZ@%mwAQIH`zaj;=^e=k|h{Rv^5D;nX zW-c}eh%|W6%ZV&-ZeKMBh%_dAqvVH8A$wU|RsrP0y)~N^8K(U|-r)i9=T@P<4BF~D z^^oNO4HfW%OM*vlxhyWZpRdM>VJ{20Pxd3ezOpwz+Sf}qyRrMvanT&|!1GYY5WUBdp+_FHe2*hGwI^{5&B-DRn6rEwFze@WJQ*Hm`2_mBtV|2A zD4ynnI0>eRld>``fwkOO9K%WH@0BqPW&>yWut=t()B0GJ9BrM&$>;RaGN|i*s*h*5 zPxbL44?TZcpTNQ<`8-bk+-8yg+-3=XZnMO*pZWlr4Ab!Gdu368adaF9R;(;ok)Owb zz596_1GZwH;=uU)JPz|@ZajSlY}C%a2Sc+W|0y5G!pX#0SplxiJCEZp#>t=a$@Hi9 z3N(H6J+`_snX9^KKdQl9=RaYnNMw-u%9^%C?@O64vm9#ZZQg Date: Thu, 9 Nov 2017 11:02:07 +0100 Subject: [PATCH 79/79] reintroduced debug #define's --- src/line_detection_fu/src/laneDetection.cpp | 137 ++++++++++---------- 1 file changed, 68 insertions(+), 69 deletions(-) diff --git a/src/line_detection_fu/src/laneDetection.cpp b/src/line_detection_fu/src/laneDetection.cpp index 43f99ea9..293de236 100644 --- a/src/line_detection_fu/src/laneDetection.cpp +++ b/src/line_detection_fu/src/laneDetection.cpp @@ -2,18 +2,18 @@ using namespace std; -// publish ransac and grouped lane frames to show it in rviz -const bool PUBLISH_IMAGES = true; - // save frames as images in ~/.ros/ -const bool SAVE_FRAME_IMAGES = true; +//#define SAVE_FRAME_IMAGES // show windows with results of each step in pipeline of one frame -const bool SHOW_EDGE_WINDOW = true; -const bool SHOW_LANE_MARKINGS_WINDOW = true; -const bool SHOW_GROUPED_LANE_MARKINGS_WINDOW = true; -const bool SHOW_RANSAC_WINDOW = true; -const bool SHOW_ANGLE_WINDOW = true; +#define SHOW_EDGE_WINDOW +#define SHOW_LANE_MARKINGS_WINDOW +#define SHOW_GROUPED_LANE_MARKINGS_WINDOW +#define SHOW_RANSAC_WINDOW +#define SHOW_ANGLE_WINDOW + +// publish ransac and grouped lane frames to show it in rviz +//#define PUBLISH_IMAGES // try kernel width 5 for now const static int kernel1DWidth = 5; @@ -136,10 +136,10 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") imagePublisher = image_transport.advertiseCamera("/lane_model/lane_model_image", MY_ROS_QUEUE_SIZE); - if (PUBLISH_IMAGES) { +#ifdef PUBLISH_IMAGES imagePublisherRansac = image_transport.advertiseCamera("/lane_model/ransac", MY_ROS_QUEUE_SIZE); imagePublisherLaneMarkings = image_transport.advertiseCamera("/lane_model/lane_markings", MY_ROS_QUEUE_SIZE); - } +#endif if (!rgbCameraInfo) { rgbCameraInfo.reset(new sensor_msgs::CameraInfo()); @@ -153,18 +153,19 @@ cLaneDetectionFu::cLaneDetectionFu(ros::NodeHandle nh) : nh_(nh), priv_nh_("~") //we should generate this only once in the beginning! or even just have it pregenerated for our cam scanlines = getScanlines(); - if (SAVE_FRAME_IMAGES) { - struct stat st; - if (!stat("groupedLaneMarkings",&st)) - system("exec rm -r ./groupedLaneMarkings/*"); - else - mkdir("groupedLaneMarkings", S_IRWXU); +#ifdef SAVE_FRAME_IMAGES + struct stat st; + if (!stat("groupedLaneMarkings",&st)) + system("exec rm -r ./groupedLaneMarkings/*"); + else + mkdir("groupedLaneMarkings", S_IRWXU); + + if (!stat("ransac",&st)) + system("exec rm -r ./ransac/*"); + else + mkdir("ransac", S_IRWXU); +#endif - if (!stat("ransac",&st)) - system("exec rm -r ./ransac/*"); - else - mkdir("ransac", S_IRWXU); - } } cLaneDetectionFu::~cLaneDetectionFu() { @@ -197,28 +198,43 @@ void cLaneDetectionFu::ProcessInput(const sensor_msgs::Image::ConstPtr &msg) { // scanlines -> edges (in each scanline we find maximum and minimum of kernel fn ~= where the edge is) // this is where we use input image! vector> edges = cLaneDetectionFu::scanImage(transformedImage); + +#ifdef SHOW_EDGE_WINDOW drawEdgeWindow(transformedImage, edges); +#endif // edges -> lane markings vector> laneMarkings = cLaneDetectionFu::extractLaneMarkings(edges); + +#if defined(PUBLISH_IMAGES) || defined(SHOW_GROUPED_LANE_MARKINGS_WINDOW) drawLaneMarkingsWindow(transformedImage, laneMarkings); +#endif // initialize defaultXLeft, defaultXCenter and defaultXRight values findLanePositions(laneMarkings); // assign lane markings to lanes buildLaneMarkingsLists(laneMarkings); + +#ifdef SHOW_GROUPED_LANE_MARKINGS_WINDOW drawGroupedLaneMarkingsWindow(transformedImage); +#endif // try to fit a polynomial for each lane ransac(); // generate new polynomials based on polynomials found in ransac for lanes without ransac polynomial generateMovedPolynomials(); + +#if defined(PUBLISH_IMAGES) || defined(SHOW_RANSAC_WINDOW) drawRansacWindow(transformedImage); +#endif // calculate and publish the angle the car should drive pubAngle(); + +#if defined(PUBLISH_IMAGES) || defined(SHOW_ANGLE_WINDOW) drawAngleWindow(transformedImage); +#endif } /* @@ -1204,9 +1220,6 @@ bool cLaneDetectionFu::isSimilar(const NewtonPolynomial &poly1, const NewtonPoly void cLaneDetectionFu::drawEdgeWindow(Mat &img, vector> edges) { - if (!SHOW_EDGE_WINDOW) { - return; - } Mat transformedImagePaintable = img.clone(); cvtColor(transformedImagePaintable, transformedImagePaintable, CV_GRAY2BGR); @@ -1224,9 +1237,6 @@ void cLaneDetectionFu::drawEdgeWindow(Mat &img, vector> edges) } void cLaneDetectionFu::drawLaneMarkingsWindow(Mat &img, vector> &laneMarkings) { - if (!SHOW_LANE_MARKINGS_WINDOW) { - return; - } Mat transformedImagePaintable = img.clone(); cv::cvtColor(transformedImagePaintable, transformedImagePaintable, CV_GRAY2BGR); @@ -1242,9 +1252,6 @@ void cLaneDetectionFu::drawLaneMarkingsWindow(Mat &img, vector> &la } void cLaneDetectionFu::drawGroupedLaneMarkingsWindow(Mat &img) { - if (!PUBLISH_IMAGES && !SHOW_GROUPED_LANE_MARKINGS_WINDOW && !SAVE_FRAME_IMAGES) { - return; - } Mat transformedImagePaintable = img.clone(); cv::cvtColor(transformedImagePaintable, transformedImagePaintable, CV_GRAY2BGR); @@ -1275,28 +1282,24 @@ void cLaneDetectionFu::drawGroupedLaneMarkingsWindow(Mat &img) { cv::line(transformedImagePaintable, p3, p4, cv::Scalar(0, 200, 0)); cv::line(transformedImagePaintable, p4, p1, cv::Scalar(0, 200, 0)); - if (PUBLISH_IMAGES) { - pubRGBImageMsg(transformedImagePaintable, imagePublisherLaneMarkings); - } +#ifdef PUBLISH_IMAGES + pubRGBImageMsg(transformedImagePaintable, imagePublisherLaneMarkings); +#endif - if (SHOW_GROUPED_LANE_MARKINGS_WINDOW) { - cv::namedWindow("Grouped Lane Markings", WINDOW_NORMAL); - cv::imshow("Grouped Lane Markings", transformedImagePaintable); - cv::waitKey(1); - } +#ifdef SHOW_GROUPED_LANE_MARKINGS_WINDOW + cv::namedWindow("Grouped Lane Markings", WINDOW_NORMAL); + cv::imshow("Grouped Lane Markings", transformedImagePaintable); + cv::waitKey(1); +#endif - if (SAVE_FRAME_IMAGES) { - debugWriteImg(transformedImagePaintable, "groupedLaneMarkings"); - } +#ifdef SAVE_FRAME_IMAGES + debugWriteImg(transformedImagePaintable, "groupedLaneMarkings"); +#endif } void cLaneDetectionFu::drawRansacWindow(cv::Mat &img) { - if (!PUBLISH_IMAGES && !SHOW_RANSAC_WINDOW && !SAVE_FRAME_IMAGES) { - return; - } - cv::Mat transformedImagePaintableRansac = img.clone(); cv::cvtColor(transformedImagePaintableRansac, transformedImagePaintableRansac, CV_GRAY2BGR); @@ -1309,28 +1312,24 @@ void cLaneDetectionFu::drawRansacWindow(cv::Mat &img) { debugPaintPolynom(transformedImagePaintableRansac, cv::Scalar(255, 120, 0), movedPolyRight, minYPolyRoi, maxYRoi); - if (PUBLISH_IMAGES) { - pubRGBImageMsg(transformedImagePaintableRansac, imagePublisherRansac); - } +#ifdef PUBLISH_IMAGES + pubRGBImageMsg(transformedImagePaintableRansac, imagePublisherRansac); +#endif - if (SHOW_RANSAC_WINDOW) { - cv::namedWindow("RANSAC results", WINDOW_NORMAL); - cv::imshow("RANSAC results", transformedImagePaintableRansac); - cv::waitKey(1); - } +#ifdef SHOW_RANSAC_WINDOW + cv::namedWindow("RANSAC results", WINDOW_NORMAL); + cv::imshow("RANSAC results", transformedImagePaintableRansac); + cv::waitKey(1); +#endif - if (SAVE_FRAME_IMAGES) { - debugWriteImg(transformedImagePaintableRansac, "ransac"); - } +#ifdef SAVE_FRAME_IMAGES + debugWriteImg(transformedImagePaintableRansac, "ransac"); +#endif } void cLaneDetectionFu::drawAngleWindow(Mat &img) { frame++; - if (!PUBLISH_IMAGES && !SHOW_ANGLE_WINDOW) { - return; - } - Mat transformedImagePaintableLaneModel = img.clone(); cvtColor(transformedImagePaintableLaneModel, transformedImagePaintableLaneModel, CV_GRAY2BGR); @@ -1362,15 +1361,15 @@ void cLaneDetectionFu::drawAngleWindow(Mat &img) { cv::circle(transformedImagePaintableLaneModel, pointLoc, 3, cv::Scalar(0, 0, 255), 0); } - if (PUBLISH_IMAGES) { - pubRGBImageMsg(transformedImagePaintableLaneModel, imagePublisher); - } +#ifdef PUBLISH_IMAGES + pubRGBImageMsg(transformedImagePaintableLaneModel, imagePublisher); +#endif - if (SHOW_ANGLE_WINDOW) { - cv::namedWindow("Lane polynomial", WINDOW_NORMAL); - cv::imshow("Lane polynomial", transformedImagePaintableLaneModel); - cv::waitKey(1); - } +#ifdef SHOW_ANGLE_WINDOW + cv::namedWindow("Lane polynomial", WINDOW_NORMAL); + cv::imshow("Lane polynomial", transformedImagePaintableLaneModel); + cv::waitKey(1); +#endif } void cLaneDetectionFu::pubRGBImageMsg(cv::Mat &rgb_mat, image_transport::CameraPublisher publisher) {