RM装甲算法流程

图像处理与目标检测
- 装甲
- 神符
坐标空间转换，如何将目标的像素坐标->图像坐标->云台坐标->云台偏转角度
数据通信

1. 数据结构

装甲检测类ArmorDetector

//主要成员变量
bool _is_lost;
int _lost_cnt;
bool _is_small_armor;           // true if armor is the small one, otherwise false
cv::RotatedRect _res_last;      //上次检测到的装甲的旋转矩形
cv::Rect _dect_rect;            // detect roi of original image
ArmorParam _para;               // parameter of alg
cv::Mat _binary_template;       // 装甲模板的二值图像
cv::Mat _binary_template_small; // 小的装甲模板的二值图像
cv::Mat _src;                   // 原图像
cv::Mat _g;                     // 绿色通道图像
cv::Mat _ec;                    // 目标颜色通道的图像
cv::Mat _max_color;             // 阈值化处理的二值图像
cv::Size _size;

//主要成员函数
void initTemplate(const cv::Mat & _template, const cv::Mat & _template_small);
cv::RotatedRect getTargetAera(const cv::Mat & src);
void findContourInEnemyColor(
        cv::Mat & left, cv::Mat & right,
        std::vector<std::vector<cv::Point2i> > &contours_left,
        std::vector<std::vector<cv::Point2i> > &contours_right);

void findTargetInContours(
        const std::vector<std::vector<cv::Point> > & contours_left,
        const std::vector<std::vector<cv::Point> > & contours_right,
        std::vector<cv::RotatedRect> & rects,
        std::vector<double> & score);
void setImage(const cv::Mat & src);
cv::RotatedRect chooseTarget(const std::vector<cv::RotatedRect> & rects, const std::vector<double> & score);
int templateDist(const cv::Mat & img, bool is_small);
cv::RotatedRect boundingRRect(const cv::RotatedRect & left, const cv::RotatedRect & right);
cv::RotatedRect adjustRRect(const cv::RotatedRect & rect);

2. 函数调用过程

getTargetAera() -> setImage()

                -> findContourInEnemyColor() -> findContours()
                                            -> boundingRect()
                                                                                                                           
                -> findTargetInContours() -> minAreaRect()
                                          -> adjustRRect()                                                            
                                          -> boundingRRect()
                -> chooseTarget()

函数功能介绍：

cv::RotatedRect ArmorDetector::getTargetAera(const cv::Mat & src)

检测装甲的核心函数，功能是从输入的一帧图像中检测装甲的位置，返回装甲所在位置的一个旋转矩形。

void ArmorDetector::setImage(const cv::Mat & src)

对原图像进行通道分离，得到绿色通道图像，目标颜色通道图像，和二值化图像，复制给ArmorDetector的成员变量。

void ArmorDetector::findContourInEnemyColor(  cv::Mat & left, cv::Mat & right, 
                                              vector<vector<Point2i> > &contours_left,
                                              vector<vector<Point2i> > &contours_right
                                           )

该函数比较特别，四个参数均是输出参数，即函数执行的结果直接修改四个引用参数，不返回任何值。功能是，在二值图像中提取轮廓，并用外界矩形定位，然后在该矩形区域采用模板匹配，在left、right标记出左右灯柱的位置，在left和right上分别提取轮廓得到contours_left和contours_right。该函数比较繁琐。

void cv::findContours(  InputOutputArray    image,
                        OutputArrayOfArrays     contours,
                        OutputArray     hierarchy,
                        int     mode,
                        int     method,
                        Point   offset = Point() 
                        )

OpenCV提取轮廓的函数。

Rect cv::boundingRect   (   InputArray  points  )

OpenCV函数。对输入的点集求一个直立且外接的矩形。

void ArmorDetector::findTargetInContours(const vector<vector<Point> > & contours_left,
                                         const vector<vector<Point> > & contours_right,                                              vector<RotatedRect> & rects, 
                                         std::vector<double> & score
                                         )

在输入的可能的左右灯柱的轮廓中找到装甲的位置，并建立旋转矩形，添加到rects向量中。

RotatedRect cv::minAreaRect (   InputArray  points  )

opencv函数。再输入的点集中拟合一个面积最小的旋转矩形

cv::RotatedRect adjustRRect(const cv::RotatedRect & rect);

