first push

xuepengqiang

2019-12-26 025429cbd5dd3dc6f130b8de07664665b0c4b55a

first push

 03_runing.cpp

New file
@@ -0,0 +1,42 @@
// 03_runing.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//
#include <iostream>
#include "human_runing.h"
using namespace std;

int main()
{
    int rps = 10;
    int human_id = 1;
    //box的两个点坐标
    int x1 = 100;
    int y1 = 100;
    int x2 = 150;
    int y2 = 300;
    //摄像机标识 
    char cam_id = 'A';
    // 每一个相机都需要常见要给检测类，并且有对应的相机参数文件
    Human_Run CamA_HR(cam_id); //初始化摄像头信息
    cout << "Asd" << endl;
    double human_velocity;
    for (int i = 0; i < 40; ++i)
    {
        human_velocity = CamA_HR.get_human_velocity(rps, human_id, x1, y1, x2, y2);
        cout << "\nhuman_velocity::" <<human_velocity << endl;
        if(i<20)
        {
            x1++;
            x2++;
        }else{
            x1--;
            x2++;
        }
    }
//    Human_Run human(human_id,  x1, y1, x2, y2);


//    std::cout << human_A;
//    cout<<"Area="<<c.Area()<<endl;
    return 1;
}


 README.md

@@ -2,3 +2,6 @@

奔跑算法

mian方法在03_runing.cpp下

初始化需要相机信息的配置文件

 camera_config.cpp

New file
@@ -0,0 +1,50 @@
#include "camera_config.h"



CameraInfo::CameraInfo()
{
    this->bottom_distance = 295;
    int p1x = 587, p1y = 396;
    int p2x = 309, p2y = 402;
    int p3x = 378, p3y = 190;
    int p4x = 522, p4y = 181;


    vector<int> vec1;
    vector<int> vec2;
    vector<int> vec3;
    vector<int> vec4;
    vec1.push_back(p1x);
    vec1.push_back(p1y);
    vec2.push_back(p2x);
    vec2.push_back(p2y);
    vec3.push_back(p3x);
    vec3.push_back(p3y);
    vec4.push_back(p4x);
    vec4.push_back(p4y);

    this->vec_ray.push_back(vec1);
    this->vec_ray.push_back(vec2);
    this->vec_ray.push_back(vec3);
    this->vec_ray.push_back(vec4);

}

vector<vector<int>> CameraInfo::get_vec_ray()
{
    return this->vec_ray;
}

int get_BASE_DISTANCE()
{
    return BASE_LENGTH;
}
int get_BASE_LENGTH()
{
    return BASE_DISTANCE;
}
int get_ANGLE()
{
    return ANGLE;
}

 camera_config.h

New file
@@ -0,0 +1,24 @@
#ifndef CAMERA_CONFIG_H
#define CAMERA_CONFIG_H
#include<iostream>
#include <vector>
const int BASE_DISTANCE = 610; 
const int BASE_LENGTH = 300; 
const int ANGLE = 10; 

using namespace std;
class CameraInfo
{
public:
    int bottom_distance;
    vector<vector<int>> vec_ray;

public:
    CameraInfo();
    vector<vector<int>> get_vec_ray();
    int get_BASE_DISTANCE();
    int get_BASE_LENGTH();
    int get_ANGLE();

};
#endif

 human_runing.cpp

New file
@@ -0,0 +1,64 @@
#include "human_runing.h"
#include "linear.h"
#include <iostream>
#include "vector_utils.h"
#include "human_velocity.h"


Human_Run::Human_Run(char cam_id)
{
    this->cam_id = cam_id;
//    cout << this->cam_id << endl;

    CameraInfo cf;
    this->cam_vec_ray = cf.get_vec_ray();
    this->base_distance = BASE_DISTANCE;
    this->base_length = BASE_LENGTH;
    this->angle = ANGLE;
    this->bottom_distance = cf.bottom_distance;


    //    for(int i=0;i< this->cam_vec_ray.size();i++)
    //    {
    //        std::cout << cam_vec_ray[i].size() << endl;
    //        for (int j = 0; j < this->cam_vec_ray[i].size() ; j++) {
    //            cout << this->cam_vec_ray[i][j]<<endl;
    //        }
    //    }

}


