数字图像处理（DIP）大作业 基于OpenCV和ros的人腿跟踪

数字图像处理（DIP）大作业 基于OpenCV和ros的人腿跟踪

数字图像处理课程相关文章 传送门

https://blog.csdn.net/qq_46164507/article/details/122503851

博文说明

本文所使用代码或多或少参考了以往博文的同类or相似文章的代码，并非纯原创
本文仅用于记录并提供一种代码思路，供大家参考

文章正文

实验要求

我们组选择的是第二个项目：人腿识别跟踪

解决思路以及算法介绍

• 1、 采用k-means算法，实现提取（切换）图像主色（即人腿特征颜色）功能
  原理介绍：在RGB域中设置固定数目的聚类中心，经过k-means聚类，实现色域分割，并提取当前图像的颜色最多的区域，认为是人的腿部特征（裤子颜色）。
• 2、采用形态学处理的算法，实现找到固定HSV/RGB范围颜色的色域中最大连通域的中心及宽度功能
  原理介绍：gamma变换（增亮图像）-高斯滤波（滤除图像坏点、平滑图像）-HSV/RGB提取（提取固定颜色范围的图像）-二值化图像-开运算（去除小块噪声、打断连通域）-提取最大连通域（找到目标人腿位置）-绘制最小外接矩形（提取人腿相对于相机的位置特征（距离、左右位置））。
• 3、设计非线性控制器，实现让小车跟踪人腿的运动控制功能
  原理介绍：以最小外接矩形的宽度（代表距离信息）和中心点水平位置（代表角度信息）为控制量，分别控制机器人运动的线速度和角速度分量，实现精确的运动控制。
• 4、设计键盘检测程序，实现半自动切换跟踪目标功能
  原理介绍：用户在进程内敲击键盘任意键，程序会自动切换运行模式（目标检测模式-目标跟踪模式）。

实验代码

运行环境：Ubuntu16.04 LTS + OpenCV 3.0.4 + ROS-kinetic-full

代码语言：c++

#include <stdlib.h>
#include <iostream>
#include <algorithm>
#include <string>
#include <cv.h>
#include <highgui.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include "ros/ros.h"
#include <termios.h>
#include <boost/thread.hpp>
#include <geometry_msgs/Twist.h>
#include <math.h>

using namespace cv;
using namespace std;

// PID控制器所用参数
// double static errorsum=0;
// double static error1=0;
// double static error2=0;

// 键盘检测所用
static struct termios initial_settings, new_settings;  
static int peek_character = -1;
void init_keyboard()  
{  
    tcgetattr(0,&initial_settings);  
    new_settings = initial_settings;  
    new_settings.c_lflag &= ~ICANON;  
    new_settings.c_lflag &= ~ECHO;  
    new_settings.c_lflag &= ~ISIG;  
    new_settings.c_cc[VMIN] = 1;  
    new_settings.c_cc[VTIME] = 0;  
    tcsetattr(0, TCSANOW, &new_settings);  
}  
void close_keyboard()  
{  
    tcsetattr(0, TCSANOW, &initial_settings);  
}  
int kbhit()  
{  
    char ch;  
    int nread;  
    if(peek_character != -1)  
        return 1;  
    new_settings.c_cc[VMIN]=0;  
    tcsetattr(0, TCSANOW, &new_settings);  
    nread = read(0,&ch,1);  
    new_settings.c_cc[VMIN]=1;  
    tcsetattr(0, TCSANOW, &new_settings);  
    if(nread == 1) {  
        peek_character = ch;  
        return 1;  
    }  
    return 0;  
}

// Gamma矫正
void GammaCorrection(Mat src, Mat &dst, float fGamma)
{
    // build look up table
    unsigned char lut[256];
    for( int i = 0; i < 256; i++ )
    {
        lut[i] = saturate_cast<uchar>(pow((float)(i/255.0), fGamma) * 255.0f);//防止颜色溢出操作
    }
    dst = src.clone();
    const int channels = dst.channels();
    switch(channels)
    {
        case 1:
        {
            MatIterator_<uchar> it, end;
            for( it = dst.begin<uchar>(), end = dst.end<uchar>(); it != end; it++ )
                *it = lut[(*it)];
            break;
        }
        case 3: 
        {
            MatIterator_<Vec3b> it, end;
            for( it = dst.begin<Vec3b>(), end = dst.end<Vec3b>(); it != end; it++ )
            {
                (*it)[0] = lut[((*it)[0])];
                (*it)[1] = lut[((*it)[1])];
                (*it)[2] = lut[((*it)[2])];
            }
            break;
        }
    }
}