调整拟合上面函数拟合成的旋转矩形，保证height>width，且其角度为锐角。

cv::RotatedRect ArmorDetector::boundingRRect(const cv::RotatedRect & left, 
                                             const cv::RotatedRect & right)

根据左右灯柱的旋转矩形计算并返回装甲的旋转矩形。

cv::RotatedRect ArmorDetector::chooseTarget(const std::vector<cv::RotatedRect> & rects, 
                                            const std::vector<double> & score
                                            )

在候选的装甲所在的旋转矩形中选择最有可能的一个。

3. 检测算法

可以分为以下几个部分：

二值化(setImage)
利用模板在二值化图像中检测灯柱(findContourInEnemyColor)
拟合灯柱为旋转矩形(findTargetInContours)
在灯柱旋转矩形中定位装甲，并包装成旋转矩形(findTargetInContours)
在候选装甲的旋转矩形中选择最佳(chooseTarget)

注：括号内为程序中相关的函数。

下面详细介绍每一部分的算法流程

3.1 二值化

3.2 利用模板检测灯柱

模板匹配的过程：

让下面图像的中心遍历矩形内的每个像素，然后分别计算I区覆盖的灰度之和 $S_l$ ，II区覆盖的灰度之和 $S_m$ ，III区覆盖的灰度之和 $S_r$ ，计算 $\Delta{S_l}=S_m-S_l$ 和 $\Delta{S_r}=S_m-S_r$ ，当 $\Delta{S_l}>\theta$ 时认为模板的中心点可能位于装甲的左侧灯柱区域，因此，在left中的相应位置修改灰度值为255（初始化时left全为0）。当 $\Delta{S_r}>\theta$ 时认为模板的中心点可能位于装甲的右侧灯柱区域，因此，在right中的相应位置修改灰度值为255（初始化时right全为0）。

这里写图片描述

3.3 定位装甲

判断准则：

左右灯柱偏离垂直方向的角度的绝对值不超过阈值
左右灯柱角度差绝对值不超过阈值
左右灯柱中心点距离介于一定范围内
左右灯柱中心点的高度差不超过阈值

注意：程序在判断过程中根据判断准则对候选的装甲旋转矩形打分，以便于在后面的程序中选择最佳的旋转矩形作为攻击目标。

3.4 选择最终的攻击目标

若存在上一次的检测目标，则当前目标与上一次目标的宽度差不能超过50%
若接收云台pitch轴角度数据，则目标区域解算出来的高度不能高于/低于云台一定范围
灯柱周围区域的目标通道灰度均值需大于其他通道的灰度均值
黑色装甲中间区域绿色通道的梯度值较大的比例 r1 ， r1 必须小于一定阈值
目标二值图像与目标模板二值图像进行距离计算，距离比例 r2 等于与模板的汉明距离除以目标像素个数， r2 必须小于一定阈值
若仍然存在多个候选目标区域，则选择权重最小的区域作为目标区域

4. 部分源码

//装甲检测函数
cv::RotatedRect ArmorDetector::getTargetAera(const cv::Mat & src)
{
    setImage(src);
    cv::Mat contrast_left, contrast_right;
    vector<vector<Point2i> > contours_left;
    vector<vector<Point2i> > contours_right;
    findContourInEnemyColor(contrast_left, contrast_right, contours_left, contours_right);
    vector<RotatedRect> rects;
    vector<double> score;
    findTargetInContours(contours_left, contours_right, rects, score);
    RotatedRect final_rect = chooseTarget(rects, score);

#ifdef SHOW_DEBUG_IMG
    Mat rect_show;
    _src.copyTo(rect_show);
    for (size_t i = 0; i < rects.size(); i++)   {
        Scalar color(rand() & 255, rand() & 255, rand() & 255);
        Point2f vertices[4];
        rects[i].points(vertices);
        for (int i = 0; i < 4; i++)
            line(rect_show, vertices[i], vertices[(i + 1) % 4], color);
    }

    Point2f vertices[4];
    final_rect.points(vertices);
    for (int i = 0; i < 4; i++)
        line(rect_show, vertices[i], vertices[(i + 1) % 4], CV_RGB(255, 0, 0));
    imshow("5.rect", rect_show);
#endif

    if(final_rect.size.width != 0){
        final_rect.center.x += _dect_rect.x;
        final_rect.center.y += _dect_rect.y;
        _res_last = final_rect;
        _lost_cnt = 0;
    }
    else{
        ++_lost_cnt;

        if (_lost_cnt < 3)
            _res_last.size =Size2f(_res_last.size.width * 2, _res_last.size.height * 1.5);
        else if(_lost_cnt == 6)
            _res_last.size =Size2f(_res_last.size.width * 1.5, _res_last.size.height * 1.5);
        else if(_lost_cnt == 12)
            _res_last.size =Size2f(_res_last.size.width * 1.5, _res_last.size.height * 1.5);
        else if(_lost_cnt == 18)
            _res_last.size =Size2f(_res_last.size.width * 1.5, _res_last.size.height * 1.5);
        else if (_lost_cnt > 60 )
            _res_last = RotatedRect();
    }
    return final_rect;
}

