基于激光雷達的人體跟蹤(2)
3.1 人體腿部檢測算法
這里使用北洋公司生產的二維激光雷達HOKUYO UＲG-04LX-UG01。它的測量范圍是 5. 6 m，角度范圍是 240° (角度分辨率為0.36°)。本文使用 180° /0.36°的掃描模式，單次掃描可獲得 500 個測量數(shù)據(jù)，掃描時間為 100 ms。激光雷達水平安裝在距離地面 0.3 m 高機器人位置處。
本文通過檢測人腿來檢測人體，首先將運動區(qū)域中的激光數(shù)據(jù)點進行分段，若連續(xù)兩個點之間的距離小于某一個閾值(實驗中設為8cm)，則他們屬于同一段，通過激光掃描出來的人腿形狀為類圓弧形，大多數(shù)情況下激光能夠掃描到的圖像如圖2所示，圖2左邊表示激光在人的后面能夠掃描到人的兩條腿，右邊表示激光在人的側面時只能掃到一條腿。如圖3所示，由于人腿的特殊性，激光掃描出來的人腿形狀有一些幾何學上的特征，通過提取人腿形狀的一些有效的特征來識別人腿。文[1]使用了AdaBoost方法來訓練腿部數(shù)據(jù)特征，AdaBoost通過訓練正樣本(腿部的特征數(shù)據(jù))和負樣本(非腿部的特征數(shù)據(jù))能夠找到最優(yōu)的特征，但是AdaBoost分類器只能將正樣本和負樣本用直線分開，所以很難用更加細致的邊界曲線來區(qū)別正樣本和負樣本。而SVM就能用更加復雜的邊界曲線來更好地區(qū)分正樣本和負樣本。這里給出支持向量機講的比較好的幾個博客，也可參考西瓜書<<機器學習>>
https://www.cnblogs.com/steven-yang/p/5658362.html
https://blog.csdn.net/zx10212029/article/details/50198547
https://blog.csdn.net/xmdxcsj/article/details/51511346
由于人穿褲子時，褲子表面會隨時變化，所以激光掃描檢測到的人體腿部的形狀也不能被簡單地描述，為了分析人體腿部聚類數(shù)據(jù)的特征，需要使用大量的訓練數(shù)據(jù)。我們采集了五個人的數(shù)據(jù)，每個人都穿普通的非寬松的褲子，每個人都在距離激光1-5m的距離隨意走動，激光放在距離地面30cm高的位置上。首先在干擾物較少的環(huán)境中采集人體腿部數(shù)據(jù)，其次在普通辦公室環(huán)境下采集非腿部數(shù)據(jù)，一共采集到2215組聚類數(shù)據(jù)，其中包括腿部數(shù)據(jù)和一些室內環(huán)境中的非腿部數(shù)據(jù)，使用SVM來訓練這些數(shù)據(jù)。
#include "laser.h"
#include "ml.h"
QFile laser1File("laser1.txt");
QFile laser2File("laser2.txt");
bool laser1TxtRemoveFlag=true;
bool laser2TxtRemoveFlag=true;
double angle_laser;
double distance_car_laser;
struct coor{
double x;
double y;
int cluster;
};
struct curve{
double L;
double D;
};
struct ceter{
double x;
double y;
};
// initial position
//std_msgs::Int16 V_L;
// std_msgs::Int16 V_R;
//編碼器的值
double right_enc=0;
double left_enc=0;
double right_enc_old=0;
double left_enc_old=0;
double dtcout = 0;
void LaserCallback (const sensor_msgs::LaserScan::ConstPtr& msg){
// To get header data from sensor msg
// SensorMsg = *msg;
double px, py, pr, pt,dx,dy,R;
std::vector < double > laser_x;
std::vector < double > laser_y;
std::vector < double > laser_r;
std::vector < double > laser_t;
std::vector<coor> C;
std::vector<ceter> Ceter;
ceter temp1;
coor temp;
for( unsigned i = 0; i < msg->ranges.size(); i++ ){
pr = msg->ranges[ i ];
pt = msg->angle_min + ( i * msg->angle_increment);
laser_r.push_back( pr );
laser_t.push_back( pt*180/3.14 );
}
for( unsigned i = 0; i < msg->ranges.size(); i++ ){
pr = laser_r[ i ] * cos( laser_t[ i ]*3.14/180 );
pt = laser_r[ i ] * sin( laser_t[ i ]*3.14/180 );
//激光數(shù)據(jù)中有很多無效數(shù)據(jù)，為無限大(inf)或者非數(shù)字(nan)的值，通過isnormal()函數(shù)來排除這些數(shù)據(jù)，
//參考：http://blog.csdn.net/wokaowokaowokao12345/article/details/72846436
if(isnormal(pr)&&isnormal(pt))
{
laser_x.push_back( pr );
laser_y.push_back( pt );
}
}
//計算有效數(shù)據(jù)的個數(shù)
int laser_size=laser_x.size();
//類別至少為１
int sort=1;
int sort_max=1;
temp.x=laser_x[ 0 ];
temp.y=laser_y[ 0 ];
temp.cluster=sort;
C.push_back(temp);
//按類別把數(shù)據(jù)分類，兩點之間距離小于0.5米就歸為一類,然后不歸為一類的就再加上一類
for( unsigned i = 0; i <laser_size; i++ ){
unsigned ii=i,jj=i+1;
dx=laser_x[jj]-laser_x[ii];
dy=laser_y[jj]-laser_y[ii];
R=sqrt(dx*dx+dy*dy);
if(R<0.5)
{
temp.x=laser_x[jj];
temp.y=laser_y[jj];
temp.cluster=sort;
C.push_back(temp);
}
else
{
temp.x=laser_x[jj];
temp.y=laser_y[jj];
temp.cluster=sort+1;
sort=sort+1;
sort_max=sort;
C.push_back(temp);
}
}
double Lk,Dk,dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx_begin,dy_begin,dx_end,dy_end,curve;
vector<coor>::iterator it1;
for(int category=1;category<=sort_max;category++){
bool test1=true;
bool test2=true;
bool test3=true;
Lk=0;
//for(it1 = C.begin();it1 != C.end();it1++)
for(it1 = C.begin();it1 != C.end();it1++)
{
//if((*it1).cluster==category&&((abs((*it1).x-Xc))<=0.3)&&((abs((*it1).y-Yc))<=0.3))
if((*it1).cluster==category)
{
//找到一類的起始點
if(test1)
{
dx_begin=(*it1).x;
dy_begin=(*it1).y;
test1=false;
}
//找到一類的起始點，但是這里dx1,dy1只用一次，不能每次都用，
//否則不能實現(xiàn)依次兩個數(shù)據(jù)減的效果，后面再給dx1=dx2,dy1=dy2。
if(test3)
{
dx1=(*it1).x;
dy1=(*it1).y;
test3=false;
}
dx2=(*it1).x;
dy2=(*it1).y;
dx3=dx2-dx1;
dy3=dy2-dy1;
//計算每一類從頭到尾連起來的長度
Lk=Lk+sqrt(dx3*dx3+dy3*dy3);
vector<coor>::iterator it3;
for(it3=C.end();it3 != C.begin();it3--)
{
if((*it3).cluster==category)
{
if(test2)
{
dx_end=(*it3).x;
dy_end=(*it3).y;
test2=false;
}
}
}
dx4=dx_end-dx_begin;
dy4=dy_end-dy_begin;
//計算每一類第一個與最后一個的距離
Dk=sqrt(dx4*dx4+dy4*dy4);
dx1=dx2;
dy1=dy2;
}
int cc =1;
cc=2;
}
//計算兩者的比例
curve=Lk/Dk;
// 加上一些限定條件來找到腿部數(shù)據(jù)，也可以加上一些距離信息，這里還沒有加.
CvSVM *SVM;
SVM->load("/home/cyy/ros_car/carcontrol/SVM_DATA.xml");
float a[] = { Lk, curve};
CvMat sampleMat;
cvInitMatHeader(&sampleMat, 1, 2, CV_32FC1, a);
cvmSet(&sampleMat, 0, 0, Lk); // Set M(i,j)
cvmSet(&sampleMat, 0, 1,curve); // Set M(i,j)
float response = SVM->predict(&sampleMat);
if(response==1)
{
if (laser1TxtRemoveFlag)
{
QFile::remove("./laser1.txt");
laser1TxtRemoveFlag = false;
}
laser1File.open(QIODevice::WriteOnly | QIODevice::Append);
QTextStream writetxt123(&laser1File);
writetxt123 << QString::number(Lk) + " " + QString::number(Dk) + " " + QString::number(curve) + " " + QString::number(0) + " " << '\n';
laser1File.close();
double test_curve=curve;
for(it1=C.begin();it1 != C.end();it1++)
{
if((*it1).cluster==category&&((*it1).x<1.1)&&(abs((*it1).y)<0.5))
{
temp1.x=(*it1).x;
temp1.y=(*it1).y;
Ceter.push_back(temp1);
}
}
}
else
{
if (laser1TxtRemoveFlag)
{
QFile::remove("./laser1.txt");
laser1TxtRemoveFlag = false;
}
laser1File.open(QIODevice::WriteOnly | QIODevice::Append);
QTextStream writetxt123(&laser1File);
writetxt123 << QString::number(Lk) + " " + QString::number(Dk) + " " + QString::number(curve) + " " + QString::number(0) + " "<< '\n';
laser1File.close();
}
}
double Xc,Yc,Xc_1,Yc_1;
vector<ceter>::iterator it2;
for(it2=Ceter.begin();it2!= Ceter.end();it2++)
{
Xc_1=(*it2).x+Xc_1;
Yc_1=(*it2).y+Yc_1;
}
Xc=Xc_1/Ceter.size();
Yc=Yc_1/Ceter.size();
distance_car_laser=sqrt(Xc*Xc+Yc*Yc);
angle_laser=atan(Yc/Xc);
double test_Yc=Yc;
if (laser2TxtRemoveFlag)
{
QFile::remove("./laser2.txt");
laser2TxtRemoveFlag = false;
}
laser2File.open(QIODevice::WriteOnly | QIODevice::Append);
QTextStream writetxt123(&laser2File);
writetxt123 << QString::number(distance_car_laser) + " " + QString::number(angle_laser) + " " << '\n';
laser2File.close();
}
laser::laser()
{
}
void laser::run()
{
while (true) //收到Ctrl+C 后停止
{
ros::NodeHandle nh;
// ros::Rate loop_rate(125); //發(fā)送速率為20hz
ros::Subscriber node_sub = nh.subscribe("/scan", 2, LaserCallback);
ros::spin();
// loop_rate.sleep(); //按照20hz的頻率掛起
}
}