Mat DoKmeans(Mat src)
{
    //Gamma变换内置
    Mat gammad = src.clone();
    GammaCorrection(src,gammad,1/1.6);

    //获取源图像的宽，高和图像的通道数
    //函数里的,与外面的不同
    int width = src.cols;
    int height = src.rows;
    int dims = src.channels();

    //定义初始值
    int sampleCount = width * height;
    int clusterCount = 8;
    cv::Mat Points(sampleCount,dims,CV_32F,cv::Scalar(10));
    cv::Mat labels;
    cv::Mat centers(clusterCount,1,Points.type());

    //RGB数据转换为样本数据
    for (int row= 0;row<height;row++) {
        for (int col = 0;col <width; col++) {
            int index = row * width + col;
            cv::Vec3b bgr =gammad.at<cv::Vec3b>(row,col);
            Points.at<float>(index,0)=static_cast<int>(bgr[0]);
            Points.at<float>(index,1)=static_cast<int>(bgr[1]);
            Points.at<float>(index,2)=static_cast<int>(bgr[2]);
        }
    }

    //运行KMeans
    cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT,10,0.1);
    cv::kmeans(Points, clusterCount,labels, criteria,3,cv::KMEANS_PP_CENTERS,centers);

    //将分割结果重新绘制到新的Mat里
    int labelamount[clusterCount]={0};
    cv::Mat result =cv::Mat::zeros(src.size(),src.type());
    for (int row = 0;row < height;row++) {
        for (int col = 0; col < width; col++) {
            int index =row * width + col;
            int label = labels.at<int>(index,0);
            labelamount[label]=labelamount[label]+1;
            result. at<cv::Vec3b>(row,col)[0] = centers.at<float>(label,0);
            result. at<cv::Vec3b>(row,col)[1] = centers.at<float>(label,1);
            result. at<cv::Vec3b>(row,col)[2] = centers.at<float>(label,2);
        }
    }

    //统计label数目
    int maxlabeli = 0;
    int maxlabelamount = 0;
    for (int i = 0; i < clusterCount; i++)
    {
        if (labelamount[i] > maxlabelamount)
        {
            maxlabeli = i;
            maxlabelamount = labelamount[i];
        }
    }

    //将最多label区域绘制成绿色
    //求在最大簇内的label
    for (int row = 0;row < height;row++) {
        for (int col = 0; col < width; col++) {
            int index =row * width + col;
            if (labels.at<int>(index,0) == maxlabeli)
            {
                result.at<cv::Vec3b>(row,col)[0] = 0;
                result.at<cv::Vec3b>(row,col)[1] = 255;
                result.at<cv::Vec3b>(row,col)[2] = 0;
            }
        }
    }

    //求均值
    cv::Mat RGBSum =cv::Mat::zeros(1,dims,CV_32F);
    for (int row = 0;row < height;row++) {
        for (int col = 0; col < width; col++) {
            int index =row * width + col;
            if (labels.at<int>(index,0) == maxlabeli)
            {
                RGBSum.at<float>(0,0) += Points.at<float>(index,0);
                RGBSum.at<float>(0,1) += Points.at<float>(index,1);
                RGBSum.at<float>(0,2) += Points.at<float>(index,2);
            }
        }
    }
    Mat RGBAverage = RGBSum / maxlabelamount;

    //求方差
    cv::Mat RGBSigma2 =cv::Mat::zeros(1,dims,CV_32F);
    for (int row = 0;row < height;row++) {
        for (int col = 0; col < width; col++) {
            int index =row * width + col;
            if (labels.at<int>(index,0) == maxlabeli)
            {
                RGBSigma2.at<float>(0,0) += pow((Points.at<float>(index,0)-RGBAverage.at<float>(0,0)),2);
                RGBSigma2.at<float>(0,1) += pow((Points.at<float>(index,1)-RGBAverage.at<float>(0,1)),2);
                RGBSigma2.at<float>(0,2) += pow((Points.at<float>(index,2)-RGBAverage.at<float>(0,2)),2);
            }
        }
    }
    cv::Mat RGBSigma(1,dims,CV_32F,cv::Scalar(10));
    RGBSigma.at<float>(0,0) = sqrt((RGBSigma2.at<float>(0,0) / maxlabelamount));
    RGBSigma.at<float>(0,1) = sqrt((RGBSigma2.at<float>(0,1) / maxlabelamount));
    RGBSigma.at<float>(0,2) = sqrt((RGBSigma2.at<float>(0,2) / maxlabelamount));

    //4sigma原则,囊括
    cv::Mat maxlabelRGB(1,dims*3,CV_32F,cv::Scalar(10));
    maxlabelRGB.at<float>(0,0) = RGBAverage.at<float>(0,0);//中心R
    maxlabelRGB.at<float>(0,1) = RGBAverage.at<float>(0,1);//中心G
    maxlabelRGB.at<float>(0,2) = RGBAverage.at<float>(0,2);//中心B
    maxlabelRGB.at<float>(0,3) = RGBAverage.at<float>(0,0) - 4 * RGBSigma.at<float>(0,0);//最小R
    maxlabelRGB.at<float>(0,4) = RGBAverage.at<float>(0,1) - 4 * RGBSigma.at<float>(0,1);//最小G
    maxlabelRGB.at<float>(0,5) = RGBAverage.at<float>(0,2) - 4 * RGBSigma.at<float>(0,2);//最小B
    maxlabelRGB.at<float>(0,6) = RGBAverage.at<float>(0,0) + 4 * RGBSigma.at<float>(0,0);//最大R
    maxlabelRGB.at<float>(0,7) = RGBAverage.at<float>(0,1) + 4 * RGBSigma.at<float>(0,1);//最大G
    maxlabelRGB.at<float>(0,8) = RGBAverage.at<float>(0,2) + 4 * RGBSigma.at<float>(0,2);//最大B

    //显示最终结果图像
    cv::imshow("Kmeans_Result",result);
    cv::waitKey(1);

    return maxlabelRGB;
}