void ArmorDetector::setImage(const cv::Mat & src)
{
    _size = src.size();
    const cv::Point & last_result = _res_last.center;
    if(last_result.x == 0 || last_result.y == 0){
        _src = src;
        _dect_rect = Rect(0, 0, src.cols, src.rows);
    }
    else{
        Rect rect = _res_last.boundingRect();
        int max_half_w = _para.max_light_delta_h * 1.3;
        int max_half_h = 300;
        double scale = src.rows == 480 ? 1.8 : 2.5;

        int exp_half_w = min(max_half_w / 2, int(rect.width * scale));
        int exp_half_h = min(max_half_h / 2, int(rect.height * scale));

        int w = std::min(max_half_w, exp_half_w);
        int h = std::min(max_half_h, exp_half_h);
        Point center = last_result;
        int x = std::max(center.x - w, 0);
        int y = std::max(center.y - h, 0);
        Point lu = Point(x, y);
        x = std::min(center.x + w, src.cols);
        y = std::min(center.y + h, src.rows);
        Point rd = Point(x, y);
        _dect_rect = Rect(lu, rd);
        if (makeRectSafe(_dect_rect, src.size()) == false){
            _res_last = cv::RotatedRect();
            _dect_rect = Rect(0, 0, src.cols, src.rows);
            _src = src;
        }
        else
            src(_dect_rect).copyTo(_src);
    }

    int total_pixel = _src.cols * _src.rows;
    const uchar * ptr_src = _src.data;
    const uchar * ptr_src_end = _src.data + total_pixel * 3;

    _g.create(_src.size(), CV_8UC1);
    _ec.create(_src.size(), CV_8UC1);
    _max_color = cv::Mat(_src.size(), CV_8UC1, cv::Scalar(0));
    uchar *ptr_g = _g.data, *ptr_ec = _ec.data, *ptr_max_color = _max_color.data;
    if (_para.enemy_color == RED){
        for (; ptr_src != ptr_src_end; ++ptr_src, ++ptr_g, ++ptr_max_color, ++ptr_ec)   {
            uchar b = *ptr_src;
            uchar g = *(++ptr_src);
            uchar r = *(++ptr_src);
            *ptr_g = g;
            *ptr_ec = r;
            //*ptr_g = b;
            if (r > _para.min_light_gray)
                *ptr_max_color = 255;
        }
    }
    else {
        for (; ptr_src != ptr_src_end; ++ptr_src, ++ptr_g, ++ptr_max_color, ++ptr_ec)   {
            uchar b = *ptr_src;
            uchar g = *(++ptr_src);
            uchar r = *(++ptr_src);
            *ptr_g = g;
            *ptr_ec = b;
            //*ptr_g = r;
            if (b > _para.min_light_gray)
                *ptr_max_color = 255;
        }
    }

#ifdef SHOW_DEBUG_IMG
    cv::imshow("g", _g);
    cv::imshow("_max_color", _max_color);
#endif
}