double Human_Run::get_human_velocity(int rps, int human_id, int x1, int y1, int x2, int y2)
{
    int center_pointx = (int)(x1+x2)/2;
    int center_pointy = (int)(y2-(y2-y1)/10);
    cout << "runing" << endl;

    Linear linear(this->cam_vec_ray, center_pointx, center_pointy);
    double center_distance = linear.cal_distance();

//    cout << typeid(center_distance).name() << endl;
//    cout << center_distance << endl;

    double deep_distance = linear.adjust_distance(this->base_distance,this->bottom_distance, center_distance,this->angle);
    printf("deep_distance::%f",deep_distance);


    VectorUtils VU;
    if(VU.is_element_in_vector(this->human_vec,human_id))
    {
        
        double frame_presrate = center_distance/this->bottom_distance;
        this->human_velocity_map[human_id].add_next_point(center_pointx, center_pointy, deep_distance, frame_presrate);
    } else{
        this->human_vec.push_back(human_id);
        HumanVelocity HV(this->bottom_distance, this->base_distance);
        this->human_velocity_map[human_id] = HV;
    }
//    printf("%d",this->human_velocity_map[human_id]);
    double human_velocity = this->human_velocity_map[human_id].cal_human_velocity(rps);
    return human_velocity;

}


 human_runing.h

New file
@@ -0,0 +1,26 @@
#ifndef HUMAN_RUNING_H
#define HUMAN_RUNING_H
#include <iostream>
#include "camera_config.h"
#include "human_velocity.h"
#include <map>
using namespace std;
class Human_Run{
    public:
        char cam_id; //相机编号
        vector<vector<int>> cam_vec_ray; //区域信息
        int base_distance; //相机基线距离
        int base_length; //相机基线长度
        int angle; //相机角度
        int frame_presrate;
        int bottom_distance;

        vector<int> human_vec;
        map<int, HumanVelocity> human_velocity_map;

    public:
        Human_Run(char cam_id);
        double get_human_velocity(int rps,int human_id, int x1, int y1, int x2, int y2);

};
#endif

 human_velocity.cpp

New file
@@ -0,0 +1,59 @@
#include "human_velocity.h"
#include <iomanip>
#include "math_utils.h"
HumanVelocity::HumanVelocity()
{
    printf("34343333333333");
}
HumanVelocity::HumanVelocity(int bottom_distance, int base_distance)
{
    this->MAX_SIZE = 10;
    for (int i = 0; i <this->MAX_SIZE ; ++i) {
        this->coords_structure.push_back({0,0,0});
        this->move_dist.push_back(0);
    }
    this->RPS = 25;
    this->total_distance = 0.0;
    this->store_pointer = 0;
    this->init_size = 0;
    this->real_world_rate = base_distance/bottom_distance;
    MathUtils mu;
    this->math_utils = mu;

}

void HumanVelocity::add_next_point(int point_x, int point_y, double point_z, double pers_rate)
{
    printf("add_next_point");
    this->coords_structure[this->store_pointer][0] = point_x;
    this->coords_structure[this->store_pointer][1] = point_y;
    this->coords_structure[this->store_pointer][2] = point_z; 
    this->store_pointer = (this->store_pointer + 1)%this->MAX_SIZE;
    this->init_size ++;
    if(this->init_size>1)
    {
//        printf("coords::::", this->coords_structure[(this->store_pointer -2 + this->MAX_SIZE)%this->MAX_SIZE]);
        double pers_distance = this->math_utils.cal_distance(this->coords_structure[(this->store_pointer -2 + this->MAX_SIZE)%this->MAX_SIZE],
                this->coords_structure[(this->store_pointer -1 + this->MAX_SIZE)%this->MAX_SIZE]);
        pers_distance = (pers_distance / pers_rate)*this->real_world_rate*0.1;

        double move_dist = sqrt(pow(pers_distance,2) + pow(
                                                           this->coords_structure[(this->store_pointer - 2 + this->MAX_SIZE) % this->MAX_SIZE][2] -
                                                           this->coords_structure[(this->store_pointer - 1 + this->MAX_SIZE) % this->MAX_SIZE][2],2));
        move_dist *= (this->coords_structure[this->store_pointer][2]/10);  

        this->total_distance += move_dist;
        this->move_dist[this->store_pointer-1] = move_dist;
    }

    this->total_distance -= this->move_dist[this->store_pointer];
}


double HumanVelocity::cal_human_velocity(int rps)
{
    this->RPS = rps;
    double h_velocity = this->total_distance*this->RPS/(this->MAX_SIZE - 2);

    return h_velocity/100;
}

 human_velocity.h

New file
@@ -0,0 +1,26 @@
#ifndef HUMAN_VELOCITY_H
#define HUMAN_VELOCITY_H
#include <vector>
#include "math_utils.h"
using namespace std;
class HumanVelocity
{
public:
    int MAX_SIZE;
    vector<vector<int>> coords_structure;
    vector<float> move_dist;
    int RPS;
    double total_distance;
    int store_pointer;
    int init_size;
    double real_world_rate;
    MathUtils math_utils;

public:
    HumanVelocity();
    HumanVelocity(int bottom_distance, int base_distance);
    void add_next_point(int point_x, int point_y, double point_z, double frame_presrate);
    double cal_human_velocity(int rps);
};