// 旧版
// void Separate(Mat src,Mat &dst)
// {   
//     GammaCorrection(src,src,1/1.6);//伽马变换
//     GaussianBlur(src,src,Size(7,7),7,7);//高斯模糊
    
//     //转换到HSV色度空间
//     Mat hsv;
//     cvtColor(src,hsv,COLOR_BGR2HSV);
//     //提取黑色
//     inRange(hsv, Scalar(0,0, 0),Scalar(180,255, 40), dst);//颜色分割
//     // //开运算
//     Mat element=getStructuringElement(MORPH_RECT, Size(15,15));
//     morphologyEx(dst, dst, MORPH_OPEN, element);
// }

void RGBSeparate(Mat src,Mat &dst,Mat maxlabelRGB)
{   
    // 伽马矫正放到函数里面
    GammaCorrection(src,src,1/1.6);//伽马变换
    //提取目标颜色
    inRange(src, Scalar(maxlabelRGB.at<float>(0,3),maxlabelRGB.at<float>(0,4),maxlabelRGB.at<float>(0,5)),Scalar(maxlabelRGB.at<float>(0,6),maxlabelRGB.at<float>(0,7),maxlabelRGB.at<float>(0,8)), dst);//颜色分割
    //开运算
    Mat element=getStructuringElement(MORPH_RECT, Size(20,20));//15,15
    morphologyEx(dst, dst, MORPH_OPEN, element);
}

void LargestConnectedComponent(Mat srcImage, Mat &dstImage)
{
    Mat temp;
    Mat labels;
    srcImage.copyTo(temp);

    //1. 标记连通域
    int n_comps = connectedComponents(temp, labels, 4, CV_16U);
    vector<int> histogram_of_labels;
    for (int i = 0; i < n_comps; i++)//初始化labels的个数为0
    {
        histogram_of_labels.push_back(0);
    }

    int rows = labels.rows;
    int cols = labels.cols;
    for (int row = 0; row < rows; row++) //计算每个labels的个数
    {
        for (int col = 0; col < cols; col++)
        {
            histogram_of_labels.at(labels.at<unsigned short>(row, col)) += 1;
        }
    }
    histogram_of_labels.at(0) = 0; //将背景的labels个数设置为0

    //2. 计算最大的连通域labels索引
    int maximum = 0;
    int max_idx = 0;
    for (int i = 0; i < n_comps; i++)
    {
        if (histogram_of_labels.at(i) > maximum)
        {
            maximum = histogram_of_labels.at(i);
            max_idx = i;
        }
    }

    //3. 将最大连通域标记为1
    for (int row = 0; row < rows; row++) 
    {
        for (int col = 0; col < cols; col++)
        {
            if (labels.at<unsigned short>(row, col) == max_idx)
            {
                labels.at<unsigned short>(row, col) = 255;
            }
            else
            {
                labels.at<unsigned short>(row, col) = 0;
            }
        }
    }

    //4. 将图像更改为CV_8U格式
    labels.convertTo(dstImage, CV_8U);
}