void ArmorDetector::findContourInEnemyColor(
        cv::Mat & left, cv::Mat & right, 
        vector<vector<Point2i> > &contours_left,
        vector<vector<Point2i> > &contours_right)
{
    // find contour in sub image of blue and red
    vector<vector<Point2i> > contours_br;
    vector<Vec4i> hierarchy;
    findContours(_max_color, contours_br, hierarchy, CV_RETR_EXTERNAL , CV_CHAIN_APPROX_SIMPLE);
    vector<vector<Point2i> >::const_iterator it = contours_br.begin();
    left = Mat::zeros(_max_color.size(), CV_8UC1);
    right = Mat::zeros(_max_color.size(), CV_8UC1);
    const int margin_l = 1, margin_r = 10;
    const int margin_h = 3;

    while (it != contours_br.end()){
        Rect rect = cv::boundingRect(*it);
        if (rect.height < _para.min_light_height){
            ++it;
            continue;
        }

        int max_i = rect.x + rect.width;
        max_i = std::min(_max_color.cols, max_i + margin_r);
        int half_j = (margin_h >> 1), max_j = rect.y + rect.height, min_j = rect.y;
        max_j = std::min(_max_color.rows - 1, max_j + half_j);
        min_j = std::max(min_j, half_j);
        int count_left = 0, count_right = 0;

        const uchar * ptr_gray_base = _g.data;
        for (size_t j = min_j; j < max_j; ++j)  {
            const uchar * ptr_gray = ptr_gray_base + j * _g.cols;
            for (size_t i = rect.x; i < max_i; ++i) {
                if (*(ptr_gray + i) < _para.min_light_gray)
                    continue;

                float block0 = 0, block1 = 0, block_1 = 0;
                // do margin protection
                if (i >= margin_r){
                    for (int m = -half_j; m <= half_j; ++m) {
                        const uchar * ptr = ptr_gray + m * _g.cols + i;
                        // for common 'o' of template
                        for (int k = 0; k < margin_l; k++)
                            block0 += *(ptr + k);
                        // for 'x' of left template
                        for (int k = margin_l; k < margin_r; k++)
                            block1 += *(ptr + k);
                        // for 'x' of right template
                        for (int k = -margin_l; k > -margin_r; --k)
                            block_1 += *(ptr + k);
                    }
                    block0 /= margin_h * margin_l;
                    block1 /= margin_h * (margin_r - margin_l);
                    block_1 /= margin_h * (margin_r - margin_l);
                    int avgdist = block0 - block1;
                    left.at<uchar>(j, i) = avgdist > _para.avg_contrast_threshold ? (++count_left, 255) : 0;
                    avgdist = block0 - block_1;
                    right.at<uchar>(j, i) = avgdist > _para.avg_contrast_threshold ? (++count_right, 255) : 0;
                }
                else {
                    for (int m = -half_j; m <= half_j; ++m) {
                        const uchar * ptr = ptr_gray + m * _g.cols + i;
                        // for 'o' of left template
                        for (int k = 0; k < margin_l; k++)
                            block0 += *(ptr + k);
                        // for 'x' of left template
                        for (int k = margin_l; k < margin_r; k++)
                            block1 += *(ptr + k);;
                    }
                    block0 /= margin_h * margin_l;
                    block1 /= margin_h * (margin_r - margin_l);
                    int avgdist = block0 - block1;
                    left.at<uchar>(j, i) = avgdist > _para.avg_contrast_threshold ? (++count_left, 255) : 0;
                }
            }
        }
        if (count_left > 10){
            vector<vector<Point2i> > contour;
            vector<Vec4i> hierarchy;
            findContours(left(rect), contour, hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cv::Point2i(rect.x, rect.y));
            contours_left.insert(contours_left.end(), contour.begin(), contour.end());
        }

        if (count_right > 10){
            vector<vector<Point2i> > contour;
            vector<Vec4i> hierarchy;
            findContours(right(rect), contour, hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cv::Point2i(rect.x, rect.y));
            contours_right.insert(contours_right.end(), contour.begin(), contour.end());
        }
        ++it;
    }
}

