均值漂移算法C++

自己写的 还没测试 记录一下

//均值漂移算法
void cal_shift(vector>& point_class_times, vector& used_point_idx, vector& cur_class_idx, vector& cur_class_point, vector point_vec, Point2f center_point, Point2f& next_point, Point2f& shift_point, int radius, int class_idx)
{
    vector cur_point_vec;
    for (size_t i_nums = 0; i_nums < point_vec.size(); i_nums++)
    {
        float dis = sqrt((center_point.x - point_vec[i_nums].x) * (center_point.x - point_vec[i_nums].x) + (center_point.y - point_vec[i_nums].y) * (center_point.y - point_vec[i_nums].y));
        if (dis < radius)
        {
            cur_point_vec.push_back(point_vec[i_nums]);
            point_class_times[i_nums][class_idx].y = point_class_times[i_nums][class_idx].y + 1;
            vector::iterator result = find(used_point_idx.begin(), used_point_idx.end(), i_nums);
            if (result == used_point_idx.end())
            {
                used_point_idx.push_back(i_nums);
            }
            vector::iterator result_class = find(cur_class_idx.begin(), cur_class_idx.end(), i_nums);
            if (result_class == cur_class_idx.end())
            {
                cur_class_idx.push_back(i_nums);
                cur_class_point.push_back(point_vec[i_nums]);
            }
        }
    }
    for (size_t i_nums = 0; i_nums < cur_point_vec.size(); i_nums++)
    {
        Point2f cur_vec;
        cur_vec.x = cur_point_vec[i_nums].x - center_point.x;
        cur_vec.y = cur_point_vec[i_nums].y - center_point.y;
        shift_point.x = shift_point.x + cur_vec.x;
        shift_point.y = shift_point.y + cur_vec.y;
    }
    shift_point.x = shift_point.x / float(cur_point_vec.size());
    shift_point.y = shift_point.y / float(cur_point_vec.size());
    next_point.x = center_point.x + shift_point.x;
    next_point.y = center_point.y + shift_point.y;
}

void Mean_shift(vector point_vec)
{
    srand((int)time(0));

    vector check_point_vec = point_vec;

    vector> result_point;
    vector> point_class_times;
    point_class_times.resize(point_vec.size());

    int radius = 5;
    int class_idx = 0;

    vector used_point_idx;

    vector> class_point_vec;
    vector> class_point_idx_vec;

    bool last_times_insert = false;
    vector class_mean_vec;
    while (true)
    {
        int rand_first = rand() % check_point_vec.size();
        Point2f first_point = check_point_vec[rand_first];
        check_point_vec.clear();
        Point2f next_point;
        Point2f shift_point(0, 0);
        for (size_t i_nums = 0; i_nums < point_class_times.size(); i_nums++)
        {
            point_class_times[i_nums].push_back(Point2i(class_idx, 0));
        }

        vector cur_class_idx;
        vector cur_class_point;
        while (true)
        {
            cal_shift(point_class_times, used_point_idx, cur_class_idx, cur_class_point, point_vec, first_point, next_point, shift_point, radius, class_idx);
            if (point_dis(first_point, next_point) < 1)
            {
                break;
            }
            first_point = next_point;
            next_point.x = 0;
            next_point.y = 0;
        }

        Point2f cur_class_mean;
        Point2f cur_class_sum;
        for (size_t i_nums = 0; i_nums < cur_class_point.size(); i_nums++)
        {
            cur_class_sum.x += cur_class_point[i_nums].x;
            cur_class_sum.y += cur_class_point[i_nums].y;
        }

        cur_class_mean.x = cur_class_sum.x / float(cur_class_point.size());
        cur_class_mean.y = cur_class_sum.y / float(cur_class_point.size());

        if (0 == class_mean_vec.size())
        {
            class_mean_vec.push_back(cur_class_mean);
            class_point_vec.push_back(cur_class_point);
            class_point_idx_vec.push_back(cur_class_idx);
            class_idx++;
        }
        else
        {
            bool chk_insert = false;
            for (size_t i_nums = 0; i_nums < class_mean_vec.size(); i_nums++)
            {
                float dis = point_dis(cur_class_mean, class_mean_vec[i_nums]);
                if (dis < 3)
                {
                    chk_insert = true;
                    for (size_t j_nums = 0; j_nums < cur_class_idx.size(); j_nums++)
                    {
                        point_class_times[cur_class_idx[j_nums]][i_nums].y = point_class_times[cur_class_idx[j_nums]][i_nums].y + 1;                        
                        vector::iterator result = find(class_point_idx_vec[i_nums].begin(), class_point_idx_vec[i_nums].end(), cur_class_idx[j_nums]);
                        if (result == class_point_idx_vec[i_nums].end())
                        {
                            class_point_vec[i_nums].push_back(point_vec[cur_class_idx[j_nums]]);
                        }
                    }
                    Point2f new_class_mean;
                    Point2f new_class_sum;
                    for (size_t j_nums = 0; j_nums < class_point_vec[i_nums].size(); j_nums++)
                    {
                        new_class_sum.x += class_point_vec[i_nums][j_nums].x;
                        new_class_sum.y += class_point_vec[i_nums][j_nums].y;
                    }
                    new_class_mean.x = new_class_sum.x / (float(class_point_vec[i_nums].size()));
                    new_class_mean.y = new_class_sum.y / (float(class_point_vec[i_nums].size()));
                    class_mean_vec[i_nums] = new_class_mean;
                    break;
                }
            }
            if (false == chk_insert)
            {
                class_mean_vec.push_back(cur_class_mean);
                class_point_vec.push_back(cur_class_point);
                class_point_idx_vec.push_back(cur_class_idx);
                class_idx++;
            }
        }

        class_point_vec.push_back(cur_class_point);
        for (size_t i_nums = 0; i_nums < point_vec.size(); i_nums++)
        {
            vector::iterator result = find(used_point_idx.begin(), used_point_idx.end(), i_nums);
            if (result == used_point_idx.end())
            {
                check_point_vec.push_back(point_vec[i_nums]);
            }
        }

        if (used_point_idx.size() == point_vec.size())
        {
            break;
        }
    }

}