pPrev = NULL;
pCurUnit->pNext = m_pFreeMemBlock; //Insert the new unit at head.
if(NULL != m_pFreeMemBlock)
{
m_pFreeMemBlock->pPrev = pCurUnit;
}
m_pFreeMemBlock = pCurUnit;
}
}
}
/*===============================================================
~CMemPool():
Destructor of this class. Its task is to free memory block.
//===============================================================
*/
CMemPool::~CMemPool()
{
free(m_pMemBlock);
}
/*================================================================
Alloc:
To allocate a memory unit. If memory pool can`t provide proper memory unit,
It will call system function.
Parameters:
[in]ulSize
Memory unit size.
[in]bUseMemPool
Whether use memory pool.
Return Values:
Return a pointer to a memory unit.
//=================================================================
*/
void* CMemPool::Alloc(unsigned long ulSize, bool bUseMemPool)
{
if( ulSize > m_ulUnitSize || false == bUseMemPool ||
NULL == m_pMemBlock || NULL == m_pFreeMemBlock)
{
return malloc(ulSize);
}
//Now FreeList isn`t empty
struct _Unit *pCurUnit = m_pFreeMemBlock;
m_pFreeMemBlock = pCurUnit->pNext; //Get a unit from free linkedlist.
if(NULL != m_pFreeMemBlock)
{
m_pFreeMemBlock->pPrev = NULL;
}
pCurUnit->pNext = m_pAllocatedMemBlock;
if(NULL != m_pAllocatedMemBlock)
{
m_pAllocatedMemBlock->pPrev = pCurUnit;
}
m_pAllocatedMemBlock = pCurUnit;
return (void *)((char *)pCurUnit + sizeof(struct _Unit) );
}
/*================================================================
Free:
To free a memory unit. If the pointer of parameter point to a memory unit,
then insert it to "Free linked list". Otherwise, call system function "free".
Parameters:
[in]p
It point to a memory unit and prepare to free it.
Return Values:
none
//================================================================
*/
void CMemPool::Free( void* p )
{
if(m_pMemBlockpNext;
if(NULL != m_pAllocatedMemBlock)
{
m_pAllocatedMemBlock->pPrev = NULL;
}
pCurUnit->pNext = m_pFreeMemBlock;
if(NULL != m_pFreeMemBlock)
{
m_pFreeMemBlock->pPrev = pCurUnit;
}
m_pFreeMemBlock = pCurUnit;
}
else
{
free(p);
}
}
================================================
FILE: server/src/mem_pool.hpp
================================================
#ifndef __MEMPOOL_H__
#define __MEMPOOL_H__
// https://www.codeproject.com/Articles/27487/Why-to-use-memory-pool-and-how-to-implement-it
class CMemPool
{
private:
//The purpose of the structure`s definition is that we can operate linkedlist conveniently
struct _Unit //The type of the node of linkedlist.
{
struct _Unit *pPrev, *pNext;
};
void* m_pMemBlock; //The address of memory pool.
//Manage all unit with two linkedlist.
struct _Unit* m_pAllocatedMemBlock; //Head pointer to Allocated linkedlist.
struct _Unit* m_pFreeMemBlock; //Head pointer to Free linkedlist.
unsigned long m_ulUnitSize; //Memory unit size. There are much unit in memory pool.
unsigned long m_ulBlockSize;//Memory pool size. Memory pool is make of memory unit.
public:
CMemPool(unsigned long lUnitNum = 50, unsigned long lUnitSize = 1024);
~CMemPool();
void* Alloc(unsigned long ulSize, bool bUseMemPool = true); //Allocate memory unit
void Free( void* p ); //Free memory unit
};
#endif //__MEMPOOL_H__
================================================
FILE: server/src/people.cpp
================================================
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include "people.hpp"
void Person::set_part(int part, float x, float y) {
history.front().x[part] = x;
history.front().y[part] = y;
// bounding box update
if (x < min_x)
min_x = x;
else if (x > max_x)
max_x = x;
if (y < min_y)
min_y = y;
else if (y > max_y)
max_y = y;
}
void Person::set_action(int type) {
if (type < 0)
type = ACTION_TYPE_NUM;
action = type;
if (actions.size() == ACTION_HIS_NUM)
actions.pop_front();
actions.push_back(type);
}
float Person::get_dist(float x1, float y1, float x2, float y2) {
if (x1 == 0 || x2 == 0)
return 0.0;
else {
return sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
}
}
float Person::get_deg(float x1, float y1, float x2, float y2) {
if (x1 == 0 || x2 == 0)
return 0.0;
double dx = x2 - x1;
double dy = y2 - y1;
double rad = atan2(dy, dx);
double degree = (rad * 180) / M_PI;
if (degree < 0)
degree += 360;
return degree;
}
bool Person::has_output(void) {
if (overlap_count <= 0 && history.size() == HIS_NUM) {
overlap_count = OVERLAP_NUM;
return true;
}
else
return false;
}
void Person::update(Person* n_p)
{
static const int change_part[] = {
RELBOW, RWRIST, LELBOW, LWRIST, RKNEE, RANKLE, LKNEE, LANKLE
};
static const int change_pair[] = {
RSHOULDER, RELBOW,
RELBOW, RWRIST,
LSHOULDER, LELBOW,
LELBOW, LWRIST,
RHIP, RKNEE,
RKNEE, RANKLE,
LHIP, LKNEE,
LKNEE, LANKLE
};
Change c;
double deg, n_deg;
int part, pair_1, pair_2;
const Joint& j = history.back();
const Joint& n_j = n_p->history.front();
// change calc
for (int i = 0; i < CHANGE_NUM; i++) {
part = change_part[i];
c.dist[i] = abs(get_dist(j.x[part], n_j.x[part], j.y[part], n_j.y[part]));
pair_1 = change_pair[i * 2];
pair_2 = change_pair[i * 2 + 1];
deg = get_deg(j.x[pair_1], j.y[pair_1], j.x[pair_2], j.y[pair_2]);
n_deg = get_deg(n_j.x[pair_1], n_j.y[pair_1], n_j.x[pair_2], n_j.y[pair_2]);
c.cur_deg[i] = n_deg;
if (deg == 0 || n_deg == 0)
c.deg[i] = 0.0;
else
c.deg[i] = abs(deg - n_deg);
}
if (history.size() == HIS_NUM) {
history.pop_front();
change_history.pop_front();
}
history.push_back(n_p->history.front());
change_history.push_back(c);
overlap_count--;
// delete
delete n_p;
}
/*
std::string get_history(void) const
{
std::stringstream ss;
for (int i = 0; i < history.size(); i++) {
if (i != 0)
ss << '\n';
for (int j = 0; j < JOINT_NUM; j++) {
if (j != 0)
ss << ',';
ss << history[i].x[j] << ',' << history[i].y[j];
}
}
for (int i = history.size(); i < HIS_NUM; i++) {
if (i != 0)
ss << '\n';
for (int j = 0; j < JOINT_NUM; j++) {
if (j != 0)
ss << ',';
ss << 0.0 << ',' << 0.0;
}
}
return ss.str();
}
*/
bool Person::check_crash(const Person& other) const
{
const static int punch_check_joint[] = {RELBOW, RWRIST, LELBOW, LWRIST};
const static int kick_check_joint[] = {RKNEE, RANKLE, LKNEE, LANKLE};
const int* check_joint = nullptr;
int my_action = this->get_action();
if (my_action == PUNCH)
check_joint = punch_check_joint;
else if (my_action == KICK)
check_joint = kick_check_joint;
else
return false;
cv::Rect_ other_rect = other.get_rect();
const Joint& j = history.back();
float x, y;
for (int i = 0; i < 4; i++) {
x = j.x[check_joint[i]];
y = j.y[check_joint[i]];
if (other_rect.x <= x && x <= other_rect.x + other_rect.width &&
other_rect.y <= y && y <= other_rect.y + other_rect.height)
return true;
}
return false;
}
cv::Rect_ Person::get_crash_rect(const Person& p) const
{
cv::Rect_ a_rect = this->get_rect();
cv::Rect_ b_rect = p.get_rect();
float min_x, min_y, max_x, max_y;
min_x = a_rect.x < b_rect.x ? a_rect.x : b_rect.x;
min_y = a_rect.y < b_rect.y ? a_rect.y : b_rect.y;
max_x = (a_rect.x + a_rect.width) > (b_rect.x + b_rect.width) ? (a_rect.x + a_rect.width) : (b_rect.x +
b_rect.width);
max_y = (a_rect.y + a_rect.height) > (b_rect.y + b_rect.height) ? (a_rect.y + a_rect.height) : (b_rect.y +
b_rect.height);
return cv::Rect_(cv::Point_(min_x, min_y),
cv::Point_(max_x, max_y));
}
std::string Person::get_history(void) const
{
std::stringstream ss;
for (size_t i = 0; i < change_history.size(); i++) {
if (i != 0)
ss << '\n';
for (int j = 0; j < CHANGE_NUM; j++) {
if (j != 0)
ss << ',';
ss << change_history[i].dist[j] << ',' << change_history[i].deg[j] << ',' << change_history[i].cur_deg[j];
}
}
for (int i = change_history.size(); i < HIS_NUM - 1; i++) {
if (i != 0)
ss << '\n';
for (int j = 0; j < CHANGE_NUM; j++) {
if (j != 0)
ss << ',';
ss << 0.0 << ',' << 0.0 << ',' << 0.0;
}
}
return ss.str();
}
cv::Rect_ Person::get_rect(void) const
{
return cv::Rect_(cv::Point_(min_x, min_y),
cv::Point_(max_x, max_y));
}
void Person::set_rect(cv::Rect_& rect)
{
min_x = rect.x;
min_y = rect.y;
max_x = rect.x + rect.width;
max_y = rect.y + rect.height;
}
Person& Person::operator=(const Person& p)
{
track_id = p.track_id;
max_x = p.max_x;
max_y = p.max_y;
min_x = p.min_x;
min_y = p.min_y;
history = p.history;
change_history = p.change_history;
overlap_count = p.overlap_count;
return *this;
}
std::ostream& operator<<(std::ostream &out, const Person &p)
{
for (size_t i = 0; i < p.change_history.size(); i++) {
for (int j = 0; j < p.CHANGE_NUM; j++) {
if (j != 0)
out << ',';
out << p.change_history[i].dist[j] << ',' << p.change_history[i].deg[j] << ',' << p.change_history[i].cur_deg[j];
}
out << '\n';
}
return out;
}
std::vector People::to_person(void) {
std::vector persons;
int person_num = keyshape[0];
int part_num = keyshape[1];
for (int person = 0; person < person_num; person++) {
Person *p = new Person();
for (int part = 0; part < part_num; part++) {
int index = (person * part_num + part) * keyshape[2];
if (keypoints[index + 2] > thresh) {
p->set_part(part, keypoints[index] * scale, keypoints[index + 1] * scale);
}
}
persons.push_back(p);
}
return persons;
}
std::string People::get_output(void) {
std::string out_str = "\"people\": [\n";
int person_num = keyshape[0];
int part_num = keyshape[1];
for (int person = 0; person < person_num; person++) {
if (person != 0)
out_str += ",\n";
out_str += " {\n";
for (int part = 0; part < part_num; part++) {
if (part != 0)
out_str += ",\n ";
int index = (person * part_num + part) * keyshape[2];
char *buf = (char*)calloc(2048, sizeof(char));
if (keypoints[index + 2] > thresh) {
sprintf(buf, " \"%d\":[%f, %f]", part, keypoints[index] * scale, keypoints[index + 1] * scale);
}
else {
sprintf(buf, " \"%d\":[%f, %f]", part, 0.0, 0.0);
}
out_str += buf;
free(buf);
}
out_str += "\n }";
}
out_str += "\n ]";
return out_str;
}
void People::render_pose_keypoints(cv::Mat& frame)
{
const int num_keypoints = keyshape[1];
unsigned int pairs[] =
{
1, 2, 1, 5, 2, 3, 3, 4, 5, 6, 6, 7, 1, 8, 8, 9, 9, 10,
1, 11, 11, 12, 12, 13, 1, 0, 0, 14, 14, 16, 0, 15, 15, 17
};
float colors[] =
{
255.f, 0.f, 85.f, 255.f, 0.f, 0.f, 255.f, 85.f, 0.f, 255.f, 170.f, 0.f,
255.f, 255.f, 0.f, 170.f, 255.f, 0.f, 85.f, 255.f, 0.f, 0.f, 255.f, 0.f,
0.f, 255.f, 85.f, 0.f, 255.f, 170.f, 0.f, 255.f, 255.f, 0.f, 170.f, 255.f,
0.f, 85.f, 255.f, 0.f, 0.f, 255.f, 255.f, 0.f, 170.f, 170.f, 0.f, 255.f,
255.f, 0.f, 255.f, 85.f, 0.f, 255.f
};
const int pairs_size = sizeof(pairs) / sizeof(unsigned int);
const int number_colors = sizeof(colors) / sizeof(float);
for (int person = 0; person < keyshape[0]; ++person)
{
// Draw lines
for (int pair = 0u; pair < pairs_size; pair += 2)
{
const int index1 = (person * num_keypoints + pairs[pair]) * keyshape[2];
const int index2 = (person * num_keypoints + pairs[pair + 1]) * keyshape[2];
if (keypoints[index1 + 2] > thresh && keypoints[index2 + 2] > thresh)
{
const int color_index = pairs[pair + 1] * 3;
cv::Scalar color { colors[(color_index + 2) % number_colors],
colors[(color_index + 1) % number_colors],
colors[(color_index + 0) % number_colors]};
cv::Point keypoint1{ intRoundUp(keypoints[index1] * scale), intRoundUp(keypoints[index1 + 1] * scale) };
cv::Point keypoint2{ intRoundUp(keypoints[index2] * scale), intRoundUp(keypoints[index2 + 1] * scale) };
cv::line(frame, keypoint1, keypoint2, color, 2);
}
}
// Draw circles
for (int part = 0; part < num_keypoints; ++part)
{
const int index = (person * num_keypoints + part) * keyshape[2];
if (keypoints[index + 2] > thresh)
{
const int color_index = part * 3;
cv::Scalar color { colors[(color_index + 2) % number_colors],
colors[(color_index + 1) % number_colors],
colors[(color_index + 0) % number_colors]};
cv::Point center{ intRoundUp(keypoints[index] * scale), intRoundUp(keypoints[index + 1] * scale) };
cv::circle(frame, center, 3, color, -1);
}
}
}
}
================================================
FILE: server/src/people.hpp
================================================
#ifndef __PEOPLE
#define __PEOPLE
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
template
inline int intRoundUp(const T a)
{
return int(a+0.5f);
}
class Person
{
private:
enum {
UNTRACK = -1,
OVERLAP_NUM = 6,
HIS_NUM = 33,
CHANGE_NUM = 8,
// action
ACTION_TYPE_NUM = 4, ACTION_HIS_NUM = 10,
STAND = 0, WALK = 1, PUNCH = 2, KICK = 3, UNKNOWN = 4,
// coco joint keypoint
JOINT_NUM = 18,
NOSE = 0, NECK = 1, RSHOULDER = 2, RELBOW = 3, RWRIST = 4, LSHOULDER = 5, LELBOW = 6,
LWRIST = 7, RHIP = 8, RKNEE = 9, RANKLE = 10, LHIP = 11, LKNEE = 12, LANKLE = 13,
REYE = 14, LEYE = 15, REAR = 16, LEAR = 17
};
struct Joint {
float x[JOINT_NUM];
float y[JOINT_NUM];
};
struct Change {
float dist[CHANGE_NUM];
float deg[CHANGE_NUM];
float cur_deg[CHANGE_NUM];
};
std::deque history;
std::deque change_history;
std::deque actions;
int overlap_count;
int track_id;
int action;
// for bounding box
float max_x;
float max_y;
float min_x;
float min_y;
Person* enemy;
public:
Person() : enemy(nullptr), overlap_count(0), track_id(UNTRACK), action(ACTION_TYPE_NUM), max_x(FLT_MIN), max_y(FLT_MIN), min_x(FLT_MAX), min_y(FLT_MAX) { history.assign(1, {0}); }
~Person() { if (enemy != nullptr) enemy->set_enemy(nullptr); }
// set
void set_part(int part, float x, float y);
void set_id(int id) { track_id = id; }
void set_action(int type);
void set_enemy(Person* p) { enemy = p; }
void set_rect(cv::Rect_& rect);
// get
inline int get_id(void) const { return track_id; }
inline int get_action(void) const { return action; }
inline const char* get_action_str(void) const {
static const char* action_str[] = {"STAND", "WALK", "PUNCH", "KICK", "UNKNOWN"};
return action_str[action];
}
inline const Person* get_enemy(void) const { return enemy; }
cv::Rect_ get_rect(void) const;
cv::Rect_ get_crash_rect(const Person& p) const;
// crash
inline bool is_danger(void) const { return (action == PUNCH || action == KICK); }
bool check_crash(const Person& p) const;
void update(Person* n_p);
bool has_output(void);
std::string get_history(void) const;
// util
float get_dist(float x1, float y1, float x2, float y2);
float get_deg(float x1, float y1, float x2, float y2);
Person& operator=(const Person& p);
friend std::ostream& operator<<(std::ostream &out, const Person &p);
};
class People
{
public:
friend class boost::serialization::access;
const float thresh = 0.05;
std::vector keypoints;
std::vector keyshape;
float scale;
People() {}
People(std::vector _keypoints, std::vector _keyshape, float _scale) :
keypoints(_keypoints), keyshape(_keyshape), scale(_scale) {}
inline int get_person_num(void) const { return keyshape[0]; };
std::vector to_person(void);
std::string get_output(void);
void render_pose_keypoints(cv::Mat& frame);
template
void serialize(Archive & ar, const unsigned int version)
{
ar & keypoints;
ar & keyshape;
ar & scale;
}
};
#endif
================================================
FILE: server/src/pose_detector.cpp
================================================
#include
#include
using namespace std;
#include
#include
#include
using namespace cv;
#include "pose_detector.hpp"
#define POSE_MAX_PEOPLE 96
#define NET_OUT_CHANNELS 57 // 38 for pafs, 19 for parts
template
inline int intRound(const T a)
{
return int(a+0.5f);
}
template
inline T fastMin(const T a, const T b)
{
return (a < b ? a : b);
}
void PoseDetector::connect_bodyparts
(
vector& pose_keypoints,
const float* const map,
const float* const peaks,
int mapw,
int maph,
const int inter_min_above_th,
const float inter_th,
const int min_subset_cnt,
const float min_subset_score,
vector& keypoint_shape
)
{
keypoint_shape.resize(3);
const int body_part_pairs[] =
{
1, 2, 1, 5, 2, 3, 3, 4, 5, 6, 6, 7, 1, 8, 8, 9, 9, 10, 1, 11, 11,
12, 12, 13, 1, 0, 0, 14, 14, 16, 0, 15, 15, 17, 2, 16, 5, 17
};
const int limb_idx[] =
{
31, 32, 39, 40, 33, 34, 35, 36, 41, 42, 43, 44, 19, 20, 21, 22, 23, 24, 25,
26, 27, 28, 29, 30, 47, 48, 49, 50, 53, 54, 51, 52, 55, 56, 37, 38, 45, 46
};
const int num_body_parts = 18; // COCO part number
const int num_body_part_pairs = num_body_parts + 1;
std::vector, double>> subset;
const int subset_counter_index = num_body_parts;
const int subset_size = num_body_parts + 1;
const int peaks_offset = 3 * (POSE_MAX_PEOPLE + 1);
const int map_offset = mapw * maph;
for (unsigned int pair_index = 0u; pair_index < num_body_part_pairs; ++pair_index)
{
const int body_partA = body_part_pairs[2 * pair_index];
const int body_partB = body_part_pairs[2 * pair_index + 1];
const float* candidateA = peaks + body_partA*peaks_offset;
const float* candidateB = peaks + body_partB*peaks_offset;
const int nA = (int)(candidateA[0]); // number of part A candidates
const int nB = (int)(candidateB[0]); // number of part B candidates
// add parts into the subset in special case
if (nA == 0 || nB == 0)
{
// Change w.r.t. other
if (nA == 0) // nB == 0 or not
{
for (int i = 1; i <= nB; ++i)
{
bool num = false;
for (unsigned int j = 0u; j < subset.size(); ++j)
{
const int off = body_partB*peaks_offset + i * 3 + 2;
if (subset[j].first[body_partB] == off)
{
num = true;
break;
}
}
if (!num)
{
std::vector row_vector(subset_size, 0);
// store the index
row_vector[body_partB] = body_partB*peaks_offset + i * 3 + 2;
// the parts number of that person
row_vector[subset_counter_index] = 1;
// total score
const float subsetScore = candidateB[i * 3 + 2];
subset.emplace_back(std::make_pair(row_vector, subsetScore));
}
}
}
else // if (nA != 0 && nB == 0)
{
for (int i = 1; i <= nA; i++)
{
bool num = false;
for (unsigned int j = 0u; j < subset.size(); ++j)
{
const int off = body_partA*peaks_offset + i * 3 + 2;
if (subset[j].first[body_partA] == off)
{
num = true;
break;
}
}
if (!num)
{
std::vector row_vector(subset_size, 0);
// store the index
row_vector[body_partA] = body_partA*peaks_offset + i * 3 + 2;
// parts number of that person
row_vector[subset_counter_index] = 1;
// total score
const float subsetScore = candidateA[i * 3 + 2];
subset.emplace_back(std::make_pair(row_vector, subsetScore));
}
}
}
}
else // if (nA != 0 && nB != 0)
{
std::vector> temp;
const int num_inter = 10;
// limb PAF x-direction heatmap
const float* const mapX = map + limb_idx[2 * pair_index] * map_offset;
// limb PAF y-direction heatmap
const float* const mapY = map + limb_idx[2 * pair_index + 1] * map_offset;
// start greedy algorithm
for (int i = 1; i <= nA; i++)
{
for (int j = 1; j <= nB; j++)
{
const int dX = candidateB[j * 3] - candidateA[i * 3];
const int dY = candidateB[j * 3 + 1] - candidateA[i * 3 + 1];
const float norm_vec = float(std::sqrt(dX*dX + dY*dY));
// If the peaksPtr are coincident. Don't connect them.
if (norm_vec > 1e-6)
{
const float sX = candidateA[i * 3];
const float sY = candidateA[i * 3 + 1];
const float vecX = dX / norm_vec;
const float vecY = dY / norm_vec;
float sum = 0.;
int count = 0;
for (int lm = 0; lm < num_inter; lm++)
{
const int mX = fastMin(mapw - 1, intRound(sX + lm*dX / num_inter));
const int mY = fastMin(maph - 1, intRound(sY + lm*dY / num_inter));
const int idx = mY * mapw + mX;
const float score = (vecX*mapX[idx] + vecY*mapY[idx]);
if (score > inter_th)
{
sum += score;
++count;
}
}
// parts score + connection score
if (count > inter_min_above_th)
{
temp.emplace_back(std::make_tuple(sum / count, i, j));
}
}
}
}
// select the top minAB connection, assuming that each part occur only once
// sort rows in descending order based on parts + connection score
if (!temp.empty())
{
std::sort(temp.begin(), temp.end(), std::greater>());
}
std::vector> connectionK;
const int minAB = fastMin(nA, nB);
// assuming that each part occur only once, filter out same part1 to different part2
std::vector occurA(nA, 0);
std::vector occurB(nB, 0);
int counter = 0;
for (unsigned int row = 0u; row < temp.size(); row++)
{
const float score = std::get<0>(temp[row]);
const int aidx = std::get<1>(temp[row]);
const int bidx = std::get<2>(temp[row]);
if (!occurA[aidx - 1] && !occurB[bidx - 1])
{
// save two part score "position" and limb mean PAF score
connectionK.emplace_back(std::make_tuple(body_partA*peaks_offset + aidx * 3 + 2,
body_partB*peaks_offset + bidx * 3 + 2, score));
++counter;
if (counter == minAB)
{
break;
}
occurA[aidx - 1] = 1;
occurB[bidx - 1] = 1;
}
}
// Cluster all the body part candidates into subset based on the part connection
// initialize first body part connection
if (pair_index == 0)
{
for (const auto connectionKI : connectionK)
{
std::vector row_vector(num_body_parts + 3, 0);
const int indexA = std::get<0>(connectionKI);
const int indexB = std::get<1>(connectionKI);
const double score = std::get<2>(connectionKI);
row_vector[body_part_pairs[0]] = indexA;
row_vector[body_part_pairs[1]] = indexB;
row_vector[subset_counter_index] = 2;
// add the score of parts and the connection
const double subset_score = peaks[indexA] + peaks[indexB] + score;
subset.emplace_back(std::make_pair(row_vector, subset_score));
}
}
// Add ears connections (in case person is looking to opposite direction to camera)
else if (pair_index == 17 || pair_index == 18)
{
for (const auto& connectionKI : connectionK)
{
const int indexA = std::get<0>(connectionKI);
const int indexB = std::get<1>(connectionKI);
for (auto& subsetJ : subset)
{
auto& subsetJ_first = subsetJ.first[body_partA];
auto& subsetJ_first_plus1 = subsetJ.first[body_partB];
if (subsetJ_first == indexA && subsetJ_first_plus1 == 0)
{
subsetJ_first_plus1 = indexB;
}
else if (subsetJ_first_plus1 == indexB && subsetJ_first == 0)
{
subsetJ_first = indexA;
}
}
}
}
else
{
if (!connectionK.empty())
{
for (unsigned int i = 0u; i < connectionK.size(); ++i)
{
const int indexA = std::get<0>(connectionK[i]);
const int indexB = std::get<1>(connectionK[i]);
const double score = std::get<2>(connectionK[i]);
int num = 0;
// if A is already in the subset, add B
for (unsigned int j = 0u; j < subset.size(); j++)
{
if (subset[j].first[body_partA] == indexA)
{
subset[j].first[body_partB] = indexB;
++num;
subset[j].first[subset_counter_index] = subset[j].first[subset_counter_index] + 1;
subset[j].second = subset[j].second + peaks[indexB] + score;
}
}
// if A is not found in the subset, create new one and add both
if (num == 0)
{
std::vector row_vector(subset_size, 0);
row_vector[body_partA] = indexA;
row_vector[body_partB] = indexB;
row_vector[subset_counter_index] = 2;
const float subsetScore = peaks[indexA] + peaks[indexB] + score;
subset.emplace_back(std::make_pair(row_vector, subsetScore));
}
}
}
}
}
}
// Delete people below thresholds, and save to output
int number_people = 0;
std::vector valid_subset_indexes;
valid_subset_indexes.reserve(fastMin((size_t)POSE_MAX_PEOPLE, subset.size()));
for (unsigned int index = 0; index < subset.size(); ++index)
{
const int subset_counter = subset[index].first[subset_counter_index];
const double subset_score = subset[index].second;
if (subset_counter >= min_subset_cnt && (subset_score / subset_counter) > min_subset_score)
{
++number_people;
valid_subset_indexes.emplace_back(index);
if (number_people == POSE_MAX_PEOPLE)
{
break;
}
}
}
// Fill and return pose_keypoints
keypoint_shape = { number_people, (int)num_body_parts, 3 };
if (number_people > 0)
{
pose_keypoints.resize(number_people * (int)num_body_parts * 3);
}
else
{
pose_keypoints.clear();
}
for (unsigned int person = 0u; person < valid_subset_indexes.size(); ++person)
{
const auto& subsetI = subset[valid_subset_indexes[person]].first;
for (int bodyPart = 0u; bodyPart < num_body_parts; bodyPart++)
{
const int base_offset = (person*num_body_parts + bodyPart) * 3;
const int body_part_index = subsetI[bodyPart];
if (body_part_index > 0)
{
pose_keypoints[base_offset] = peaks[body_part_index - 2];
pose_keypoints[base_offset + 1] = peaks[body_part_index - 1];
pose_keypoints[base_offset + 2] = peaks[body_part_index];
}
else
{
pose_keypoints[base_offset] = 0.f;
pose_keypoints[base_offset + 1] = 0.f;
pose_keypoints[base_offset + 2] = 0.f;
}
}
}
}
void PoseDetector::find_heatmap_peaks
(
const float *src,
float *dst,
const int SRCW,
const int SRCH,
const int SRC_CH,
const float TH
)
{
// find peaks (8-connected neighbor), weights with 7 by 7 area to get sub-pixel location and response
const int SRC_PLANE_OFFSET = SRCW * SRCH;
// add 1 for saving total people count, 3 for x, y, score
const int DST_PLANE_OFFSET = (POSE_MAX_PEOPLE + 1) * 3;
float *dstptr = dst;
int c = 0;
int x = 0;
int y = 0;
int i = 0;
int j = 0;
// TODO: reduce multiplication by using pointer
for(c = 0; c < SRC_CH - 1; ++c)
{
int num_people = 0;
for(y = 1; y < SRCH - 1 && num_people != POSE_MAX_PEOPLE; ++y)
{
for(x = 1; x < SRCW - 1 && num_people != POSE_MAX_PEOPLE; ++x)
{
int idx = y * SRCW + x;
float value = src[idx];
if (value > TH)
{
const float TOPLEFT = src[idx - SRCW - 1];
const float TOP = src[idx - SRCW];
const float TOPRIGHT = src[idx - SRCW + 1];
const float LEFT = src[idx - 1];
const float RIGHT = src[idx + 1];
const float BUTTOMLEFT = src[idx + SRCW - 1];
const float BUTTOM = src[idx + SRCW];
const float BUTTOMRIGHT = src[idx + SRCW + 1];
if(value > TOPLEFT && value > TOP && value > TOPRIGHT && value > LEFT &&
value > RIGHT && value > BUTTOMLEFT && value > BUTTOM && value > BUTTOMRIGHT)
{
float x_acc = 0;
float y_acc = 0;
float score_acc = 0;
for (i = -3; i <= 3; ++i)
{
int ux = x + i;
if (ux >= 0 && ux < SRCW)
{
for (j = -3; j <= 3; ++j)
{
int uy = y + j;
if (uy >= 0 && uy < SRCH)
{
float score = src[uy * SRCW + ux];
x_acc += ux * score;
y_acc += uy * score;
score_acc += score;
}
}
}
}
x_acc /= score_acc;
y_acc /= score_acc;
score_acc = value;
dstptr[(num_people + 1) * 3 + 0] = x_acc;
dstptr[(num_people + 1) * 3 + 1] = y_acc;
dstptr[(num_people + 1) * 3 + 2] = score_acc;
++num_people;
}
}
}
}
dstptr[0] = num_people;
src += SRC_PLANE_OFFSET;
dstptr += DST_PLANE_OFFSET;
}
}
Mat PoseDetector::create_netsize_im
(
const Mat &im,
const int netw,
const int neth,
float *scale
)
{
// for tall image
int newh = neth;
float s = newh / (float)im.rows;
int neww = im.cols * s;
if (neww > netw)
{
//for fat image
neww = netw;
s = neww / (float)im.cols;
newh = im.rows * s;
}
*scale = 1 / s;
Rect dst_area(0, 0, neww, newh);
Mat dst = Mat::zeros(neth, netw, CV_8UC3);
resize(im, dst(dst_area), Size(neww, newh));
return dst;
}
PoseDetector::PoseDetector(const char *cfg_path, const char *weight_path, int gpu_id) : Detector(cfg_path, weight_path,
gpu_id) {
det_people = nullptr;
// initialize net
net_inw = get_net_width();
net_inh = get_net_height();
net_outw = get_net_out_width();
net_outh = get_net_out_height();
}
void PoseDetector::detect(cv::Mat im, float thresh) {
// 3. resize to net input size, put scaled image on the top left
float scale = 0.0f;
Mat netim = create_netsize_im(im, net_inw, net_inh, &scale);
// 4. normalized to float type
netim.convertTo(netim, CV_32F, 1 / 256.f, -0.5);
// 5. split channels
float *netin_data = new float[net_inw * net_inh * 3]();
float *netin_data_ptr = netin_data;
vector input_channels;
for (int i = 0; i < 3; ++i)
{
Mat channel(net_inh, net_inw, CV_32FC1, netin_data_ptr);
input_channels.emplace_back(channel);
netin_data_ptr += (net_inw * net_inh);
}
split(netim, input_channels);
// 6. feed forward
double time_begin = getTickCount();
float *netoutdata = Detector::predict(netin_data);
double fee_time = (getTickCount() - time_begin) / getTickFrequency() * 1000;
#ifdef DEBUG
cout << "forward fee: " << fee_time << "ms" << endl;
#endif
// 7. resize net output back to input size to get heatmap
float *heatmap = new float[net_inw * net_inh * NET_OUT_CHANNELS];
for (int i = 0; i < NET_OUT_CHANNELS; ++i)
{
Mat netout(net_outh, net_outw, CV_32F, (netoutdata + net_outh*net_outw*i));
Mat nmsin(net_inh, net_inw, CV_32F, heatmap + net_inh*net_inw*i);
resize(netout, nmsin, Size(net_inw, net_inh), 0, 0, CV_INTER_CUBIC);
}
// 8. get heatmap peaks
float *heatmap_peaks = new float[3 * (POSE_MAX_PEOPLE+1) * (NET_OUT_CHANNELS-1)];
find_heatmap_peaks(heatmap, heatmap_peaks, net_inw, net_inh, NET_OUT_CHANNELS, 0.05);
// 9. link parts
vector keypoints;
vector shape;
connect_bodyparts(keypoints, heatmap, heatmap_peaks, net_inw, net_inh, 9, 0.05, 6, 0.4, shape);
delete [] heatmap_peaks;
delete [] heatmap;
delete [] netin_data;
// people
if (det_people != nullptr)
delete det_people;
det_people = new People(keypoints, shape, scale);
}
void PoseDetector::draw(cv::Mat mat)
{
det_people->render_pose_keypoints(mat);
}
std::string PoseDetector::det_to_json(int frame_id)
{
std::string out_str;
char* tmp_buf = (char *)calloc(1024, sizeof(char));
sprintf(tmp_buf, "{\n \"frame_id\":%d, \n ", frame_id);
out_str = tmp_buf;
out_str += det_people->get_output();
out_str += "\n}";
free(tmp_buf);
return out_str;
}
PoseDetector::~PoseDetector() {
}
================================================
FILE: server/src/pose_detector.hpp
================================================
#ifndef __POSE_DETECTOR
#define __POSE_DETECTOR
#include
#include
#include
#include
#include
#include "yolo_v2_class.hpp"
#include "DetectorInterface.hpp"
#include "people.hpp"
class PoseDetector : public Detector, public DetectorInterface
{
private:
int net_inw;
int net_inh;
int net_outw;
int net_outh;
People* det_people;
public:
PoseDetector(const char *cfg_path, const char *weight_path, int gpu_id);
~PoseDetector();
inline People* get_people(void) { return det_people; }
virtual void detect(cv::Mat mat, float thresh);
virtual void draw(cv::Mat mat);
virtual std::string det_to_json(int frame_id);
private:
void connect_bodyparts
(
std::vector& pose_keypoints,
const float* const map,
const float* const peaks,
int mapw,
int maph,
const int inter_min_above_th,
const float inter_th,
const int min_subset_cnt,
const float min_subset_score,
std::vector& keypoint_shape
);
void find_heatmap_peaks
(
const float *src,
float *dst,
const int SRCW,
const int SRCH,
const int SRC_CH,
const float TH
);
cv::Mat create_netsize_im
(
const cv::Mat &im,
const int netw,
const int neth,
float *scale
);
};
#endif
================================================
FILE: server/src/share_queue.h
================================================
#pragma once
// https://stackoverflow.com/questions/36762248/why-is-stdqueue-not-thread-safe
#include
#include
#include
template
class SharedQueue
{
public:
SharedQueue();
~SharedQueue();
T& front();
void pop_front();
void push_back(const T& item);
void push_back(T&& item);
int size();
bool empty();
private:
std::deque queue_;
std::mutex mutex_;
std::condition_variable cond_;
};
template
SharedQueue::SharedQueue() {}
template
SharedQueue::~SharedQueue() {}
template
T& SharedQueue::front()
{
std::unique_lock mlock(mutex_);
while (queue_.empty())
{
cond_.wait(mlock);
}
return queue_.front();
}
template
void SharedQueue::pop_front()
{
std::unique_lock mlock(mutex_);
while (queue_.empty())
{
cond_.wait(mlock);
}
queue_.pop_front();
}
template
void SharedQueue::push_back(const T& item)
{
std::unique_lock mlock(mutex_);
queue_.push_back(item);
mlock.unlock(); // unlock before notificiation to minimize mutex con
cond_.notify_one(); // notify one waiting thread
}
template
void SharedQueue::push_back(T&& item)
{
std::unique_lock mlock(mutex_);
queue_.push_back(std::move(item));
mlock.unlock(); // unlock before notificiation to minimize mutex con
cond_.notify_one(); // notify one waiting thread
}
template
int SharedQueue::size()
{
std::unique_lock mlock(mutex_);
int size = queue_.size();
mlock.unlock();
return size;
}
================================================
FILE: server/src/sink.cpp
================================================
#include
#include
#include
#include
#include
#include
#include
#include
|
|
|:---:|:---:|
|
|
|
|
|
|:---:|:---:|
|
|
|
|
### Pose Estimation and Object Tracking
Made pipeline to get 2D pose time series data in video sequence.
In worker process, Pose Estimation is performed using ***OpenPose***. Input image pass through the Pose_detector, and get the people object which packed up people joint coordinates. People object serialized and send to sink process.
In Sink process, people object convert to person objects. and every person object are send to Tracker. Tracker receive person object and produces object identities Using ***SORT***(simple online and realtime tracking algorithm).
Finally, can get the joint time series data per each person. each person's Time series data is managed by queue container. so person object always maintain recent 32 frame.
| ```Tracking Pipeline``` | ```Time series data``` |
|:---:|:---:|
|
|
|
* #### Examples of Result (Pose Estimation and Object Tracking)
|
|
|
|:---:|:---:|
### Collect action video
Collected Four Action type (Standing, Walking, Punching, Kicking) video.
Punching Action type video is a subset of the ***Berkeley Multimodal Human Action Database (MHAD) dataset***.
This video data is comprised of 12 subjects doing the punching actions for 5 repetitions, filmed from 4 angles. (http://tele-immersion.citris-uc.org/berkeley_mhad)
Others (Standing, Walking, Kicking) are subsets of the ***CMU Panoptic Dataset***. In Range of Motion videos(171204_pose3, 171204_pose5, 171204_pose6), cut out 3 action type. I recorded timestamp per action type and cut out the video using python script(util/concat.py). This video data is comprised of 13 subjects doing the three actions, filmed from 31 angles. (http://domedb.perception.cs.cmu.edu/index.html)
``` jsonc
0, 1, 10 // 0 : Standing, 1 : begin timestamp, 10: end timestamp
1, 11, 15 // 1 : Walking, 11 : begin timestamp, 15: end timestamp
3, 39, 46 // 3 : Kicking, 39 : begin timestamp, 46: end timestamp
```
* #### Examples of Dataset (Stand & Walk)
|
|
|:---:|:---:|
|
|
|
|
|
|:---:|:---:|
|
|
|
|
|
* #### Overview of feature vector
|
|:---:|
|
|:---:|
### RNN Training and Result
The network used in this experiment is based on that of :
* Guillaume Chevalier, 'LSTMs for Human Activity Recognition, 2016' https://github.com/guillaume-chevalier/LSTM-Human-Activity-Recognition
* stuarteiffert, 'RNN-for-Human-Activity-Recognition-using-2D-Pose-Input' https://github.com/stuarteiffert/RNN-for-Human-Activity-Recognition-using-2D-Pose-Input
training was run for 300 epochs with a batch size of 1024. (weights/action.h5)
After training, To get action recognition result in real time, made action recognition pipeline. Sink process send each person's time series feature vector to action process as string. Action process put received data into RNN network and send back results of prediction. (0 : Standing, 1 : Walking, 2 : Punching, 3 : Kicking)
| ```Action Recognition Pipeline``` | ```RNN Model``` |
|:---:|:---:|
|
|
|
* #### Examples of Result (RNN Action Recognition)
| [```standing```](https://www.youtube.com/watch?v=Orc0Eq9bWOs) | [```Walking```](https://www.youtube.com/watch?v=Orc0Eq9bWOs) | [```Punching```](https://www.youtube.com/watch?v=kbgkeTTSau8) | [```Kicking```](https://www.youtube.com/watch?v=R1UWcG9N6tI) |
|:---:|:---:|:---:|:---:|
|
|
|
|
|
### Fight Detection
This stage check that person who kick or punch is hitting someone. if some person has hit other, those people set enemy each other.
System count it as fight and then track them until they exist in frame.
| ```Fight Detection Pipeline``` |
|:---:|
|
|
* #### Examples of Result
| [```Fighting Championship```](https://www.youtube.com/watch?v=cIhoK4cPbC4) | [```CCTV Video```](https://www.youtube.com/watch?v=stJPOb6zW7U) |
|:---:|:---:|
|
|
|
| [```Sparring video A```](https://www.youtube.com/watch?v=x0kJmieuFzI) | [```Sparring video B```](https://www.youtube.com/watch?v=x0kJmieuFzI) |
|:---:|:---:|
|
|
|
* #### Examples of Result (Failure case)
| [```Fake Person```](https://www.youtube.com/watch?v=kbgkeTTSau8) | [```Small Person```](https://www.youtube.com/watch?v=aUdKzb4LGJI) |
|:---:|:---:|
|
|
|
### References
* #### Darknet : https://github.com/AlexeyAB/darknet
* #### OpenCV : https://github.com/opencv/opencv
* #### ZeroMQ : https://github.com/zeromq/libzmq
* #### json-c : https://github.com/json-c/json-c
* #### openpose-darknet : https://github.com/lincolnhard/openpose-darknet
* #### sort-cpp : https://github.com/mcximing/sort-cpp
* #### cpp-base64 : https://github.com/ReneNyffenegger/cpp-base64
* #### mem_pool : https://www.codeproject.com/Articles/27487/Why-to-use-memory-pool-and-how-to-implement-it
* #### share_queue : https://stackoverflow.com/questions/36762248/why-is-stdqueue-not-thread-safe
* #### RNN-for-Human-Activity-Recognition-using-2D-Pose-Input : https://github.com/stuarteiffert/RNN-for-Human-Activity-Recognition-using-2D-Pose-Input
* #### LSTM-Human-Activity-Recognition : https://github.com/guillaume-chevalier/LSTM-Human-Activity-Recognition
================================================
FILE: client/darknet_client/Makefile
================================================
DEBUG = 1
CPP = g++
COMMON = -DOPENCV
CXXFLAGS = -g -Wall -O2 -std=c++11 -DOPENCV
LDFLAGS = -lstdc++ -lpthread -lzmq -lrt -ltbb
CXXFLAGS += `pkg-config --cflags json-c`
CXXFLAGS += `pkg-config --cflags opencv`
LDFLAGS += `pkg-config --libs json-c`
LDFLAGS += `pkg-config --libs opencv`
ifeq ($(DEBUG), 1)
COMMON += -DDEBUG
endif
VPATH = ./src/
OBJDIR = ./obj/
DEPS = $(wildcard src/*.h*)
EXEC1 = darknet_client
EXEC1_OBJ = main.o frame.o mem_pool.o base64.o args.o util.o
EXEC1_OBJS = $(addprefix $(OBJDIR), $(EXEC1_OBJ))
OBJS = $(EXEC1_OBJS)
EXECS = $(EXEC1)
all: $(OBJDIR) $(EXECS)
$(EXEC1): $(EXEC1_OBJS)
$(CPP) $(COMMON) $(CXXFLAGS) $^ -o $@ $(LDFLAGS)
$(OBJDIR)%.o: %.cpp $(DEPS)
$(CPP) $(COMMON) $(CXXFLAGS) -c $< -o $@
$(OBJDIR):
mkdir -p $(OBJDIR) res
clean:
rm -rf $(OBJS) $(EXECS)
================================================
FILE: client/darknet_client/darknet_client.vcxproj
================================================