void ArmorDetector::findTargetInContours(const vector<vector<Point> > & contours_left, const vector<vector<Point> > & contours_right, vector<RotatedRect> & rects, std::vector<double> & score)
{
    // 用直线拟合轮廓，找出符合斜率范围的轮廓
    vector<RotatedRect> final_contour_rect_left, final_contour_rect_right;
    vector<double> score_left, score_right;
#ifdef SHOW_DEBUG_IMG
    Mat contours_show_left, contours_show_right;
    _src.copyTo(contours_show_left);
    _src.copyTo(contours_show_right);
#endif

    for (size_t i = 0; i < contours_left.size(); ++i){
        // fit the lamp contour as a eclipse
        RotatedRect rrect = minAreaRect(contours_left[i]);
        rrect = adjustRRect(rrect);
        double angle = rrect.angle;
        angle = 90 - angle;
        angle = angle < 0 ? angle + 180 : angle;

#ifdef SHOW_DEBUG_IMG
        //cout << "(angle)\t(" << angle << ")\n";
        Scalar color(rand() & 255, rand() & 255, rand() & 255);
        char slope_str[15];
        sprintf(slope_str, "%.1f", angle);
        putText(contours_show_left, slope_str, contours_left[i][0], CV_FONT_HERSHEY_COMPLEX_SMALL, 0.5, color, 1);
        drawContours(contours_show_left, contours_left, i, color, CV_FILLED, 8);
#endif
        // the contour must be near-vertical
        float delta_angle = abs(angle - 90);
        if (delta_angle < _para.light_slope_offset)
        {
            final_contour_rect_left.push_back(rrect);
            score_left.push_back(delta_angle);
        }
    }

    for (size_t i = 0; i < contours_right.size(); ++i){
        // fit the lamp contour as a eclipse
        RotatedRect rrect = minAreaRect(contours_right[i]);
        rrect = adjustRRect(rrect);
        double angle = rrect.angle;
        angle = 90 - angle;
        angle = angle < 0 ? angle + 180 : angle;

#ifdef SHOW_DEBUG_IMG
        //cout << "(angle)\t(" << angle << ")\n";
        Scalar color(rand() & 255, rand() & 255, rand() & 255);
        char slope_str[15];
        sprintf(slope_str, "%.1f", angle);
        putText(contours_show_right, slope_str, contours_right[i][0], CV_FONT_HERSHEY_COMPLEX_SMALL, 0.5, color, 1);
        drawContours(contours_show_right, contours_right, i, color, CV_FILLED, 8);
#endif
        // the contour must be near-vertical
        float delta_angle = abs(angle - 90);
        if (delta_angle < _para.light_slope_offset){
            final_contour_rect_right.push_back(rrect);
            score_right.push_back(delta_angle);
        }
    }

    // using all the left edge and right edge to make up rectangles
    for (size_t i = 0; i < final_contour_rect_left.size(); ++i) {
        const RotatedRect & rect_i = final_contour_rect_left[i];
        const Point & center_i = rect_i.center;
        float xi = center_i.x;
        float yi = center_i.y;

        for (size_t j = 0; j < final_contour_rect_right.size(); j++) {
            const RotatedRect & rect_j = final_contour_rect_right[j];
            const Point & center_j = rect_j.center;
            float xj = center_j.x;
            float yj = center_j.y;
            float delta_h = xj - xi;
            float delta_angle = abs(rect_j.angle - rect_i.angle);

            // if rectangle is match condition, put it in candidate vector
            if (delta_h > _para.min_light_delta_h && delta_h < _para.max_light_delta_h &&
                abs(yi - yj) < _para.max_light_delta_v &&
                delta_angle < _para.max_light_delta_angle) {
                RotatedRect rect = boundingRRect(rect_i, rect_j);
                rects.push_back(rect);
                score.push_back((score_right[j] + score_left[i]) / 6.0 + delta_angle);
            }
        }
    }
#ifdef SHOW_DEBUG_IMG
    imshow("4.contours_l", contours_show_left);
    imshow("4.contours_r", contours_show_right);
#endif
}

RotatedRect ArmorDetector::adjustRRect(const RotatedRect & rect)
{
    const Size2f & s = rect.size;
    if (s.width < s.height)
        return rect;
    return RotatedRect(rect.center, Size2f(s.height, s.width), rect.angle + 90.0);
}

cv::RotatedRect ArmorDetector::boundingRRect(const cv::RotatedRect & left, const cv::RotatedRect & right)
{
    const Point & pl = left.center, & pr = right.center;
    Point2f center = (pl + pr) / 2.0;
    cv::Size2f wh_l = left.size;
    cv::Size2f wh_r = right.size;
    float width = POINT_DIST(pl, pr) - (wh_l.width + wh_r.width) / 2.0;
    float height = std::max(wh_l.height, wh_r.height);
    //float height = (wh_l.height + wh_r.height) / 2.0;
    float angle = std::atan2(right.center.y - left.center.y, right.center.x - left.center.x);
    return RotatedRect(center, Size2f(width, height), angle * 180 / CV_PI);
}