#endif

 linear.cpp

New file
@@ -0,0 +1,123 @@
#include <math.h>
#include "linear.h"

Linear::Linear(vector<vector<int>> cam_vec_ray, int center_x,int center_y)
{
    this->points_list = cam_vec_ray;
    this->center_x = center_x;
    this->center_y = center_y;

    Linear::bottom_line();
    Linear::third_line();

}

// 直线法表示底边直线
void Linear::bottom_line() {
    int x1 = this->points_list[0][0];
    int y1 = this->points_list[0][1];

    int x2 = this->points_list[1][0];
    int y2 = this->points_list[1][1];

    if(x1 == x2) {
        this->flag = 1; 
    }else if(y2 == y1) {
        this->flag = 2; 
    } else{
        this->flag = 0;
        this->k = (float)(y2-y1)/(x2-x1);
    }
//    printf("%d,%d\n",x1,y1);
//    printf("%d,%d\n",x2,y2);
//    cout << this->flag << endl;
//    cout << "sssss" << endl;
//    cout <<  this->k <<endl;
}

void Linear::third_line()
{
    if(this->flag == 0)
    {
        this->b = this->center_y - this->k * this->center_x;
    }
//    printf("%f,%f",this->b,this->k);
}


vector<float> Linear::inter_points(vector<int> point_m, vector<int> point_n)
{
    int xm = point_m[0];
    int ym = point_m[1];
    int xn = point_n[0];
    int yn = point_n[1];
    vector<float> inter_point_vec;
    float inter_x, inter_y;
    if(xm == xn) {
        inter_x = (float) xm;
        if (this->flag == 2)
            inter_y = (float) this->center_y;
        else
            inter_y = (float) (this->k * inter_x + this->b);
    }else if(ym == yn){
        inter_y = (float)ym;
        if(this->flag == 1)
            inter_x = (float) this->center_x;
        else
            inter_x = (inter_y - this->b)/this->k;
    }else
    {
        float k1 = (float)(ym - yn)/(xm - xn);
        float b1 = ym - k1*xm;
        if(this->flag == 1)
        {
            inter_x = (float) this->center_x;
            inter_y = k1*inter_x + b1;
        } else if (this->flag == 2)
        {
            inter_y = (float) this->center_y;
            inter_x = (inter_y - b1)/k1;
        } else
        {
            inter_x = (this->b - b1)/(k1 - this->k);
            inter_y = k1*inter_x + b1;
        }
    }

    inter_point_vec.push_back(inter_x);
    inter_point_vec.push_back(inter_y);

    return inter_point_vec;

}


double Linear::cal_distance()
{
    printf("cal distance");
    vector<float> s_point_vec = Linear::inter_points(this->points_list[0],this->points_list[3]);
    vector<float> e_point_vec = Linear::inter_points(this->points_list[1],this->points_list[2]);
    printf("%f,%f,other,%f,%f",s_point_vec[0],s_point_vec[1],e_point_vec[0],e_point_vec[1]);

    float pow_x = pow((s_point_vec[0] - e_point_vec[0]),2);
    float pow_y = pow((s_point_vec[1] - e_point_vec[1]),2);
    double distance =  pow((pow_x + pow_y),0.5);

    return distance;
}


double Linear::adjust_distance(int base_distance,int bottom_distance, double center_distance,int angle)
{
    MathUtils mm;
    double rate = cos(mm.angle_to_radian((double)angle));
    if(rate == 0)
    {
        rate = 1e-5;
    }

    double result_distance = base_distance * bottom_distance / (center_distance * rate);
    printf("\n%f::%f\n",rate,result_distance);
//    printf("\na%d%d%f%daa%fhe%f",ragresult_distance);
    return result_distance;
}

 linear.h

New file
@@ -0,0 +1,34 @@
#ifndef LINEAR_H
#define LINEAR_H
#include <iostream>
#include "camera_config.h"
#include "math_utils.h"
#include <math.h>


class Linear
{
private:
    vector<vector<int>> points_list;
    int center_x;
    int center_y;
    float k = 0;
    float b = 0;
    int flag = 0;

public:
    Linear(vector<vector<int>> cam_vec_ray, int center_x,int center_y);
    double cal_distance();
    double adjust_distance(int base_distance,int bottom_distance, double center_distance,int angle);


private:
    void bottom_line();
    void third_line();
    vector<float> inter_points(vector<int> point_A, vector<int> point_B);


};

