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;
}