cv::RotatedRect ArmorDetector::chooseTarget(const std::vector<cv::RotatedRect> & rects, const std::vector<double> & score) 
{
    if (rects.size() < 1){
        _is_lost = true;
        return RotatedRect();
    }

    int ret_idx = -1;
    double avg_score = 0.0;
    for(int i = 0; i < score.size(); ++i){
        avg_score += score[i];
    }
    avg_score /= score.size();
    const double small_armor_wh_threshold = 3.6;
    const double avg_slope = 4.0;
    const double exp_weight_scale = 15.0;
    const double degree2rad_scale = 3.1415926 / 180.0;
    double percent_large_grad_threshold = 0.25;
    double template_dist_threshold = 0.20;
    double max_wh_ratio = 5.2, min_wh_ratio = 1.25;
    if( _is_lost == false){
        template_dist_threshold = 0.4;
        percent_large_grad_threshold = 0.5;
        max_wh_ratio += 0.5;
        min_wh_ratio -= 0.2;
    }
    double weight = template_dist_threshold * percent_large_grad_threshold / exp(-(avg_slope + avg_score) * degree2rad_scale * exp_weight_scale);
    bool is_small = false;

    #define SafeRect(rect, max_size) {if (makeRectSafe(rect, max_size) == false) continue;}

    for (size_t i = 0; i < rects.size(); ++i){
        const RotatedRect & rect = rects[i];

        // the ratio of width and height must be matched
        double w = rect.size.width;
        double h = rect.size.height;
        double wh_ratio = w / h;
        if (wh_ratio > max_wh_ratio || wh_ratio < min_wh_ratio)
            continue;

        AngleSolver * slover = NULL;
        if(_size.height == 480)
            slover = s_solver;
        else if(_size.height == 720)
            slover = l_solver;

        if (wh_ratio < small_armor_wh_threshold)
            is_small = true;
        else
            is_small = false;

        if (slover != NULL && _is_lost){
            is_small == true ? slover->setTargetSize(12.4, 5.4) : slover->setTargetSize(21.6, 5.4);
            double angle_y = 0.0, angle_x = 0.0;
            if (false == slover->getAngle(rect, angle_x, angle_y, 0., 0., cv::Point(_dect_rect.x, _dect_rect.y)))
                continue;
            Mat xyz_in_ptz = slover->position_in_ptz;
            double d = sqrt(xyz_in_ptz.at<double>(1) * xyz_in_ptz.at<double>(1) + xyz_in_ptz.at<double>(2) * xyz_in_ptz.at<double>(2));
            double t_offset = sin((pitch_angle -  angle_y) * 3.1415926 / 180.0) * d;
            if(t_offset < -40.0 || t_offset > 35.0){
#ifdef COUT_LOG
                cout << "refused : target out of range: " <<
                        "\n\tcurrent ptz angle: " << pitch_angle <<
                        "\n\tcurrent target position: "  << xyz_in_ptz.t() <<
                        "\n\tcurrent target offet set: " << t_offset << "\n";
#endif
                continue;
            }
            if(t_offset > 5.0)
                is_small = false;
            else
                is_small = true;
        }
        // width must close to the last result
        const Size2f size_last = _res_last.size;
        if(_is_lost == false && size_last.width > _para.min_light_delta_h){
            double percent = 0.50 * size_last.width;
            if (abs(w - size_last.width) > percent){
#ifdef COUT_LOG
                cout << "refused 0 : size_last.width: " << size_last.width << "\tcur width: "  << w << endl;
#endif
                continue;
            }
        }

        // rotate the area
        int lamp_width = max((int)w / (is_small ? 12 : 20), 1);
        cv::Rect bounding_roi = rect.boundingRect();
        bounding_roi.x -= w / 8;
        bounding_roi.width += w / 4;
        SafeRect(bounding_roi, _src.size());

        Point2f new_center = rect.center - Point2f(bounding_roi.x, bounding_roi.y);
        Mat roi_src = _src(bounding_roi);
        Mat rotation = getRotationMatrix2D(new_center, rect.angle, 1);
        Mat rectify_target;
        cv::warpAffine(roi_src, rectify_target, rotation, bounding_roi.size());

        // get the black board of the armor
        cv::Point ul = Point(std::max(int(new_center.x - (w / 2.0)) + 1, 0), std::max((int)(new_center.y - h / 2.0), 0));
        cv::Point dr = Point(new_center.x + w / 2.0, new_center.y + h / 2.0);
        Rect roi_black = Rect(cv::Point(ul.x, ul.y), cv::Point(dr.x, dr.y));
        // get the left lamp and right lamp of the armor
        Rect roi_left_lamp = Rect(Point(max(0, ul.x - lamp_width), ul.y), Point(max(rectify_target.cols, ul.x), dr.y));
        Rect roi_right_lamp = Rect(Point(dr.x , ul.y), Point(min(dr.x + lamp_width, rectify_target.cols), dr.y));

        SafeRect(roi_left_lamp, rectify_target.size());
        SafeRect(roi_right_lamp, rectify_target.size());
        SafeRect(roi_black, rectify_target.size());

        // valid the gray value of black area
        Mat black_part;
        rectify_target(roi_black).copyTo(black_part);
        int black_side = min(_para.min_light_delta_h / 2, 4);
        Mat gray_mid_black(Size(roi_black.width - black_side * 2, roi_black.height), CV_8UC1);
        const uchar * ptr = black_part.data;
        uchar * ptr_gray = gray_mid_black.data;
        int avg_green_mid = 0;
        int avg_red_side = 0;
        int avg_blue_side = 0;
        int avg_green_side = 0;
        int cf = black_part.cols - black_side;
        for(int j = 0; j < black_part.rows; ++j){
            for (int k = 0; k < black_side; ++k, ++ptr){
                uchar b = *ptr;
                uchar g = *(++ptr);
                uchar r = *(++ptr);
                avg_red_side += r;
                avg_blue_side += b;
                avg_green_side += g;
            }
            for (int k = black_side; k < cf; ++k, ++ptr, ++ptr_gray){
                uchar b = *ptr;
                uchar g = *(++ptr);
                uchar r = *(++ptr);
                avg_green_mid += g;
                *ptr_gray = (uchar)((r * 38 + g * 75 + b * 15) >> 7);
            }
            for (int k = cf; k < black_part.cols; ++k, ++ptr){
                uchar b = *ptr;
                uchar g = *(++ptr);
                uchar r = *(++ptr);
                avg_red_side += r;
                avg_blue_side += b;
                avg_green_side += g;
            }
        }
        avg_green_mid /= (gray_mid_black.cols * gray_mid_black.rows);
        int side_total = black_side * 2 * gray_mid_black.rows;
        avg_green_side /= side_total;
        if (avg_green_mid > _para.avg_board_gray_threshold){
#ifdef COUT_LOG
            cout << "refused 1 : avg_green: " << avg_green_mid << "\tavg_board_gray_threshold: "  << (int)_para.avg_board_gray_threshold << endl;
#endif
            continue;
        }

        if (_para.enemy_color == RED && avg_red_side - 10 < avg_blue_side){
#ifdef COUT_LOG
            cout << "refused 1.1 : red < blue:  red:" << avg_red_side/side_total << "\tblue: "  << avg_blue_side/side_total << endl;
#endif
            continue;
        }
        else if (_para.enemy_color == BLUE && avg_blue_side - 10 < avg_red_side){
#ifdef COUT_LOG
            cout << "refused 1.2 : red > blue:  red:" << avg_red_side/side_total << "\tblue: "  << avg_blue_side/side_total << endl;
#endif
            continue;
        }

        // valid the gradient of the black area
        Mat gradX, gradY;
        cv::Sobel(gray_mid_black, gradX, CV_16S, 1, 0);
        cv::Sobel(gray_mid_black, gradY, CV_16S, 0, 1);

        int y_grad = 0, x_grad = 0;
        int large_grad_count = 0;
        int side_width = gray_mid_black.cols * 10.0 / 100; // jump over the side；
        short * ptr_x = (short *)gradX.data;
        short * ptr_y = (short *)gradY.data;
        for (size_t j = 0; j < gradX.rows; ++j){
            // jump over left side part
            ptr_x+= side_width;
            ptr_y+= side_width;

            // compute the middle part
            int up_b = gradX.cols - side_width;
            for (size_t k = side_width; k < up_b; ++k, ++ptr_x, ++ptr_y)    {
                int x = abs(*ptr_x);
                int y = abs(*ptr_y);
                x_grad += x;
                y_grad += y;
                large_grad_count += y / _para.grad_threshold;
            }
            // jump over right side part
            ptr_x+= gradX.cols-up_b;
            ptr_y+= gradX.cols-up_b;
        }

        // kick out the area of large gradients
        int total_pixel = (gradX.cols - (side_width << 1)) * gradX.rows;
        double large_grad_percent =(double)large_grad_count/total_pixel;
        if(large_grad_percent > percent_large_grad_threshold){
#ifdef COUT_LOG
            cout << "refused 2: large_grad_percent: " << large_grad_percent <<  endl;
#endif
            continue;
        }

        // valid the average gradient of the black area
        double avg_x = (double)x_grad / total_pixel;
        double avg_y = (double)y_grad / total_pixel;
        if (avg_x < _para.avg_board_grad_threshold + 30 && avg_y < _para.avg_board_grad_threshold ){
            Point p1(roi_left_lamp.x, roi_left_lamp.y);
            Point p2(roi_right_lamp.x+roi_right_lamp.width, roi_right_lamp.y+roi_right_lamp.height);
            // get the whole area of armor
            Rect armor_rect(p1, p2);
            SafeRect(armor_rect, rectify_target.size());
            Mat armor = rectify_target(armor_rect);

            // compute the distance of template
            cv::Size cur_size;
            if (is_small)
                cur_size = _binary_template_small.size();
            else
                cur_size = _binary_template.size();
            resize(armor, armor, cur_size);
            double dist = templateDist(armor, is_small);
            dist = dist / (cur_size.width * cur_size.height);
            if(dist >  template_dist_threshold){
#ifdef COUT_LOG
                cout << "refused 3: dist: " << dist << "\tdist threshold:" << (cur_size.width * cur_size.height) / 4 << endl;
#endif
                continue;
            }

            // choose the best rectangle with minimum (large_grad_percent * dist)
            dist = max(dist, 0.001);
            large_grad_percent = max(large_grad_percent, 0.001);
            double cur_weight = large_grad_percent * dist / exp(-(abs(rect.angle) + score[i]) * degree2rad_scale * exp_weight_scale);
            if(cur_weight < weight){
                weight = cur_weight;
                ret_idx = i;
                _is_small_armor = is_small;
                //cout << "red:" << avg_red_side/side_total << "\tblue: "  << avg_blue_side/side_total << endl;
                //imwrite("armor_org.bmp", rectify_target(armor_rect));
            }
#ifdef COUT_LOG
            else
                cout << "refused 4: cur_weight: " << cur_weight << "\tweight threshold:" << weight << endl;
#endif
        }
#ifdef COUT_LOG
        else
            cout << "refused 3: (x_grad, y_grad): (" << avg_x << ", " << avg_y << ")\t avg_grad_threshold: " <<  (int)_para.avg_board_grad_threshold << endl;
#endif
    }
    //return ret_idx == -1 ? RotatedRect() : rects[ret_idx];
    if (ret_idx == -1){
        _is_lost = true;
        return RotatedRect();
    }
    _is_lost = false;
    // broaden the height of target
    RotatedRect ret_rect = rects[ret_idx];
    Rect ret_rect1 = ret_rect.boundingRect();
    if (broadenRect(ret_rect1, 3, 3, _src.size()) == false)
        return ret_rect;
    //rectangle(_src, ret_rect1, CV_RGB(255,128,128),2);
    //imshow("_src", _src);

    Mat s_b = _ec(ret_rect1);
    threshold(s_b, s_b, 128, 255, CV_THRESH_OTSU);
    vector<vector<Point2i> > contours_br;
    vector<Vec4i> hierarchy;
    findContours(s_b, contours_br, hierarchy, CV_RETR_EXTERNAL , CV_CHAIN_APPROX_SIMPLE);
    for (int k = 0; k < contours_br.size(); ++ k){
        Rect r = boundingRect(contours_br[k]);
        if (r.height > ret_rect.size.height){
            ret_rect.size.height = r.height;
            if(ret_rect.size.width / ret_rect.size.height < small_armor_wh_threshold)
                _is_small_armor = true;
        }
    }
    return ret_rect;
}

RM装甲检测程序分析（二）