#endif


 math_utils.cpp

New file
@@ -0,0 +1,44 @@
#include "math_utils.h"
#include <math.h>


double MathUtils::angle_to_radian(double degree)
{
    double flag = (degree < 0)? -1.0:1.0; 
    if(degree<0)
    {
       degree = degree * (-1.0);
    }
    double angle = degree;
    double result = flag * (angle* M_PI)/180;
    return result;
}


void MathUtils::radian_to_angle(double rad, double ang[])
{
    double flag = (rad < 0)? -1.0 : 1.0;
    if(rad<0)
    {
        rad = rad * (-1.0);
    }
    double result = (rad*180)/ M_PI;
    double degree = int(result);
    double min =(result - degree)*60;
    double second = (min - int(min)) * 60;
    ang[0] = flag * degree;
    ang[1] = int(min);
    ang[2] = second;
}

double MathUtils::cal_distance(vector<int> coords_A, vector<int> coords_B)
{
    int square_sum = 0;
    for (int i = 0; i < 3; ++i) {
        square_sum +=  (coords_A[i] - coords_B[i])*(coords_A[i] - coords_B[i]);
    }
    return sqrt(square_sum);
}




 math_utils.h

New file
@@ -0,0 +1,16 @@
#ifndef MATH_UTILS_H
#define MATH_UTILS_H
#define _USE_MATH_DEFINES
#include <vector>
using namespace std;

class MathUtils
{
public:
    double angle_to_radian(double degree);
    void radian_to_angle(double rad, double ang[]);
    double cal_distance(vector<int> coords_A, vector<int> coords_B);
};

#endif


 vector_utils.cpp

New file
@@ -0,0 +1,85 @@
#include "iostream"
#include <vector>
#include "algorithm" 
#include "iterator" 
#include "vector_utils.h"

using namespace std;


void print_vector(vector<int> v){
    if(v.size()>0){
        cout<<"{";
        for(int i=0;i<int(v.size());i++){
            cout<<v[i]<<",";
        }
        cout<<"\b}";
    }
    else{
        cout<<"{}";
    }
}


vector<int> unique_element_in_vector(vector<int> v){
    vector<int>::iterator vector_iterator;
    sort(v.begin(),v.end());
    vector_iterator = unique(v.begin(),v.end());
    if(vector_iterator != v.end()){
        v.erase(vector_iterator,v.end());
    }
    return v;
}


vector<int> vectors_intersection(vector<int> v1,vector<int> v2){
    vector<int> v;
    sort(v1.begin(),v1.end());
    sort(v2.begin(),v2.end());
    set_intersection(v1.begin(),v1.end(),v2.begin(),v2.end(),back_inserter(v));//求交集
    return v;
}


vector<int> vectors_set_union(vector<int> v1,vector<int> v2){
    vector<int> v;
    sort(v1.begin(),v1.end());
    sort(v2.begin(),v2.end());
    set_union(v1.begin(),v1.end(),v2.begin(),v2.end(),back_inserter(v));//求交集
    return v;
}


bool VectorUtils::is_element_in_vector(vector<int> v,int element)
{
    printf("\nDetermine whether an element of a vector exists");
    vector<int>::iterator it;
    it=find(v.begin(),v.end(),element);
    if (it!=v.end()){
        return true;
    }
    else{
        return false;
    }
}

//int main(){
//    vector<int> v1,v2,v;
//    v1.push_back(22);v1.push_back(22);v1.push_back(23);v2.push_back(23);v2.push_back(24);
//    cout<<"v1?\n" <<is_element_in_vector(v1,1)<< endl;
//    cout<<"vi：\n";
////    cout<<"v1是否存在1这个元素？" <<is_element_in_vector(v1,1)<< endl;
////    cout<<"对v1去重：";
//    v1=unique_element_in_vector(v1);
//    print_vector(v1);
//    cout<<endl;
//    cout<<"v1：\n";
////    cout<<"求v1与v2的交集：";
//    v=vectors_intersection(v1,v2);
//    print_vector(v);
//    cout<<endl;
//    cout<<"v23：\n";
//    v=vectors_set_union(v1,v2);
//    print_vector(v);
//    return 0;
//}

 vector_utils.h

New file
@@ -0,0 +1,13 @@
#ifndef VECTOR_UTILS_H
#define VECTOR_UTILS_H
#include <iostream>
#include <vector>
using namespace std;
class VectorUtils
{
public:
    bool is_element_in_vector(vector<int> v,int element);

};

#endif