// double pidController(double targetVelocity, double currentVelocity)
// {
//     double kp=0.8;
//     double ki=0;
//     double kd=0.4;
//     double T=1;
//     double a0=kp+ki*T+kd/T;
//     double a1=kp+2*kd/T;
//     double a2=kd/T;

//     double e= targetVelocity - currentVelocity;
//     double u1 =currentVelocity+a0*e-a1*error2+a2*error1;
    
//     error1=error2;
//     error2=e;
//     // errorsum+=ek10;
//     return u1;
// }

double * Getcenter(Mat src,double SPEED[2],Mat maxlabelRGB)
{
    Mat aimcolor=src.clone();
    RGBSeparate(src,aimcolor,maxlabelRGB);
    imshow("Color_Search",aimcolor);
    Mat maxearea=src.clone();
    LargestConnectedComponent(aimcolor,maxearea);
    
    double static speed[2];
    for(int i=0;i<2;i++){speed[i]=0;}
    Mat resultImage=Mat::zeros(src.rows, src.cols, CV_8UC1);;
    vector<vector<Point> > contours;
    vector<Vec4i> hierarcy;
    findContours(maxearea, contours, hierarcy, RETR_EXTERNAL, CHAIN_APPROX_NONE);
    vector<RotatedRect> box(contours.size()); //最小外接矩形
    Point2f rect[4];
    float width = 0;//外接矩形的宽和高
    float height = 0;
    float ratio = 0;  //存储长宽比=width/heigth
    Point2f center;
    for (int i = 0; i < contours.size(); i++)
    {
        box[i] = minAreaRect(Mat(contours[i]));
        box[i].points(rect);          //最小外接矩形的4个端点
        width = box[i].size.width; 
        height = box[i].size.height;
        if (height >= width)
        {
            float x = 0;
            x = height;
            height = width;
            width = x;
        }
        ratio = width / height;
        center=(rect[1]+rect[3])/2;
        cout << "宽" << width << " " << "高" << height << "长宽比"<<ratio<<"中心"<<center<<endl;
        for (int j = 0; j < 4; j++)
        {
            line(maxearea, rect[j], rect[(j + 1) % 4], Scalar(255,255,255), 3, 25);//绘制最小外接矩形的每条边
            line(src, rect[j], rect[(j + 1) % 4], Scalar(50,155,0), 3, 25);//绘制最小外接矩形的每条边
            // 50,155,0
        }
    }

    imshow("Largest_Connected_Component",maxearea);
    imshow("Result_Image",src);

    double s1=4;double s2=0.03;
    // double s1=3;double s2=0.02;//三次方
    double yz1=300;
    double yz2=365;
    double yz3=650;
    if(width<yz1)
    {
        double linearspeed=s1*(yz1-width)/yz1;
        speed[0]=linearspeed;
        ROS_WARN("*****GOAHEAD*****");
    }
    else if (width>yz2 && width<yz3)
    {
        double linearspeed=s1*(yz2-width)/yz2;
        speed[0]=linearspeed;
        ROS_WARN("*****BACKFORWARD*****");
    }
    else
    {
        double linearspeed=0;
        speed[0]=linearspeed;
        ROS_WARN("*****STOP*****");
    }
    if(center.x-src.cols/2<-25){
        cout<<"偏差为"<<center.x-src.cols/2;
        
        double anglespeed=-s2*(pow(center.x-src.cols/2,3)/pow(40,3)-1);
        // anglespeed=pidController(SPEED[1],anglespeed);
        // speed[1]=min(anglespeed,0.6);
		speed[1]=anglespeed;
        ROS_WARN("*****LEFT*****");
    }
    else if(center.x-src.cols/2>+25){
        cout<<"偏差为"<<center.x-src.cols/2;
        double anglespeed=-s2*(pow(center.x-src.cols/2,3)/pow(40,3)-1);
        // anglespeed=pidController(SPEED[1],anglespeed);
        // anglespeed=-s2*(pow(center.x-src.cols/2,3)/pow(40,3)-1);
        // speed[1]=max(anglespeed,-0.6);
		speed[1]=anglespeed;
        ROS_WARN("*****RIGHT*****");
    }
    return speed;
}

