void MPTreeMgr::buildInitMPTree() { vector<NodeList> node(4); // 0: BL; 1: BR; 2: TL;3: TR Node * pNode; int centerX , centerY , nodeX , nodeY , i , n; centerX = _chipWidth / 2; centerY = _chipHeight / 2; for ( i = 0 , n = _allNode.size() ; i < n ; ++i ) { pNode = _allNode[i]; nodeX = pNode->centerX(); nodeY = pNode->centerY(); if ( nodeX <= centerX ) { // left if ( nodeY <= centerY ) // BL node[0].push_back( pNode ); else // TL node[2].push_back( pNode ); } else { // right if ( nodeY <= centerY ) // BR node[1].push_back( pNode ); else // TR node[3].push_back( pNode ); } } initMPTreeRoot(); for ( i = 0 ; i < 4 ; ++i ) { if ( !buildRegionTree( node[i] ) ) { cout << "[Warning] Empty region! i = " << i << endl; continue; } if ( i == 3 ) { node[i][0]->_curPtr._p = _treeRoot[i-1]; _treeRoot[i-1]->_curPtr._right = node[i][0]; } else { node[i][0]->_curPtr._p = _treeRoot[i]; _treeRoot[i]->_curPtr._left = node[i][0]; } } if ( !checkMPTree() ) { cout << "[Error] build initial MP trees fail has failed.\n"; assert(0); } }
ProcessResult PluginDetectRobots::process(FrameData * data, RenderOptions * options) { (void)options; if (data==0) return ProcessingFailed; SSL_DetectionFrame * detection_frame = 0; detection_frame=(SSL_DetectionFrame *)data->map.get("ssl_detection_frame"); if (detection_frame == 0) detection_frame=(SSL_DetectionFrame *)data->map.insert("ssl_detection_frame",new SSL_DetectionFrame()); //acquire orange region list from data-map: CMVision::ColorRegionList * colorlist; colorlist=(CMVision::ColorRegionList *)data->map.get("cmv_colorlist"); if (colorlist==0) { printf("error in robot detection plugin: no region-lists were found!\n"); return ProcessingFailed; } //acquire color-labeled image from data-map: const Image<raw8> * image = (Image<raw8> *)(data->map.get("cmv_threshold")); if (image==0) { printf("error in robot detection plugin: no color-thresholded image was found!\n"); return ProcessingFailed; } CMPattern::Team * team=0; ::google::protobuf::RepeatedPtrField< ::SSL_DetectionRobot >* robotlist=0; int color_id; int num_robots; CMPattern::TeamDetector * detector; //TODO: lookup color label from LUT buildRegionTree(colorlist); bool need_reinit=_notifier.hasChanged(); for (int team_i = 0; team_i < 2; team_i++) { //team_i: 0==blue, 1==yellow if (team_i==0) { color_id=color_id_blue; team=global_team_selector_blue->getSelectedTeam(); num_robots=global_team_selector_blue->getNumberRobots(); detection_frame->clear_robots_blue(); robotlist=detection_frame->mutable_robots_blue(); detector=team_detector_blue; } else { color_id=color_id_yellow; team=global_team_selector_yellow->getSelectedTeam(); num_robots=global_team_selector_yellow->getNumberRobots(); detection_frame->clear_robots_yellow(); robotlist=detection_frame->mutable_robots_yellow(); detector=team_detector_yellow; } if (team!=0) { if (need_reinit) { detector->init(team); } detector->update(robotlist, color_id, num_robots, image, colorlist, reg_tree); } else { _notifier.changeSlotOtherChange(); } // printf("DETECTED %d robots on team %d\n",robotlist->size(),team_i); // fflush(stdout); } return ProcessingOk; }