tea_sorter.cpp 44 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598
  1. #include <opencv.hpp>
  2. #include <math.h>
  3. #include <io.h>
  4. #include "tea_sorter.h"
  5. #include "utils.h"
  6. using namespace cv;
  7. namespace graft_cv{
  8. CTeaSort::CTeaSort(
  9. ConfigParam& cp,
  10. img_type dtpye,
  11. CGcvLogger*pLog)
  12. :
  13. m_cp(cp),
  14. m_dtype(dtpye),
  15. m_pLogger(pLog),
  16. m_ppImgSaver(0),
  17. m_pImginfoRaw(0),
  18. m_pImginfoDetected(0)
  19. {
  20. m_drop_detector = RetinaDrop(m_pLogger, 0.5, 0.5);
  21. }
  22. CTeaSort::~CTeaSort()
  23. {
  24. clear_imginfo();
  25. }
  26. int CTeaSort::detect(
  27. ImgInfo*imginfo,
  28. PositionInfo& posinfo,
  29. const char* fn
  30. )
  31. {
  32. //1 model status
  33. if (!m_drop_detector.IsModelLoaded()) {
  34. m_pLogger->ERRORINFO(
  35. string("tea detect model NOT loaded"));
  36. return 1;
  37. }
  38. //2 update recognize threshold
  39. if (m_dtype == img_type::tea_grab) {
  40. m_drop_detector.SetThreshold(m_cp.object_threshold_grab, m_cp.nms_threshold_grab);
  41. }
  42. else {
  43. m_drop_detector.SetThreshold(m_cp.object_threshold_cut, m_cp.nms_threshold_cut);
  44. }
  45. //3 load data
  46. load_data(imginfo, fn);
  47. if (m_cp.image_show) {
  48. cv::destroyAllWindows();
  49. imshow_wait("input_img", m_raw_img);
  50. }
  51. //4 generate_detect_windows(vector<Rect>&boxes)
  52. vector<Rect> drop_regions;
  53. int region_cnt = generate_detect_windows(drop_regions);
  54. if (region_cnt == 0) {
  55. stringstream buff_;
  56. buff_ << m_imgId << m_dtype_str << "tea detect image regions' size == 0";
  57. m_pLogger->ERRORINFO(buff_.str());
  58. return 1;
  59. }
  60. else {
  61. stringstream bufftmp;
  62. bufftmp << m_imgId << m_dtype_str << "tea detect image regions' size = "<<region_cnt;
  63. m_pLogger->INFO(bufftmp.str());
  64. }
  65. if (m_cp.image_show) {
  66. cv::Mat rects_img = m_raw_img.clone();
  67. int step_c = int(255 / (float)region_cnt);
  68. int step_cc = step_c / 2;
  69. int step_ccc = step_cc / 2;
  70. int cnt = 0;
  71. for (auto&r : drop_regions) {
  72. cv::rectangle(rects_img, r, cv::Scalar(step_cc*cnt, step_c*cnt, step_ccc*cnt), 3);
  73. cnt += 1;
  74. }
  75. imshow_wait("regions_img", rects_img);
  76. }
  77. //5 detect
  78. vector<Bbox> droplets_raw;
  79. int dn = detect_impl(m_raw_img, drop_regions, droplets_raw);
  80. if (dn < 2 && m_dtype == img_type::tea_grab) {
  81. //up-down flip
  82. cv::Mat flip_img;
  83. cv::flip(m_raw_img, flip_img, 0);
  84. if (m_cp.image_show) {
  85. imshow_wait("flip_img", flip_img);
  86. }
  87. vector<Bbox> droplets_flip;
  88. int dn_flip = detect_impl(flip_img, drop_regions, droplets_flip);
  89. for (auto&b: droplets_flip) {
  90. int y2 = flip_img.rows - b.y1;
  91. int y1 = flip_img.rows - b.y2;
  92. b.y1 = y1;
  93. b.y2 = y2;
  94. for (int i = 0; i < 5; ++i) {
  95. b.ppoint[2 * i + 1] = flip_img.rows - b.ppoint[2 * i + 1];
  96. }
  97. }
  98. if (dn_flip > 0) {
  99. droplets_raw.insert(
  100. droplets_raw.end(),
  101. droplets_flip.begin(),
  102. droplets_flip.end());
  103. }
  104. }
  105. /*for (auto rect : drop_regions) {
  106. Mat roi = m_raw_img(rect);
  107. vector<Bbox> head_droplets = m_drop_detector.RunModel(roi, m_pLogger);
  108. if (m_pLogger) {
  109. stringstream buff_;
  110. buff_ << m_imgId << m_dtype_str << "-------crop_rect["<< rect.x<<","<<rect.y<<","<<rect.width
  111. <<","<<rect.height<<"],"
  112. <<" roi image detect over. tea number is " << head_droplets.size();
  113. m_pLogger->INFO(buff_.str());
  114. }
  115. for (Bbox& b : head_droplets) {
  116. b.x1 += rect.x;
  117. b.x2 += rect.x;
  118. b.y1 += rect.y;
  119. b.y2 += rect.y;
  120. for (int i = 0; i < 5; ++i) {
  121. b.ppoint[2 * i] += rect.x;
  122. b.ppoint[2 * i + 1] += rect.y;
  123. }
  124. }
  125. if (head_droplets.size()) {
  126. droplets_raw.insert(
  127. droplets_raw.end(),
  128. head_droplets.begin(),
  129. head_droplets.end());
  130. }
  131. }*/
  132. if (m_pLogger) {
  133. stringstream buff_;
  134. buff_ << m_imgId<<m_dtype_str << "image detect over. tea number is " << droplets_raw.size();
  135. m_pLogger->INFO(buff_.str());
  136. }
  137. //6 nms, width(height) filt and area calculation
  138. vector<Bbox> droplets;
  139. vector<int> keep;
  140. nms_bbox(droplets_raw, m_drop_detector.GetNmsThreshold(), keep);
  141. //nms keep and area filter
  142. double min_area_th = m_cp.min_area_ratio_grab;
  143. double max_area_th = m_cp.max_area_ratio_grab;
  144. if (m_dtype == img_type::tea_cut) {
  145. min_area_th = m_cp.min_area_ratio_cut;
  146. max_area_th = m_cp.max_area_ratio_cut;
  147. }
  148. for (int i : keep) {
  149. Bbox&tbox = droplets_raw[i];
  150. double area_ratio = static_cast<double>(tbox.y2 - tbox.y1) * static_cast<double>(tbox.x2 - tbox.x1);
  151. area_ratio = fabs(area_ratio);
  152. area_ratio /= static_cast<double>(m_raw_img.rows);
  153. area_ratio /= static_cast<double>(m_raw_img.cols);
  154. tbox.area = area_ratio;
  155. if (area_ratio < min_area_th || area_ratio > max_area_th) { continue; }
  156. //检查box边界是否在图像内,如果没有,修改之
  157. if (tbox.x1 < 0) { tbox.x1 = 0; }
  158. if (tbox.y1 < 0) { tbox.y1 = 0; }
  159. if (tbox.x2 >= m_raw_img.cols) { tbox.x2 = m_raw_img.cols - 1; }
  160. if (tbox.y2 >= m_raw_img.rows) { tbox.y2 = m_raw_img.rows - 1; }
  161. droplets.push_back(tbox);
  162. }
  163. if (m_pLogger) {
  164. stringstream buff_;
  165. buff_ << m_imgId << m_dtype_str << "after nms, keep tea number is " << droplets.size();
  166. for (auto&tbox : droplets) {
  167. buff_ << "\nscore:" << tbox.score << ", area_ratio:" << tbox.area << ", left_top:(" << tbox.x1 << "," << tbox.y1 << "), bottom_rigt:(" << tbox.x2 << "," << tbox.y2 << ")";
  168. }
  169. m_pLogger->INFO(buff_.str());
  170. }
  171. int valid_cnt = 0;
  172. if (m_dtype == img_type::tea_grab) {
  173. //grab
  174. double pre_cx, pre_cy;
  175. double min_dist_grab = m_cp.min_distance_grab;
  176. pre_cx = -min_dist_grab;
  177. pre_cy = -min_dist_grab;
  178. for (int i = 0; i < droplets.size(); ++i) {
  179. if (valid_cnt > 1) { break; }
  180. Bbox&b = droplets.at(i);
  181. double cx = 0.5*(b.x1 + b.x2);
  182. double cy = 0.5*(b.y1 + b.y2);
  183. double dist = sqrt((cx - pre_cx)*(cx - pre_cx) + (cy - pre_cy)*(cy - pre_cy));
  184. if (dist < min_dist_grab) {
  185. continue;
  186. }
  187. double grab_x, grab_y;
  188. double angle = calculate_angle(b,/* true, */grab_x, grab_y);
  189. //grab point
  190. if (valid_cnt == 0) {
  191. posinfo.tea_grab_x1 = grab_x;
  192. posinfo.tea_grab_y1 = grab_y;
  193. posinfo.tea_grab_angle1 = angle;
  194. }
  195. else {
  196. posinfo.tea_grab_x2 = grab_x;
  197. posinfo.tea_grab_y2 = grab_y;
  198. posinfo.tea_grab_angle2 = angle;
  199. }
  200. b.operate_point[0] = grab_x;
  201. b.operate_point[1] = grab_y;
  202. b.operate_angle = angle;
  203. b.status = 1;
  204. pre_cx = cx;
  205. pre_cy = cy;
  206. valid_cnt += 1;
  207. }
  208. }
  209. else {
  210. //cut
  211. for (int i = 0; i < droplets.size();++i) {
  212. if (i > 1) { break; }
  213. Bbox&b = droplets.at(i);
  214. double grab_x, grab_y;
  215. double angle = calculate_angle(b,/* true,*/ grab_x, grab_y);
  216. valid_cnt += 1;
  217. if (i == 0) {
  218. // 切割点是3、4的中间的点
  219. posinfo.tea_cut_x1 = grab_x;
  220. posinfo.tea_cut_y1 = grab_y;
  221. posinfo.tea_cut_angle1 = angle;
  222. }
  223. else {
  224. // 切割点是3、4的中间的点
  225. posinfo.tea_cut_x2 = grab_x;
  226. posinfo.tea_cut_y2 = grab_y;
  227. posinfo.tea_cut_angle2 = angle;
  228. }
  229. b.operate_point[0] = grab_x;
  230. b.operate_point[1] = grab_y;
  231. b.operate_angle = angle;
  232. b.status = 1; // selected
  233. }
  234. }
  235. //6 draw
  236. if (m_cp.image_return) {
  237. this->clear_imginfo();
  238. cv::Mat img_rst = m_raw_img.clone();
  239. for (auto& b : droplets) {
  240. //rectangle
  241. cv::Rect r = cv::Rect(cv::Point2i(b.x1, b.y1), cv::Point2i(b.x2, b.y2));
  242. if (b.status > 0) {
  243. cv::rectangle(img_rst, r, cv::Scalar(0, 0, 255),2);
  244. }
  245. else {
  246. cv::rectangle(img_rst, r, cv::Scalar(0, 255, 0),2);
  247. }
  248. //score
  249. char name[256];
  250. cv::Scalar color(120, 120, 0);//bgr
  251. sprintf_s(name, "%.2f", b.score);
  252. cv::putText(img_rst, name,
  253. cv::Point(b.x1, b.y1),
  254. cv::FONT_HERSHEY_COMPLEX, 0.7, color, 2);
  255. //points
  256. cv::circle(img_rst, cv::Point(int(b.ppoint[0]), int(b.ppoint[1])), 4, cv::Scalar(255, 0, 255), -1, 3, 0);
  257. cv::circle(img_rst, cv::Point(int(b.ppoint[2]), int(b.ppoint[3])), 4, cv::Scalar(0, 255, 255), -1, 3, 0);
  258. cv::circle(img_rst, cv::Point(int(b.ppoint[4]), int(b.ppoint[5])), 4, cv::Scalar(255, 0, 0), -1, 3, 0);
  259. cv::circle(img_rst, cv::Point(int(b.ppoint[6]), int(b.ppoint[7])), 4, cv::Scalar(0, 255, 0), -1, 3, 0);
  260. cv::circle(img_rst, cv::Point(int(b.ppoint[8]), int(b.ppoint[9])), 4, cv::Scalar(0, 0, 255), -1, 3, 0);
  261. //grab points
  262. if (m_dtype == img_type::tea_grab) {
  263. if (b.status == 1) {
  264. double grab_x, grab_y, grab_angle;
  265. grab_x = b.operate_point[0];
  266. grab_y = b.operate_point[1];
  267. grab_angle = b.operate_angle;
  268. //bool need_precise = b.status == 1;
  269. //double grab_angle = calculate_angle(b, /*need_precise,*/ grab_x, grab_y);
  270. //cv::circle(img_rst, cv::Point(int(grab_x), int(grab_y)), 4, cv::Scalar(0, 215, 255), -1, 3, 0);
  271. //lines, p4-p5, p5-grab
  272. cv::line(img_rst,
  273. cv::Point(int(b.ppoint[6]), int(b.ppoint[7])),
  274. cv::Point(int(b.ppoint[8]), int(b.ppoint[9])),
  275. cv::Scalar(0, 215, 255), 2);
  276. cv::line(img_rst,
  277. cv::Point(int(b.ppoint[8]), int(b.ppoint[9])),
  278. cv::Point(int(grab_x), int(grab_y)),
  279. cv::Scalar(0, 215, 255), 2);
  280. //line x
  281. int radius = 20;
  282. int cx = int(grab_x);
  283. int cy = int(grab_y);
  284. cv::line(img_rst, cv::Point(cx - radius, cy - radius), cv::Point(cx + radius, cy + radius), cv::Scalar(0, 215, 255), 2);
  285. cv::line(img_rst, cv::Point(cx - radius, cy + radius), cv::Point(cx + radius, cy - radius), cv::Scalar(0, 215, 255), 2);
  286. //grab point angle
  287. int radius_dir = m_cp.offset_grab / 2;
  288. grab_angle *= (CV_PI / 180.0);
  289. double dx = radius_dir * sin(grab_angle);
  290. double dy = radius_dir * cos(grab_angle);
  291. int dir_x = int(grab_x + dx);
  292. int dir_y = int(grab_y + dy);
  293. cv::line(img_rst, cv::Point(cx, cy), cv::Point(dir_x, dir_y), cv::Scalar(20, 255, 20), 2);
  294. }
  295. }
  296. //cut points
  297. if (m_dtype == img_type::tea_cut) {
  298. //lines, p3-p4
  299. cv::line(img_rst,
  300. cv::Point(int(b.ppoint[4]), int(b.ppoint[5])),
  301. cv::Point(int(b.ppoint[6]), int(b.ppoint[7])),
  302. cv::Scalar(0, 215, 255), 2);
  303. //line x
  304. int cx = int(b.operate_point[0]);
  305. int cy = int(b.operate_point[1]);
  306. int radius = 20;
  307. cv::line(img_rst, cv::Point(cx - radius, cy - radius), cv::Point(cx + radius, cy + radius), cv::Scalar(0, 215, 255),2);
  308. cv::line(img_rst, cv::Point(cx - radius, cy + radius), cv::Point(cx + radius, cy - radius), cv::Scalar(0, 215, 255),2);
  309. }
  310. }
  311. if (m_cp.image_show) {
  312. imshow_wait("result_img", img_rst);
  313. }
  314. m_pImginfoRaw = mat2imginfo(m_raw_img);
  315. m_pImginfoDetected = mat2imginfo(img_rst);
  316. posinfo.pp_images[0] = m_pImginfoRaw;
  317. posinfo.pp_images[1] = m_pImginfoDetected;
  318. if (m_ppImgSaver && *m_ppImgSaver) {
  319. (*m_ppImgSaver)->saveImage(img_rst, m_imgId + "_rst_0");
  320. }
  321. }
  322. //拍照无苗, 返回识别结果-1
  323. if (valid_cnt == 0) { return -1; }
  324. return 0;
  325. }
  326. int CTeaSort::detect_impl(
  327. cv::Mat& raw_img, //input, image
  328. vector<Rect>&drop_regions, //input, detect regions
  329. vector<Bbox> &droplets_raw //output, detect result
  330. )
  331. {
  332. //return number of detect result
  333. droplets_raw.clear();
  334. for (auto rect : drop_regions) {
  335. Mat roi = raw_img(rect);
  336. vector<Bbox> head_droplets = m_drop_detector.RunModel(roi, m_pLogger);
  337. if (m_pLogger) {
  338. stringstream buff_;
  339. buff_ << m_imgId << m_dtype_str << "-------crop_rect[" << rect.x << "," << rect.y << "," << rect.width
  340. << "," << rect.height << "],"
  341. << " roi image detect over. tea number is " << head_droplets.size();
  342. m_pLogger->INFO(buff_.str());
  343. }
  344. for (Bbox& b : head_droplets) {
  345. b.x1 += rect.x;
  346. b.x2 += rect.x;
  347. b.y1 += rect.y;
  348. b.y2 += rect.y;
  349. for (int i = 0; i < 5; ++i) {
  350. b.ppoint[2 * i] += rect.x;
  351. b.ppoint[2 * i + 1] += rect.y;
  352. }
  353. }
  354. if (head_droplets.size()) {
  355. droplets_raw.insert(
  356. droplets_raw.end(),
  357. head_droplets.begin(),
  358. head_droplets.end());
  359. }
  360. }
  361. return droplets_raw.size();
  362. }
  363. double CTeaSort::calculate_angle(
  364. Bbox&b, //input
  365. //bool need_precise_angle,//input
  366. double& grab_x, //output
  367. double& grab_y //output
  368. )
  369. {
  370. grab_x = grab_y = 0.0;
  371. double angle = 0.0;
  372. float x3,y3,x4,y4,x5,y5;
  373. x3 = b.ppoint[4];
  374. y3 = b.ppoint[5];
  375. x4 = b.ppoint[6];
  376. y4 = b.ppoint[7];
  377. x5 = b.ppoint[8];
  378. y5 = b.ppoint[9];
  379. if (m_dtype == img_type::tea_grab) {
  380. angle = atan2(x5 - x3, y5 - y3);
  381. calculate_stem_grab_position_opt(b, grab_x, grab_y, angle);
  382. //计算抓取点
  383. if (grab_x < 0 && grab_y < 0) {
  384. double pr = (double)m_cp.offset_grab;
  385. double dx = pr * sin(angle);
  386. double dy = pr * cos(angle);
  387. grab_x = x5 + dx;
  388. grab_y = y5 + dy;
  389. }
  390. }
  391. else {
  392. //tea cut, calculate line of p3 ans p4
  393. angle = atan2(x3 - x4, y3 - y4);
  394. calculate_stem_cut_position_opt(b, grab_x, grab_y, angle);
  395. }
  396. angle *= (180.0 / 3.1415926);
  397. return angle;
  398. }
  399. int CTeaSort::load_data(
  400. ImgInfo*imginfo,
  401. const char* fn/* = 0*/)
  402. {
  403. //数据加载功能实现,并生成imageid,保存原始数据到文件
  404. int rst = 0;
  405. //generate image id
  406. if (m_dtype == img_type::tea_grab) {
  407. m_imgId = getImgId(img_type::tea_grab);
  408. m_dtype_str = string(" tea_grab ");
  409. }
  410. else {
  411. m_imgId = getImgId(img_type::tea_cut);
  412. m_dtype_str = string(" tea_cut ");
  413. }
  414. if (imginfo) {
  415. if (m_pLogger) {
  416. stringstream buff;
  417. buff << "raw image stream: " << m_imgId << m_dtype_str << "image, width=" << imginfo->width
  418. << "\theight=" << imginfo->height << "\tchannels=" << imginfo->channel;
  419. m_pLogger->INFO(buff.str());
  420. }
  421. if (!isvalid(imginfo) || (imginfo->channel!=1 && imginfo->channel!=3)) {
  422. if (m_pLogger) {
  423. m_pLogger->ERRORINFO(m_imgId + m_dtype_str + "input image invalid.");
  424. }
  425. throw_msg(m_imgId + " invalid input image");
  426. }
  427. if (imginfo->channel == 1) {
  428. cv::Mat tmp_img = imginfo2mat(imginfo);
  429. vector<Mat> channels;
  430. for (size_t i = 0; i < 3; ++i) { channels.push_back(tmp_img); }
  431. cv::merge(channels, m_raw_img);
  432. }
  433. else {
  434. m_raw_img = imginfo2mat(imginfo);
  435. }
  436. if (m_pLogger) {
  437. stringstream buff;
  438. buff << "load image stream: " << m_imgId << m_dtype_str << "image, width=" << m_raw_img.cols
  439. << "\theight=" << m_raw_img.rows << "\tchannels=" << m_raw_img.channels();
  440. m_pLogger->INFO(buff.str());
  441. }
  442. }
  443. else {
  444. cv::Mat img = imread(fn, cv::IMREAD_COLOR);
  445. if (img.empty()) {
  446. if (m_pLogger) {
  447. m_pLogger->ERRORINFO(m_imgId + m_dtype_str + "input image invalid:" + string(fn));
  448. }
  449. throw_msg(m_imgId + m_dtype_str + "invalid input image: " + string(fn));
  450. }
  451. if (m_pLogger) {
  452. stringstream buff;
  453. buff <<"read image file: "<< m_imgId << m_dtype_str << "image, width=" << img.cols
  454. << "\theight=" << img.rows << "\tchannels=" << img.channels();
  455. m_pLogger->INFO(buff.str());
  456. }
  457. m_raw_img = img.clone();
  458. }
  459. if(m_dtype == img_type::tea_grab){
  460. double rot = m_cp.rot_degree_grab;
  461. if(fabs(rot)>1.0e-3){
  462. //rotate image
  463. cv::rotate(m_raw_img, m_raw_img,ROTATE_180);
  464. }
  465. }
  466. if (m_raw_img.channels() == 3 && m_dtype == img_type::tea_cut) {
  467. img_rgb2bgr(m_raw_img);
  468. }
  469. //image saver
  470. if (m_ppImgSaver && *m_ppImgSaver) {
  471. (*m_ppImgSaver)->saveImage(m_raw_img, m_imgId);
  472. if (m_pLogger) {
  473. stringstream buff;
  474. buff <<"saved: "<< m_imgId << m_dtype_str << "image, width=" << m_raw_img.cols
  475. << "\theight=" << m_raw_img.rows<<"\tchannels="<< m_raw_img.channels();
  476. m_pLogger->INFO(buff.str());
  477. }
  478. }
  479. return rst;
  480. }
  481. void CTeaSort::img_rgb2bgr(cv::Mat&img) {
  482. assert(img.channels() == 3);
  483. unsigned char pixel = 0;
  484. for (int r = 0; r < img.rows; ++r) {
  485. unsigned char* pRow = img.ptr(r);
  486. for (int c = 0; c < img.cols; ++c) {
  487. pixel = pRow[c*img.channels()];
  488. pRow[c*img.channels()] = pRow[c*img.channels() + 2];
  489. pRow[c*img.channels() + 2] = pixel;
  490. }
  491. }
  492. }
  493. int CTeaSort::load_model()
  494. {
  495. bool b = false;
  496. if (!m_drop_detector.IsModelLoaded()) {
  497. if (m_dtype == img_type::tea_grab) {
  498. b = m_drop_detector.LoadModel(m_cp.model_path_grab);
  499. }
  500. else {
  501. b = m_drop_detector.LoadModel(m_cp.model_path_cut);
  502. }
  503. }
  504. else {
  505. b = true;
  506. }
  507. return b ? 0 : 1;
  508. }
  509. void CTeaSort::clear_imginfo() {
  510. if (m_pImginfoDetected) {
  511. imginfo_release(&m_pImginfoDetected);
  512. m_pImginfoDetected = 0;
  513. }
  514. if (m_pImginfoRaw) {
  515. imginfo_release(&m_pImginfoRaw);
  516. m_pImginfoRaw = 0;
  517. }
  518. }
  519. int CTeaSort::generate_detect_windows(vector<Rect>&boxes)
  520. {
  521. boxes.clear();
  522. int grid_row = m_cp.grid_row_cut;
  523. int grid_col = m_cp.grid_col_cut;
  524. int grid_padding = m_cp.grid_padding_cut;
  525. if (m_dtype == img_type::tea_grab) {
  526. grid_row = m_cp.grid_row_grab;
  527. grid_col = m_cp.grid_col_grab;
  528. grid_padding = m_cp.grid_padding_grab;
  529. }
  530. if (grid_row < 1) { grid_row = 1; }
  531. if (grid_col < 1) { grid_col = 1; }
  532. if (grid_padding < 0) { grid_padding = 0; }
  533. int block_height = int(m_raw_img.rows / (float)grid_row + 0.5);
  534. int block_width = int(m_raw_img.cols / (float)grid_col + 0.5);
  535. for (int r = 0; r < grid_row; ++r) {
  536. for (int c = 0; c < grid_col; ++c) {
  537. int x0 = c*block_width - grid_padding;
  538. int y0 = r*block_height - grid_padding;
  539. int x1 = (c+1)*block_width + grid_padding;
  540. int y1 = (r+1)*block_height + grid_padding;
  541. if (x0 < 0) { x0 = 0; }
  542. if (y0 < 0) { y0 = 0; }
  543. if (x1 > m_raw_img.cols) { x1 = m_raw_img.cols; }
  544. if (y1 > m_raw_img.rows) { y1 = m_raw_img.rows; }
  545. Rect r(x0, y0, x1-x0, y1-y0);
  546. boxes.push_back(r);
  547. }
  548. }
  549. return boxes.size();
  550. }
  551. //void CTeaSort::calculate_stem_grab_position(
  552. // Bbox&b,
  553. // double& grab_x, //output
  554. // double& grab_y, //output
  555. // double& grab_angle //output
  556. //)
  557. //{
  558. //
  559. // grab_x = grab_y = -1.0;
  560. // //crop image
  561. // int padding = 2 * m_cp.offset_grab;
  562. // int y3 = int(b.ppoint[5]);
  563. // int y5 = int(b.ppoint[9]);
  564. // cv::Point p3(int(b.ppoint[4] - b.x1), int(b.ppoint[5] - b.y1));
  565. // cv::Point p4(int(b.ppoint[6] - b.x1), int(b.ppoint[7] - b.y1));
  566. // cv::Point p5(int(b.ppoint[8] - b.x1), int(b.ppoint[9] - b.y1));
  567. // cv::Mat crop_img;
  568. // if (y5 > y3) {
  569. // // Y position
  570. // int ymax = b.y2 + padding;
  571. // if (ymax > m_raw_img.rows) {
  572. // ymax = m_raw_img.rows;
  573. // }
  574. // crop_img = m_raw_img(cv::Range(b.y1, ymax), cv::Range(b.x1, b.x2)).clone();
  575. // }
  576. // else {
  577. // // ^ position
  578. // if (b.y1 - padding < 0) {
  579. // padding = b.y1;
  580. // }
  581. // p5.y = int(b.ppoint[9] - b.y1 + padding);
  582. // p4.y = int(b.ppoint[7] - b.y1 + padding);
  583. // p3.y = int(b.ppoint[5] - b.y1 + padding);
  584. // crop_img = m_raw_img(cv::Range(b.y1 - padding, b.y2), cv::Range(b.x1, b.x2)).clone();
  585. //
  586. // }
  587. // if (m_cp.image_show) {
  588. // cv::Mat crop_img_tmp = crop_img.clone();
  589. // cv::circle(crop_img_tmp, p3, 4, cv::Scalar(255, 0, 0), -1, 3, 0);
  590. // cv::circle(crop_img_tmp, p4, 4, cv::Scalar(0, 255, 0), -1, 3, 0);
  591. // cv::circle(crop_img_tmp, p5, 4, cv::Scalar(0, 0, 255), -1, 3, 0);
  592. //
  593. // imshow_wait("cropped box", crop_img_tmp);
  594. // }
  595. //
  596. // //to gray
  597. // cv::Mat gray_img;
  598. // if (crop_img.channels() == 1) { gray_img = crop_img; }
  599. // else {
  600. // cv::cvtColor(crop_img, gray_img, cv::COLOR_BGR2GRAY);
  601. // }
  602. // //binary
  603. // cv::Mat bin_img;
  604. // double th = cv::threshold(gray_img, bin_img, 255, 255, cv::THRESH_OTSU);
  605. // cv::bitwise_not(bin_img, bin_img);
  606. // if (m_cp.image_show) {
  607. // imshow_wait("cropped binary img", bin_img);
  608. // }
  609. //
  610. // // skeletonize() or medial_axis()
  611. // cv::Mat ske_img;
  612. // thinning(bin_img, ske_img);
  613. // /*if (m_cp.image_show) {
  614. // imshow_wait("skeleton img", ske_img);
  615. // }*/
  616. //
  617. // //遍历所有点,找到距离等于指定距离的点的位置, 以及距离p5最近的骨架上的点
  618. // std::vector<cv::Point> candidate_pts;
  619. // cv::Point p5_nearst;
  620. // double dist_th = 5;
  621. // double dist_min = 1.0e6;
  622. // for (int r = 0; r < ske_img.rows; ++r) {
  623. // for (int c = 0; c < ske_img.cols; ++c) {
  624. // if (ske_img.at<unsigned char>(r, c) == 0) { continue; }
  625. // double dist = std::powf((p5.x - c), 2) + std::powf((p5.y - r),2);
  626. // dist = std::sqrtf(dist);
  627. // if (dist < dist_min) {
  628. // dist_min = dist;
  629. // p5_nearst.x = c;
  630. // p5_nearst.y = r;
  631. // }
  632. // if (std::fabs(dist - m_cp.offset_grab) < dist_th) {
  633. // candidate_pts.push_back(cv::Point(c, r));
  634. // }
  635. // }
  636. // }
  637. //
  638. // //按与参考角度的差,找到有效的候选点集合
  639. // std::vector<cv::Point> valid_candidate_pts;
  640. // double ref_angle = atan2(p5.x - p3.x, p5.y - p3.y);
  641. // cv::Point p_min_angle(-1,-1);
  642. // double min_angle = CV_PI;
  643. // for (auto&p : candidate_pts) {
  644. // double angle_to_p3 = atan2(p.x - p3.x, p.y - p3.y);
  645. // //计算夹角
  646. // double fabs_angle = intersection_angle(ref_angle, angle_to_p3);
  647. // /*if (ref_angle > 0.5 * CV_PI) {
  648. // if (angle_to_p3 < 0) {
  649. // angle_to_p3 += 2 * CV_PI;
  650. // }
  651. // fabs_angle = std::fabs(angle_to_p3 - ref_angle);
  652. // }
  653. // else {
  654. // if (ref_angle < -0.5 * CV_PI) {
  655. // if (angle_to_p3 > 0) {
  656. // angle_to_p3 -= 2 * CV_PI;
  657. // }
  658. // fabs_angle = std::fabs(angle_to_p3 - ref_angle);
  659. // }
  660. // else {
  661. // fabs_angle = std::fabs(angle_to_p3 - ref_angle);
  662. // }
  663. // }*/
  664. // if (fabs_angle > CV_PI / 4.0) { continue; }
  665. // if (fabs_angle < min_angle) {
  666. // min_angle = fabs_angle;
  667. // p_min_angle.x = p.x;
  668. // p_min_angle.y = p.y;
  669. // }
  670. // valid_candidate_pts.push_back(p);
  671. // }
  672. // if (p_min_angle.x>0 && p_min_angle.y>0) {
  673. // grab_x = p_min_angle.x;
  674. // grab_y = p_min_angle.y;
  675. // }
  676. //
  677. // if (m_cp.image_show) {
  678. // cv::Mat ske_img_tmp = ske_img.clone();
  679. // for (auto&p : valid_candidate_pts) {
  680. // ske_img_tmp.at<unsigned char>(p) = 100;
  681. // }
  682. // cv::circle(ske_img_tmp, p5, 4, cv::Scalar(255, 0, 255), 1, 3, 0);
  683. // if (grab_x > 0 && grab_y > 0) {
  684. // cv::circle(ske_img_tmp, cv::Point(int(grab_x), int(grab_y)), 4, cv::Scalar(156, 0, 255), 1, 3, 0);
  685. // }
  686. // imshow_wait("skeleton img label", ske_img_tmp);
  687. // }
  688. //
  689. // //计算grab点的抓取角度
  690. // if (p_min_angle.x > 0 && p_min_angle.y > 0) {
  691. // grab_angle = get_grab_position(ske_img, p_min_angle, ref_angle);
  692. // }
  693. //
  694. // //重新得到grab_x,grab_y的坐标
  695. // if (grab_x > 0 && grab_y > 0) {
  696. // int real_padding_y = p5.y - int(b.ppoint[9] - b.y1);
  697. // grab_y -= real_padding_y;
  698. // grab_y += b.y1;
  699. // grab_x += b.x1;
  700. // }
  701. //
  702. //}
  703. /**
  704. * Code for thinning a binary image using Zhang-Suen algorithm.
  705. *
  706. * Author: Nash (nash [at] opencv-code [dot] com)
  707. * Website: http://opencv-code.com
  708. */
  709. /**
  710. * Perform one thinning iteration.
  711. * Normally you wouldn't call this function directly from your code.
  712. *
  713. * Parameters:
  714. * im Binary image with range = [0,1]
  715. * iter 0=even, 1=odd
  716. */
  717. void CTeaSort::thinningIteration(cv::Mat& img, int iter)
  718. {
  719. CV_Assert(img.channels() == 1);
  720. CV_Assert(img.depth() != sizeof(uchar));
  721. CV_Assert(img.rows > 3 && img.cols > 3);
  722. cv::Mat marker = cv::Mat::zeros(img.size(), CV_8UC1);
  723. int nRows = img.rows;
  724. int nCols = img.cols;
  725. if (img.isContinuous()) {
  726. nCols *= nRows;
  727. nRows = 1;
  728. }
  729. int x, y;
  730. uchar *pAbove;
  731. uchar *pCurr;
  732. uchar *pBelow;
  733. uchar *nw, *no, *ne; // north (pAbove)
  734. uchar *we, *me, *ea;
  735. uchar *sw, *so, *se; // south (pBelow)
  736. uchar *pDst;
  737. // initialize row pointers
  738. pAbove = NULL;
  739. pCurr = img.ptr<uchar>(0);
  740. pBelow = img.ptr<uchar>(1);
  741. for (y = 1; y < img.rows - 1; ++y) {
  742. // shift the rows up by one
  743. pAbove = pCurr;
  744. pCurr = pBelow;
  745. pBelow = img.ptr<uchar>(y + 1);
  746. pDst = marker.ptr<uchar>(y);
  747. // initialize col pointers
  748. no = &(pAbove[0]);
  749. ne = &(pAbove[1]);
  750. me = &(pCurr[0]);
  751. ea = &(pCurr[1]);
  752. so = &(pBelow[0]);
  753. se = &(pBelow[1]);
  754. for (x = 1; x < img.cols - 1; ++x) {
  755. // shift col pointers left by one (scan left to right)
  756. nw = no;
  757. no = ne;
  758. ne = &(pAbove[x + 1]);
  759. we = me;
  760. me = ea;
  761. ea = &(pCurr[x + 1]);
  762. sw = so;
  763. so = se;
  764. se = &(pBelow[x + 1]);
  765. int A = (*no == 0 && *ne == 1) + (*ne == 0 && *ea == 1) +
  766. (*ea == 0 && *se == 1) + (*se == 0 && *so == 1) +
  767. (*so == 0 && *sw == 1) + (*sw == 0 && *we == 1) +
  768. (*we == 0 && *nw == 1) + (*nw == 0 && *no == 1);
  769. int B = *no + *ne + *ea + *se + *so + *sw + *we + *nw;
  770. int m1 = iter == 0 ? (*no * *ea * *so) : (*no * *ea * *we);
  771. int m2 = iter == 0 ? (*ea * *so * *we) : (*no * *so * *we);
  772. if (A == 1 && (B >= 2 && B <= 6) && m1 == 0 && m2 == 0)
  773. pDst[x] = 1;
  774. }
  775. }
  776. img &= ~marker;
  777. }
  778. /**
  779. * Function for thinning the given binary image
  780. *
  781. * Parameters:
  782. * src The source image, binary with range = [0,255]
  783. * dst The destination image
  784. */
  785. void CTeaSort::thinning(const cv::Mat& src, cv::Mat& dst)
  786. {
  787. dst = src.clone();
  788. dst /= 255; // convert to binary image
  789. cv::Mat prev = cv::Mat::zeros(dst.size(), CV_8UC1);
  790. cv::Mat diff;
  791. do {
  792. thinningIteration(dst, 0);
  793. thinningIteration(dst, 1);
  794. cv::absdiff(dst, prev, diff);
  795. dst.copyTo(prev);
  796. } while (cv::countNonZero(diff) > 0);
  797. dst *= 255;
  798. }
  799. /**
  800. distance_thinning()
  801. distance transform based thinning
  802. -----disused
  803. */
  804. //void CTeaSort::distance_thinning(const cv::Mat& src, cv::Mat& dst)
  805. //{
  806. //
  807. // cv::Mat dist_mat(src.size(), CV_32FC1);
  808. // cv::distanceTransform(src, dist_mat, DIST_L2, 3);
  809. //
  810. // float max_dist = *max_element(dist_mat.begin<float>(), dist_mat.end<float>());
  811. // double r = 1.0;
  812. // if (max_dist > 1.0e-3) {
  813. // r = 255.0 / max_dist;
  814. // }
  815. // cv::Mat dist_img;
  816. // dist_mat.convertTo(dist_img, CV_8UC1, r, 0.0);
  817. //
  818. // cv::Canny(dist_img, dst, 50, 100, 7);
  819. //
  820. // unsigned char udist = *max_element(dst.begin<unsigned char>(), dst.end<unsigned char>());
  821. // if (m_cp.image_show) {
  822. // imshow_wait("dist_img", dist_img);
  823. // imshow_wait("canny", dst);
  824. // }
  825. //
  826. //
  827. //}
  828. /**
  829. part_thinning()
  830. 将图片缩小,thinning, 然后放大得到,用以提高效率
  831. */
  832. void CTeaSort::part_thinning(const cv::Mat& src, cv::Mat& dst)
  833. {
  834. cv::Mat part_img;
  835. cv::resize(src, part_img, cv::Size(src.cols / 2, src.rows / 2));
  836. cv::Mat part_ske_img;
  837. thinning(part_img, part_ske_img);
  838. cv::Mat gray_img;
  839. cv::resize(part_ske_img, gray_img, src.size());
  840. double th = cv::threshold(gray_img, dst, 255, 255, cv::THRESH_OTSU);
  841. /*if (m_cp.image_show) {
  842. imshow_wait("part_img", part_img);
  843. imshow_wait("part_ske_img", part_ske_img);
  844. imshow_wait("dst", dst);
  845. }*/
  846. }
  847. /**
  848. 计算 [-pi,pi]间的两个角间的夹角
  849. */
  850. double CTeaSort::intersection_angle(
  851. double ref_angle,
  852. double angle_to_p3
  853. )
  854. {
  855. //计算夹角
  856. double fabs_angle = 0;
  857. if (ref_angle > 0.5 * CV_PI) {
  858. if (angle_to_p3 < 0) {
  859. angle_to_p3 += 2 * CV_PI;
  860. }
  861. fabs_angle = std::fabs(angle_to_p3 - ref_angle);
  862. }
  863. else {
  864. if (ref_angle < -0.5 * CV_PI) {
  865. if (angle_to_p3 > 0) {
  866. angle_to_p3 -= 2 * CV_PI;
  867. }
  868. fabs_angle = std::fabs(angle_to_p3 - ref_angle);
  869. }
  870. else {
  871. fabs_angle = std::fabs(angle_to_p3 - ref_angle);
  872. }
  873. }
  874. return fabs_angle;
  875. }
  876. /**
  877. *
  878. */
  879. double CTeaSort::get_grab_position(
  880. const std::vector<cv::Point2f>& inner_pixels,
  881. const cv::Mat& skele_img,
  882. cv::Point&vertex,
  883. double ref_angle
  884. )
  885. {
  886. double grab_point_angle = CV_2PI;
  887. cv::Point pt0, pt1, pt2, pt3;
  888. double radius = static_cast<double>(m_cp.offset_grab) * 0.5;
  889. calc_bottom_vertex(vertex, ref_angle, CV_PI / 8.0, radius, pt0, pt1);
  890. calc_bottom_vertex(vertex, ref_angle+CV_PI, CV_PI / 8.0, radius, pt2, pt3);
  891. std::vector<cv::Point> triangle_region;
  892. triangle_region.push_back(pt0);
  893. triangle_region.push_back(pt1);
  894. triangle_region.push_back(pt2);
  895. triangle_region.push_back(pt3);
  896. //构建多边形,然后判别骨架图中在多边形内的骨架像素
  897. std::vector<cv::Point2f> curve_pts;
  898. for (auto&pt : inner_pixels) {
  899. double d = cv::pointPolygonTest(triangle_region, pt, false);
  900. // d 1-内部点, 0-边缘点 -1-外部点
  901. if (d > 0) {
  902. curve_pts.push_back(pt);
  903. }
  904. }
  905. //根据curve_pts进行曲线拟合,得到茎的曲线
  906. cv::Vec4f line_model;//[vx,vy, x0,y0], vx,vy---方向的归一化向量,x0,y0---直线上任意一点
  907. line_fit(curve_pts, line_model);
  908. double y_angle = atan2(line_model[0], line_model[1]);// y_angle in range [-pi, pi]
  909. double fabs_angle = intersection_angle(ref_angle, y_angle);
  910. double y_angle_inv = atan2(-line_model[0], -line_model[1]);; //y_angle_inv in range [-pi, pi]
  911. double fabs_angle_inv = intersection_angle(ref_angle, y_angle_inv);
  912. grab_point_angle = y_angle;
  913. if (fabs_angle_inv < fabs_angle) {
  914. grab_point_angle = y_angle_inv;
  915. }
  916. //可视化
  917. if (m_cp.image_show) {
  918. cv::Mat ske_img_tmp = skele_img.clone();
  919. for (auto&p : curve_pts) {
  920. ske_img_tmp.at<unsigned char>(p) = 100;
  921. }
  922. cv::circle(ske_img_tmp, vertex, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
  923. cv::circle(ske_img_tmp, pt0, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
  924. cv::circle(ske_img_tmp, pt1, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
  925. cv::circle(ske_img_tmp, pt2, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
  926. cv::circle(ske_img_tmp, pt3, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
  927. cv::line(ske_img_tmp, pt0, pt1, cv::Scalar(255, 215, 255), 2);
  928. cv::line(ske_img_tmp, pt0, pt3, cv::Scalar(255, 215, 255), 2);
  929. cv::line(ske_img_tmp, pt1, pt2, cv::Scalar(255, 215, 255), 2);
  930. cv::line(ske_img_tmp, pt2, pt3, cv::Scalar(255, 215, 255), 2);
  931. double dcx = radius * sin(grab_point_angle);
  932. double dcy = radius * cos(grab_point_angle);
  933. cv::Point dir_o;
  934. cv::Point dir_p;
  935. dir_o.x = vertex.x + 10;
  936. dir_o.y = vertex.y;
  937. dir_p.x = int(vertex.x + 10 + dcx);
  938. dir_p.y = int(vertex.y + dcy);
  939. cv::line(ske_img_tmp, dir_o, dir_p, cv::Scalar(255, 215, 255), 2);
  940. imshow_wait("grab angle", ske_img_tmp);
  941. }
  942. return grab_point_angle;
  943. }
  944. /**
  945. * calc_bottom_vertex
  946. * 找到等腰三角形两个底角点
  947. *
  948. *
  949. */
  950. void CTeaSort::calc_bottom_vertex(
  951. cv::Point&vertex, //input
  952. double ref_angle, //input, rad, 等腰三角形高的方向
  953. double delta_angle, //input, rad, 等腰三角形1/2分角
  954. double radius, //input, 等腰三角形腰长
  955. cv::Point&bpt0, //output
  956. cv::Point&bpt1 //output
  957. )
  958. {
  959. //double delta_angle = CV_PI / 8.0; // 22.5 degree
  960. //double radius = static_cast<double>(m_cp.offset_grab) * 1.5;
  961. double angle = ref_angle - delta_angle;
  962. int x = static_cast<int>(radius * sin(angle) + 0.5) + vertex.x;
  963. int y = static_cast<int>(radius * cos(angle) + 0.5) + vertex.y;
  964. bpt0.x = x;
  965. bpt0.y = y;
  966. angle = ref_angle + delta_angle;
  967. x = static_cast<int>(radius * sin(angle) + 0.5) + vertex.x;
  968. y = static_cast<int>(radius * cos(angle) + 0.5) + vertex.y;
  969. bpt1.x = x;
  970. bpt1.y = y;
  971. }
  972. //cv::Mat CTeaSort::poly_fit(
  973. // std::vector<cv::Point2f>& chain,
  974. // int n
  975. //)
  976. //{
  977. // //https://blog.csdn.net/jpc20144055069/article/details/103232641
  978. // cv::Mat y(chain.size(), 1, CV_32F, cv::Scalar::all(0));
  979. // cv::Mat phy(chain.size(), n, CV_32F, cv::Scalar::all(0));
  980. // for(int i=0;i<phy.rows;++i){
  981. // float* pr = phy.ptr<float>(i);
  982. // for(int j=0; j<phy.cols;++j){
  983. // pr[j] = pow(chain[i].x,j);
  984. // }
  985. // y.at<float>(i) = chain[i].y;
  986. // }
  987. //
  988. // cv::Mat phy_t = phy.t();
  989. // cv::Mat phyMULphy_t = phy.t() * phy;
  990. // cv::Mat phyMphyInv = phyMULphy_t.inv();
  991. // cv::Mat a = phyMphyInv * phy_t;
  992. // a = a*y;
  993. // return a;
  994. //}
  995. void CTeaSort::line_fit(std::vector<cv::Point2f>& key_point, cv::Vec4f& lines)
  996. {
  997. /*std::vector<cv::Point2f> pts;
  998. for (auto&p : key_point) {
  999. pts.push_back(cv::Point2f(p.x, p.y));
  1000. }*/
  1001. double param = 0;
  1002. double reps = 0.01;
  1003. double aeps = 0.01;
  1004. //cv::Vec4f lines;//[vx,vy, x0,y0], vx,vy---方向的归一化向量,x0,y0---直线上任意一点
  1005. cv::fitLine(key_point, lines, DIST_L1, param, reps, aeps);
  1006. }
  1007. //bool CTeaSort::poly_fit_cv(
  1008. //std::vector<cv::Point>& key_point,
  1009. //int n,
  1010. //cv::Mat& A
  1011. //)
  1012. //{
  1013. // //https://blog.csdn.net/KYJL888/article/details/103073956
  1014. // int N = key_point.size();
  1015. //
  1016. // //构造矩阵X
  1017. // cv::Mat X = cv::Mat::zeros(n+1, n+1, CV_64FC1);
  1018. // for(int i=0;i<n+1; ++i){
  1019. // for(int j=0;j<n+1;++j){
  1020. // for(int k=0;k<N;++k){
  1021. // X.at<double>(i,j) = X.at<double>(i,j) +
  1022. // std::pow(key_point[k].x, i+j);
  1023. // }
  1024. // }
  1025. // }
  1026. //
  1027. // //构造矩阵Y
  1028. // cv::Mat Y = cv::Mat::zeros(n+1, 1, CV_64FC1);
  1029. // for(int i=0;i<n+1;++i){
  1030. // for(int k=0;k<N;++k){
  1031. // Y.at<double>(i,0) = Y.at<double>(i,0) +
  1032. // std::pow(key_point[k].x, i) + key_point[k].y;
  1033. // }
  1034. // }
  1035. //
  1036. // A = cv::Mat::zeros(n+1, 1, CV_64FC1);
  1037. // cv::solve(X,Y,A,cv::DECOMP_LU);
  1038. // return true;
  1039. //}
  1040. //double CTeaSort::calc_fit_y(
  1041. //double x, //input
  1042. //cv::Mat& A //input
  1043. //)
  1044. //{
  1045. // //double y = A.at<double>(0,0) + A.at<double>(1,0) * x +
  1046. // // A.at<double>(2,0) * std::pow(x,2) + A.at<double>(3,0) * std::pow(x,3);
  1047. // //return y;
  1048. //
  1049. // double y = 0.0;
  1050. // for(int i=0; i<A.rows;++i){
  1051. // y += A.at<double>(i,0) * std::pow(x,i);
  1052. // }
  1053. // return y;
  1054. //}
  1055. //}
  1056. //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  1057. // calculate_stem_grab_position_opt()替代calculate_stem_grab_position函数
  1058. // 1)采用局部thinning方法提高效率
  1059. // 2) 重新用局部线性拟合的方向替代ref_angle(原始是p5和p3点连线与y正方向的夹角)
  1060. void CTeaSort::calculate_stem_grab_position_opt(
  1061. Bbox&b_original,
  1062. double& grab_x, //output
  1063. double& grab_y, //output
  1064. double& grab_angle //input-output
  1065. )
  1066. {
  1067. //扩展box的范围,4个方向全部扩展
  1068. Bbox b(b_original);
  1069. int padding_border = m_cp.offset_grab;
  1070. b.x1 -= padding_border;
  1071. b.x1 = b.x1 < 0 ? 0 : b.x1;
  1072. b.y1 -= padding_border;
  1073. b.y1 = b.y1 < 0 ? 0 : b.y1;
  1074. b.x2 += padding_border;
  1075. b.x2 = b.x2 < m_raw_img.cols ? b.x2 : m_raw_img.cols - 1;
  1076. b.y2 += padding_border;
  1077. b.y2 = b.y2 < m_raw_img.rows ? b.y2 : m_raw_img.rows - 1;
  1078. grab_x = grab_y = -1.0;
  1079. //crop image
  1080. int padding = 0;
  1081. int y3 = int(b.ppoint[5]);
  1082. int y5 = int(b.ppoint[9]);
  1083. cv::Point p3(int(b.ppoint[4] - b.x1), int(b.ppoint[5] - b.y1));
  1084. cv::Point p4(int(b.ppoint[6] - b.x1), int(b.ppoint[7] - b.y1));
  1085. cv::Point p5(int(b.ppoint[8] - b.x1), int(b.ppoint[9] - b.y1));
  1086. cv::Mat crop_img;
  1087. if (y5 > y3) {
  1088. // Y position
  1089. int ymax = b.y2 + padding;
  1090. if (ymax > m_raw_img.rows) {
  1091. ymax = m_raw_img.rows;
  1092. }
  1093. crop_img = m_raw_img(cv::Range(b.y1, ymax), cv::Range(b.x1, b.x2)).clone();
  1094. }
  1095. else {
  1096. // ^ position
  1097. if (b.y1 - padding < 0) {
  1098. padding = b.y1;
  1099. }
  1100. p5.y = int(b.ppoint[9] - b.y1 + padding);
  1101. p4.y = int(b.ppoint[7] - b.y1 + padding);
  1102. p3.y = int(b.ppoint[5] - b.y1 + padding);
  1103. crop_img = m_raw_img(cv::Range(b.y1 - padding, b.y2), cv::Range(b.x1, b.x2)).clone();
  1104. }
  1105. if (m_cp.image_show) {
  1106. cv::Mat crop_img_tmp = crop_img.clone();
  1107. cv::circle(crop_img_tmp, p3, 4, cv::Scalar(255, 0, 0), -1, 3, 0);
  1108. cv::circle(crop_img_tmp, p4, 4, cv::Scalar(0, 255, 0), -1, 3, 0);
  1109. cv::circle(crop_img_tmp, p5, 4, cv::Scalar(0, 0, 255), -1, 3, 0);
  1110. imshow_wait("cropped box", crop_img_tmp);
  1111. }
  1112. //to gray
  1113. cv::Mat gray_img;
  1114. if (crop_img.channels() == 1) { gray_img = crop_img; }
  1115. else {
  1116. cv::cvtColor(crop_img, gray_img, cv::COLOR_BGR2GRAY);
  1117. }
  1118. //binary
  1119. cv::Mat bin_img;
  1120. double th = cv::threshold(gray_img, bin_img, 255, 255, cv::THRESH_OTSU);
  1121. cv::bitwise_not(bin_img, bin_img);
  1122. if (m_cp.image_show) {
  1123. imshow_wait("cropped binary img", bin_img);
  1124. }
  1125. // skeletonize() or medial_axis()
  1126. cv::Mat ske_img;
  1127. //thinning(bin_img, ske_img);
  1128. part_thinning(bin_img, ske_img);
  1129. /*if (m_cp.image_show) {
  1130. imshow_wait("skeleton img", ske_img);
  1131. }*/
  1132. //获取ske_img中骨架上的点坐标
  1133. std::vector<cv::Point2f> ske_pixels;
  1134. for (int r = 1; r < ske_img.rows-1; ++r) {
  1135. for (int c = 1; c < ske_img.cols-1; ++c) {
  1136. if (ske_img.at<unsigned char>(r, c) == 0) { continue; }
  1137. ske_pixels.push_back(cv::Point2f(c, r));
  1138. }
  1139. }
  1140. //在grab_angle的指导下找到最优方向,截图,进行局部thinning
  1141. double ref_angle_init = grab_angle;
  1142. double delta_angle = CV_PI / 24.0;
  1143. double radius = static_cast<double>(m_cp.offset_grab);
  1144. cv::Point pt0, pt1, pt2, pt3;
  1145. double step_angle = CV_PI / 36.0; // 5 degree
  1146. int max_pixels = 0;
  1147. cv::Point pt0_opt, pt1_opt, pt2_opt, pt3_opt, center_opt;
  1148. //int minx_opt, maxx_opt, miny_opt, maxy_opt;
  1149. std::vector<cv::Point2f> ske_pixels_opt;
  1150. double target_angle_opt;
  1151. for (int i = -10; i <= 10; ++i) { //-30 degree ---- 30 degree
  1152. //在指定方向的矩形框内,找到内部点最多的方向,作为主方向
  1153. double target_angle = ref_angle_init + i*step_angle;
  1154. cv::Point center_pt;
  1155. center_pt.x = p5.x + static_cast<int>(radius * sin(target_angle));
  1156. center_pt.y = p5.y + static_cast<int>(radius * cos(target_angle));
  1157. calc_bottom_vertex(center_pt, target_angle, delta_angle, radius, pt0, pt1);
  1158. calc_bottom_vertex(center_pt, target_angle + CV_PI, delta_angle, radius, pt2, pt3);
  1159. std::vector<cv::Point> triangle_region;
  1160. triangle_region.push_back(pt0);
  1161. triangle_region.push_back(pt1);
  1162. triangle_region.push_back(pt2);
  1163. triangle_region.push_back(pt3);
  1164. //counting
  1165. int pixel_num = 0;
  1166. std::vector<cv::Point2f> inner_pixels;
  1167. for (auto&pt : ske_pixels) {
  1168. double d = cv::pointPolygonTest(triangle_region, pt, false);
  1169. // d 1-内部点, 0-边缘点 -1-外部点
  1170. if (d >= 0) {
  1171. pixel_num++;
  1172. inner_pixels.push_back(pt);
  1173. }
  1174. }
  1175. if (pixel_num > max_pixels) {
  1176. max_pixels = pixel_num;
  1177. pt0_opt = pt0;
  1178. pt1_opt = pt1;
  1179. pt2_opt = pt2;
  1180. pt3_opt = pt3;
  1181. center_opt = center_pt;
  1182. ske_pixels_opt.clear();
  1183. ske_pixels_opt.insert(ske_pixels_opt.begin(), inner_pixels.begin(), inner_pixels.end());
  1184. target_angle_opt = target_angle;
  1185. }
  1186. /*if (m_cp.image_show) {
  1187. cv::Mat bin_tmp = bin_img.clone();
  1188. cv::circle(bin_tmp, p5, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
  1189. cv::circle(bin_tmp, pt0, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
  1190. cv::circle(bin_tmp, pt1, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
  1191. cv::circle(bin_tmp, pt2, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
  1192. cv::circle(bin_tmp, pt3, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
  1193. cv::line(bin_tmp, pt0, pt1, cv::Scalar(180, 215, 255), 2);
  1194. cv::line(bin_tmp, pt0, pt3, cv::Scalar(180, 215, 255), 2);
  1195. cv::line(bin_tmp, pt1, pt2, cv::Scalar(180, 215, 255), 2);
  1196. cv::line(bin_tmp, pt2, pt3, cv::Scalar(180, 215, 255), 2);
  1197. imshow_wait("binary img box", bin_tmp);
  1198. }*/
  1199. }
  1200. //opt box process
  1201. if (m_cp.image_show) {
  1202. cv::Mat bin_tmp = ske_img.clone();
  1203. cv::circle(bin_tmp, p5, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
  1204. cv::circle(bin_tmp, pt0_opt, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
  1205. cv::circle(bin_tmp, pt1_opt, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
  1206. cv::circle(bin_tmp, pt2_opt, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
  1207. cv::circle(bin_tmp, pt3_opt, 4, cv::Scalar(156, 0, 255), 1, 3, 0);
  1208. cv::line(bin_tmp, pt0_opt, pt1_opt, cv::Scalar(180, 215, 255), 2);
  1209. cv::line(bin_tmp, pt0_opt, pt3_opt, cv::Scalar(180, 215, 255), 2);
  1210. cv::line(bin_tmp, pt1_opt, pt2_opt, cv::Scalar(180, 215, 255), 2);
  1211. cv::line(bin_tmp, pt2_opt, pt3_opt, cv::Scalar(180, 215, 255), 2);
  1212. imshow_wait("binary img box opt", bin_tmp);
  1213. }
  1214. //计算ref_angle
  1215. cv::Vec4f line_model;//[vx,vy, x0,y0], vx,vy---方向的归一化向量,x0,y0---直线上任意一点
  1216. line_fit(ske_pixels_opt, line_model);
  1217. double y_angle = atan2(line_model[0], line_model[1]);// y_angle in range [-pi, pi]
  1218. double fabs_angle = intersection_angle(target_angle_opt, y_angle);
  1219. double y_angle_inv = atan2(-line_model[0], -line_model[1]);; //y_angle_inv in range [-pi, pi]
  1220. double fabs_angle_inv = intersection_angle(target_angle_opt, y_angle_inv);
  1221. double ref_angle = y_angle;
  1222. if (fabs_angle_inv < fabs_angle) {
  1223. ref_angle = y_angle_inv;
  1224. }
  1225. //可视化
  1226. /*if (m_cp.image_show) {
  1227. cv::Mat ske_img_tmp = ske_img.clone();
  1228. for (auto&p : in_region_pts) {
  1229. ske_img_tmp.at<unsigned char>(p) = 100;
  1230. }
  1231. double dcx = radius * sin(ref_angle);
  1232. double dcy = radius * cos(ref_angle);
  1233. cv::Point dir_o;
  1234. cv::Point dir_p;
  1235. dir_o.x = center_opt.x + 10;
  1236. dir_o.y = center_opt.y;
  1237. dir_p.x = int(center_opt.x + 10 + dcx);
  1238. dir_p.y = int(center_opt.y + dcy);
  1239. cv::line(ske_img_tmp, dir_o, dir_p, cv::Scalar(255, 215, 255), 2);
  1240. imshow_wait("ref angle", ske_img_tmp);
  1241. }*/
  1242. //遍历所有点,找到距离等于指定距离的点的位置, 以及距离p5最近的骨架上的点
  1243. std::vector<cv::Point> candidate_pts;
  1244. cv::Point p5_nearst;
  1245. double dist_th = 5;
  1246. double dist_min = 1.0e6;
  1247. for (auto& pt : ske_pixels_opt) {
  1248. int c = int(pt.x);
  1249. int r = int(pt.y);
  1250. double dist = std::powf((p5.x - c), 2) + std::powf((p5.y - r), 2);
  1251. dist = std::sqrtf(dist);
  1252. if (dist < dist_min) {
  1253. dist_min = dist;
  1254. p5_nearst.x = c;
  1255. p5_nearst.y = r;
  1256. }
  1257. if (std::fabs(dist - m_cp.offset_grab) < dist_th) {
  1258. candidate_pts.push_back(cv::Point(c, r));
  1259. }
  1260. }
  1261. //按与参考角度的差,找到有效的候选点集合
  1262. std::vector<cv::Point> valid_candidate_pts;
  1263. cv::Point p_min_angle(-1, -1);
  1264. double min_angle = CV_PI;
  1265. for (auto&p : candidate_pts) {
  1266. double angle_to_p3 = atan2(p.x - p3.x, p.y - p3.y);
  1267. //计算夹角
  1268. double fabs_angle = intersection_angle(ref_angle, angle_to_p3);
  1269. if (fabs_angle > CV_PI / 4.0) { continue; }
  1270. if (fabs_angle < min_angle) {
  1271. min_angle = fabs_angle;
  1272. p_min_angle.x = p.x;
  1273. p_min_angle.y = p.y;
  1274. }
  1275. valid_candidate_pts.push_back(p);
  1276. }
  1277. if (p_min_angle.x>0 && p_min_angle.y>0) {
  1278. grab_x = p_min_angle.x;
  1279. grab_y = p_min_angle.y;
  1280. }
  1281. if (m_cp.image_show) {
  1282. cv::Mat ske_img_tmp = ske_img.clone();
  1283. for (auto&p : valid_candidate_pts) {
  1284. ske_img_tmp.at<unsigned char>(p) = 100;
  1285. }
  1286. cv::circle(ske_img_tmp, p5, 4, cv::Scalar(255, 0, 255), 1, 3, 0);
  1287. if (grab_x > 0 && grab_y > 0) {
  1288. cv::circle(ske_img_tmp, cv::Point(int(grab_x), int(grab_y)), 4, cv::Scalar(156, 0, 255), 1, 3, 0);
  1289. }
  1290. imshow_wait("skeleton img label", ske_img_tmp);
  1291. }
  1292. //计算grab点的抓取角度
  1293. if (p_min_angle.x > 0 && p_min_angle.y > 0) {
  1294. grab_angle = get_grab_position(ske_pixels_opt, ske_img, p_min_angle, ref_angle);
  1295. }
  1296. //重新得到grab_x,grab_y的坐标
  1297. if (grab_x > 0 && grab_y > 0) {
  1298. int real_padding_y = p5.y - int(b.ppoint[9] - b.y1);
  1299. grab_y -= real_padding_y;
  1300. grab_y += b.y1;
  1301. grab_x += b.x1;
  1302. }
  1303. }
  1304. void CTeaSort::calculate_stem_cut_position_opt(
  1305. Bbox&b,
  1306. double& grab_x, //output
  1307. double& grab_y, //output
  1308. double& grab_angle //input-output
  1309. )
  1310. {
  1311. int padding = 40;
  1312. grab_x = grab_y = -1.0;
  1313. //crop image
  1314. cv::Point p3o(int(b.ppoint[4]), int(b.ppoint[5]));
  1315. cv::Point p4o(int(b.ppoint[6]), int(b.ppoint[7]));
  1316. int x1, y1, x2, y2;
  1317. x1 = min(p3o.x, p4o.x);
  1318. y1 = min(p3o.y, p4o.y);
  1319. x2 = max(p3o.x, p4o.x);
  1320. y2 = max(p3o.y, p4o.y);
  1321. x1 -= padding;
  1322. x1 = x1 < 0 ? 0 : x1;
  1323. y1 -= padding;
  1324. y1 = y1 < 0 ? 0 : y1;
  1325. x2 += padding;
  1326. x2 = x2 < m_raw_img.cols ?x2 : m_raw_img.cols - 1;
  1327. y2 += padding;
  1328. y2 = y2 < m_raw_img.rows ? y2 : m_raw_img.rows - 1;
  1329. cv::Point p3(int(b.ppoint[4] - x1), int(b.ppoint[5] - y1));
  1330. cv::Point p4(int(b.ppoint[6] - x1), int(b.ppoint[7] - y1));
  1331. cv::Mat crop_img;
  1332. crop_img = m_raw_img(cv::Range(y1, y2), cv::Range(x1, x2)).clone();
  1333. if (m_cp.image_show) {
  1334. cv::Mat crop_img_tmp = crop_img.clone();
  1335. cv::circle(crop_img_tmp, p3, 4, cv::Scalar(255, 0, 0), -1, 3, 0);
  1336. cv::circle(crop_img_tmp, p4, 4, cv::Scalar(0, 255, 0), -1, 3, 0);
  1337. imshow_wait("cropped box", crop_img_tmp);
  1338. }
  1339. //to gray
  1340. cv::Mat gray_img;
  1341. if (crop_img.channels() == 1) { gray_img = crop_img; }
  1342. else {
  1343. cv::cvtColor(crop_img, gray_img, cv::COLOR_BGR2GRAY);
  1344. }
  1345. //binary
  1346. cv::Mat bin_img;
  1347. double th = cv::threshold(gray_img, bin_img, 255, 255, cv::THRESH_OTSU);
  1348. cv::bitwise_not(bin_img, bin_img);
  1349. if (m_cp.image_show) {
  1350. imshow_wait("cropped binary img", bin_img);
  1351. }
  1352. // skeletonize() or medial_axis()
  1353. cv::Mat ske_img;
  1354. thinning(bin_img, ske_img);
  1355. if (m_cp.image_show) {
  1356. imshow_wait("skeleton img", ske_img);
  1357. }
  1358. cv::Point2f center_pt;
  1359. center_pt.x = 0.5*(p3.x + p4.x);
  1360. center_pt.y = 0.5*(p3.y + p4.y);
  1361. //检查center_pt附近,是否有目标,如果有就用center_pt点作为切割点
  1362. int nnr = 3;
  1363. int cx, cy, knn, x, y;
  1364. cx = int(center_pt.x);
  1365. cy = int(center_pt.y);
  1366. knn = 0;
  1367. for (int r = -nnr; r <= nnr; ++r) {
  1368. y = r + cy;
  1369. if (y < 0 || y >= bin_img.rows) { continue; }
  1370. for (int c = -nnr; c <= nnr; ++c) {
  1371. x = cx + c;
  1372. if (x < 0 || x >= bin_img.cols) { continue; }
  1373. if (bin_img.at<unsigned char>(y, x) > 0) { knn++; }
  1374. }
  1375. }
  1376. if (knn > 0) {
  1377. grab_x = cx;
  1378. grab_y = cy;
  1379. grab_x += x1;
  1380. grab_y += y1;
  1381. return;
  1382. }
  1383. ///////////////////////////////////////////////////////////////////////////////////////////////////////
  1384. // 否则通过骨架化图,找到旁边的点(适用于茎弯曲的情况)
  1385. int min_x, min_y;
  1386. min_x = cx;
  1387. min_y = cy;
  1388. double min_loss = 1.0e6;
  1389. double ref_angle = grab_angle + CV_PI / 2.0;
  1390. if (ref_angle > CV_PI) {
  1391. ref_angle = ref_angle - 2 * CV_PI;
  1392. }
  1393. for (int r = 0; r < ske_img.rows; ++r) {
  1394. for (int c = 0; c < ske_img.cols; ++c) {
  1395. if (ske_img.at<unsigned char>(r, c) == 0) { continue; }
  1396. double target_angle = atan2(double(c- center_pt.x), double(r - center_pt.y));
  1397. double dangle = intersection_angle(ref_angle, target_angle);
  1398. if (dangle > CV_PI / 36.0) { continue; }
  1399. double dist = std::powf((center_pt.x - c), 2) + std::powf((center_pt.y - r), 2);
  1400. dist = std::sqrtf(dist);
  1401. double loss = dist;
  1402. // d 1-内部点, 0-边缘点 -1-外部点
  1403. if (loss < min_loss) {
  1404. min_loss = loss;
  1405. min_x = c;
  1406. min_y = r;
  1407. }
  1408. }
  1409. }
  1410. //另一个方向
  1411. ref_angle = grab_angle - CV_PI / 2.0;
  1412. if (ref_angle < -CV_PI) {
  1413. ref_angle = ref_angle + 2 * CV_PI;
  1414. }
  1415. for (int r = 0; r < ske_img.rows; ++r) {
  1416. for (int c = 0; c < ske_img.cols; ++c) {
  1417. if (ske_img.at<unsigned char>(r, c) == 0) { continue; }
  1418. double target_angle = atan2(double(c - center_pt.x), double(r - center_pt.y));
  1419. double dangle = intersection_angle(ref_angle, target_angle);
  1420. if (dangle > CV_PI / 36.0) { continue; }
  1421. double dist = std::powf((center_pt.x - c), 2) + std::powf((center_pt.y - r), 2);
  1422. dist = std::sqrtf(dist);
  1423. double loss = dist;
  1424. // d 1-内部点, 0-边缘点 -1-外部点
  1425. if (loss < min_loss) {
  1426. min_loss = loss;
  1427. min_x = c;
  1428. min_y = r;
  1429. }
  1430. }
  1431. }
  1432. grab_x = min_x;
  1433. grab_y = min_y;
  1434. grab_x += x1;
  1435. grab_y += y1;
  1436. }
  1437. }