int main(int argc,char **argv)
{
    VideoCapture capture;
    capture.open(1);//0为笔记本摄像头,1为机器人摄像头
    ROS_WARN("*****START*****");
    ros::init(argc,argv,"ColorMove");//初始化 ROS 节点
    ros::NodeHandle n;
    ros::Publisher cmd_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 5);//定义速度发布器
    geometry_msgs::Twist twist;
    if(!capture.isOpened())
    {
        printf("the camera can't work normally");
        return 0;
    }
    
    Mat src;
    int mode = 0;//0为识别颜色模式,1为跟踪模式
    cv::Mat maxlabelRGB(1,9,CV_32F,cv::Scalar(10));

    init_keyboard();  
    
    while(1){

        capture.read(src);

        //仅使用一次
        int rows = src.rows;
        int cols = src.cols;
        //截取左半平面
        Rect select=Rect(0,0,cols/2,rows);
        src=src(select);
        if(src.empty()) 
        { 
            break; 
        }

        //绘图外置
        imshow("Initial_Image",src);

        if (mode == 0)
        {
            //求得目标颜色区域
            maxlabelRGB = DoKmeans(src);

            //识别键盘输入
            if(kbhit())
            {
                peek_character = -1;
                //切换模式
                mode = 1;
                //输出消息
                cout << "已更新目标" << "\n";
                cout << "目标颜色R值为" << maxlabelRGB.at<float>(0,0) << "\n";
                cout << "目标颜色G值为" << maxlabelRGB.at<float>(0,1) << "\n";
                cout << "目标颜色B值为" << maxlabelRGB.at<float>(0,2) << "\n";
                cout << "开始跟踪" << "\n";
                //关闭窗口
                destroyWindow("Kmeans_Result");
            }
        }
        else if (mode == 1)
        {
            double *speed;
            double s1[2];
            // s1[0]=twist.linear.x;
            // s1[1]=twist.angular.z;
            speed=Getcenter(src,s1,maxlabelRGB);

            //发布速度
            twist.linear.x = speed[0];//线速度
            twist.linear.y = 0;
            twist.linear.z = 0;
            twist.angular.x = 0;
            twist.angular.y = 0;
            twist.angular.z = speed[1];//角速度
            cout<<"twist.linear.x\t"<<twist.linear.x<<"\ttwist.angular.z\t"<<twist.angular.z<<endl;
            cmd_pub.publish(twist);//发布消息
            ROS_WARN("****twist****");

            if(kbhit())
            {
                peek_character = -1;
                //停车
                twist.linear.x = 0;//线速度
                twist.linear.y = 0;
                twist.linear.z = 0;
                twist.angular.x = 0;
                twist.angular.y = 0;
                twist.angular.z = 0;//角速度
                cmd_pub.publish(twist);//发布消息
                //切换模式
                mode = 0;
                //输出消息
                cout << "开始识别新目标" << "\n";
                //关闭窗口
                destroyWindow("Largest_Connected_Component");
                destroyWindow("Result_Image");
                destroyWindow("Color_Search");
            }
        }
        //ros::spinOnce();
        waitKey(1);
    }
    return 0;
}

实验结果

本课程设计表现出了良好的识别和跟踪性能。在目标人腿颜色为黑色的实验条件下，可以有效去除黑色杆状障碍物的干扰，跟踪及时、迅速、流畅。
绕桩视频如下：
https://www.bilibili.com/video/BV1Qq4y1z7Sn/
演示视频二维码：

本课程设计对纯色目标是通用的，不仅适用于黑色和人腿，在目标颜色为其它纯色的实验条件下也表现出很好的性能。