Camshift(物体追従)

ManyCamShiftAreaDesition.h
-------------------------------------

#pragma once
#include "cvbase.h"
#include "opencv2/video/tracking.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"

#include <iostream>
#include <ctype.h>
#include <stdio.h>

using namespace cv;
using namespace std;
class CamShiftResult
{
public:
int frameNum;
RotatedRect trackBox;
};
class CamShiftArea
{
public:
CamShiftArea(void);
CamShiftArea(int width,int height);
~CamShiftArea(void);
Rect selection;
bool selectObject;
int trackObject;
Mat histimg;
Mat backproj;
Mat hist;
Rect trackWindow;
Mat positionData;
Mat graph;
int beforeAreaNum;
int beforeTime;
FILE *csvFile;
vector<Point2f> points;
vector<CamShiftResult *> results;
int getTimePoint(int x,int y);
int getAreaNum(Point2f p);
void drawGraph(Mat graph,int time,int areaNum);
void drawLines(Mat image,cv::Scalar color,bool colorFlag);
void drawTarget(Mat image,cv::Scalar color,int num);
void drawArea(Mat image);
void calucurateCamShit(Mat image,int num,int nowTime,int _vmin, int _vmax ,int smin,bool backprojMode);

};
class ManyCamShiftAreaDesition:
public CVBase
{
public:
ManyCamShiftAreaDesition(void);
~ManyCamShiftAreaDesition(void);void init(void);
void mainloop(void);
void term(void);
void mouseEvent (int event, int x, int y, int flags, void *param);
void keyEvent(char c);
void drawNowPoint(Mat graph,int num);

void startCreateArea(int x, int y);
void endCreateArea(int x, int y);
void timeShift(int x, int y);
void createWork(int x,int y);

int speed;
bool paused;
int targetArea;
int waitSprrd;

vector<CamShiftArea *> areas;
CamShiftArea *tmpArea;

Mat image;
Mat positionData;
int caluculatedNum;

bool backprojMode;
bool showHist;
Point origin;
Rect selection;
int vmin;
int vmax;
int smin;
VideoCapture cap;

vector<Point2f> tmpPoints;
};




ManyCamShiftAreaDesition.cpp
---------------------------------------

#include "ManyCamShiftAreaDesition.h"

void on_mouse (int event, int x, int y, int flags, void *param = NULL);
Scalar convertNum2Scalar(int num)
{
num++;
printf("%d,%d,%d\n",((int)num%(255*255))%255, (int)((int)num%(255*255))/255, (int)num/255/255);
return Scalar(((int)num%(255*255))%255, (int)((int)num%(255*255))/255, (int)num/255/255);
}

Scalar convertSmallNum2Scalar(int num)
{
int r,g,b;
if(num == 0){
r = 255;
g = 100;
b = 100;
}else if(num == 1){
r = 255;
g = 255;
b = 100;
}else if(num == 2){
r = 255;
g = 100;
b = 255;
}else if(num == 3){
r = 100;
g = 255;
b = 100;
}else if(num == 4){
r = 100;
g = 255;
b = 255;
}else if(num == 5){
r = 100;
g = 100;
b = 255;
}else{
r = 255;
g = 255;
b = 255;
}
return Scalar(r,g,b);
}

Scalar convertMidiumNum2Scalar(int num)
{
int r,g,b;
num %= (255*6);
if(num <255){
r = 255;
g = num;
b = 0;
}else if(num < 255 * 2){
r = 255-(num - 255 * 1);
g = 255;
b = 0;
}else if(num < 255 *3){
r = 0;
g = 255;
b = num - 255 * 2;
}else if(num < 255*4){
r = 0;
g = 255-(num - 255 * 3);
b = 255;
}else if(num < 255*5){
r = num - 255 * 4;
g = 0;
b = 255;
}else if(num < 255 * 6){
r = 255;
g = 0;
b = 255-(num - 255 * 5);
}
return Scalar(r,g,b);
}

int convertScalar2Num(int r,int g,int b)
{
return b * 255 * 255 + g * 255 + r-1;
}

CamShiftArea::CamShiftArea(void)
{
histimg = Mat::zeros(200, 320, CV_8UC3);
selectObject = false;
trackObject = 0;
}

CamShiftArea::CamShiftArea(int width,int height)
{
histimg = Mat::zeros(200, 320, CV_8UC3);
positionData = Mat::zeros(width, height, CV_8UC3);
selectObject = false;
trackObject = 0;
positionData = Mat::zeros(width, height, CV_8UC3);
beforeAreaNum = -1;
beforeTime = 0;

graph =  Mat::zeros( 10*10*5,1200, CV_8UC3);
}

CamShiftArea::~CamShiftArea(void)
{
}

void CamShiftArea::calucurateCamShit(Mat image,int num,int nowTime,int _vmin, int _vmax ,int smin,bool backprojMode)
{//http://iwaki2009.blogspot.jp/2013/02/opencv_68.html
int hsize = 16;
    float hranges[] = {0,180};
    const float* phranges = hranges;

Mat hsv,mask,hue;;
cv::cvtColor(image, hsv, CV_BGR2HSV);
if( trackObject  )//&& caluculatedNum < nowNum
{
/*============２　マスク画像の作成============*/
inRange(hsv,
Scalar(0, smin, MIN(_vmin,_vmax)),
Scalar(180, 256, MAX(_vmin, _vmax)),
mask);

/*============３　RGBからHSV変換し、H画像の取得============*/
int ch[] = {0, 0};
hue.create(hsv.size(), hsv.depth());
mixChannels(&hsv, 1, &hue, 1, ch, 1);

/*============４　指定エリアのヒストグラムを計算、正規化、描画============*/
if( trackObject < 0 ){
Mat roi(hue, selection), maskroi(mask, selection);
calcHist(&roi, 1, 0, maskroi, hist, 1, &hsize, &phranges);
normalize(hist, hist, 0, 255, CV_MINMAX);

trackWindow = selection;
trackObject = 1;

histimg = Scalar::all(0);
int binW = histimg.cols / hsize;
Mat buf(1, hsize, CV_8UC3);
for( int i = 0; i < hsize; i++ ){
buf.at<Vec3b>(i) = Vec3b(saturate_cast<uchar>(i*180./hsize), 255, 255);
}
cv::cvtColor(buf, buf, CV_HSV2BGR);

for( int i = 0; i < hsize; i++ ){
int val = saturate_cast<int>(hist.at<float>(i)*histimg.rows/255);
rectangle( histimg, Point(i*binW,histimg.rows),
Point((i+1)*binW,histimg.rows - val),
Scalar(buf.at<Vec3b>(i)), -1, 8 );
}
}


/*============５　ヒストグラムを元にBackProjectionを計算============*/
calcBackProject(&hue, 1, 0, hist, backproj, &phranges);

/*============６　計算結果にマスクをかける============*/
backproj &= mask;

/*============７　CamShiftにより対象エリアの計算============*/
RotatedRect trackBox = cv::CamShift(backproj, trackWindow,
TermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ));

/*============８　次回探索枠の計算============*/
if( trackWindow.area() <= 1 ){
int cols = backproj.cols, rows = backproj.rows, r = (MIN(cols, rows) + 5)/6;
trackWindow = Rect(trackWindow.x - r, trackWindow.y - r,
trackWindow.x + r, trackWindow.y + r) &
Rect(0, 0, cols, rows);
}
if( backprojMode ){
cv::cvtColor( backproj, image, CV_GRAY2BGR );
}

/*============現在エリアの探索============*/
int areaNum=this->getAreaNum(trackBox.center);

/*============チャートの描画============*/
if (areaNum != -1)
this->drawGraph(graph,num,areaNum);

/*============エリア移動時はCSVに出力============*/
if(areaNum != beforeAreaNum){
fprintf(csvFile,"%d,%d\n",areaNum,nowTime-beforeTime);
beforeTime = nowTime;
beforeAreaNum = areaNum;
}

/*============探索結果を保存============*/
CamShiftResult *result = new CamShiftResult();
result->frameNum = num;
result->trackBox = trackBox;
this->results.push_back(result);

/*============軌跡を描画============*/
if (this->results.size() > 1){
cv::line(positionData,
this->results[this->results.size()-1]->trackBox.center,
this->results[this->results.size()-2]->trackBox.center,
convertNum2Scalar(num),8,8,0);
}
}
}

int CamShiftArea::getAreaNum(Point2f p)
{
for (int i=0;i<(int)points.size()/4;i++){
int targeti = i * 4;
int vec1[2];
vec1[0] = points[targeti+1].x -points[targeti].x;
vec1[1] = points[targeti+1].y -points[targeti].y;
int vec2[2];
vec2[0] = p.x -points[targeti].x;
vec2[1] = p.y -points[targeti].y;
if (vec1[0]*vec2[0]+vec1[1]*vec2[1] > 0.0){
targeti++;
vec1[0] = points[targeti+1].x -points[targeti].x;
vec1[1] = points[targeti+1].y -points[targeti].y;
vec2[0] = p.x -points[targeti].x;
vec2[1] = p.y -points[targeti].y;
if (vec1[0]*vec2[0]+vec1[1]*vec2[1] > 0.0){
targeti++;
vec1[0] = points[targeti+1].x -points[targeti].x;
vec1[1] = points[targeti+1].y -points[targeti].y;
vec2[0] = p.x -points[targeti].x;
vec2[1] = p.y -points[targeti].y;
if (vec1[0]*vec2[0]+vec1[1]*vec2[1] > 0.0){
targeti++;
vec1[0] = points[targeti-3].x -points[targeti].x;
vec1[1] = points[targeti-3].y -points[targeti].y;
vec2[0] = p.x -points[targeti].x;
vec2[1] = p.y -points[targeti].y;
if (vec1[0]*vec2[0]+vec1[1]*vec2[1] > 0.0){
return i;
}
}
}
}
}
return -1;

}

void CamShiftArea::drawGraph(Mat graph,int time,int areaNum)
{
cv::line(graph,cvPoint(time%1200,(int)(time/1200) *10*10+ areaNum*10),cvPoint(time%1200,(int)(time/1200) *10*10+ areaNum*10+10),convertMidiumNum2Scalar(time) , 1, CV_AA, 0);

}

void CamShiftArea::drawLines(Mat image,cv::Scalar color,bool colorFlag)
{
for (int i = 1;i < results.size();i++){
if(colorFlag){
color = convertMidiumNum2Scalar(results[i]->frameNum);
cv::line(image,results[i-1]->trackBox.center,results[i]->trackBox.center,color,1,8,0);
}else{
cv::line(image,results[i-1]->trackBox.center,results[i]->trackBox.center,color,1,8,0);
}
}


}

void CamShiftArea::drawTarget(Mat image,cv::Scalar color,int num)
{
for (int i = 0;i < results.size();i++){
if(results[i]->frameNum == num){
ellipse( image, results[i]->trackBox, color, 3, CV_AA );
}
}
return;
}

void CamShiftArea::drawArea(Mat image)
{
int areaNum = 0;
for (int i = 1;i < points.size();i++){
Scalar color = convertMidiumNum2Scalar(areaNum*50);
areaNum++;
cv::line(image,points[i-1],points[i],color,3,8,0);
i++;
if(i < points.size()){
cv::line(image,points[i-1],points[i],color,3,8,0);
i++;
}
if(i < points.size()){
cv::line(image,points[i-1],points[i],color,3,8,0);
cv::line(image,points[i-3],points[i],color,3,8,0);
i++;
}
}
}

int CamShiftArea::getTimePoint(int x,int y)
{
cv::Vec3b bgr = positionData.at<cv::Vec3b>(y,x);
printf("%d,%d,%d\n",bgr[0],bgr[1],bgr[2]);
if(bgr[0] == 0 && bgr[1] == 0 && bgr[2]==0)return -1;
int num = convertScalar2Num(bgr[0],bgr[1],bgr[2]);
return num;
}

ManyCamShiftAreaDesition::ManyCamShiftAreaDesition(void)
{
backprojMode = false;
showHist = true;
vmin = 10;
vmax = 256;
smin = 30;
caluculatedNum = -1;
targetArea = -1;
}

ManyCamShiftAreaDesition::~ManyCamShiftAreaDesition(void)
{
}

void ManyCamShiftAreaDesition::startCreateArea(int x, int y)
{
origin = Point(x,y);
tmpArea = new CamShiftArea( image.rows, image.cols);
tmpArea->selection = Rect(x,y,0,0);
}

void ManyCamShiftAreaDesition::endCreateArea(int x, int y)
{
tmpArea->selection.x = MIN(x, origin.x);
tmpArea->selection.y = MIN(y, origin.y);
tmpArea->selection.width = std::abs(x - origin.x);
tmpArea->selection.height = std::abs(y - origin.y);
tmpArea->selection &= Rect(0, 0, image.cols, image.rows);
if( tmpArea->selection.width > 0 && tmpArea->selection.height > 0 ){
tmpArea->trackObject = -1;
tmpArea->beforeTime = cap.get(CV_CAP_PROP_POS_MSEC );
char hoge[256];
sprintf(hoge,"ManChart%d.csv",areas.size());
tmpArea->csvFile = fopen(hoge,"w");
targetArea = 0;
if (areas.size() == 0){
for (int i=0;i<this->tmpPoints.size();i++){
tmpArea->points.push_back(this->tmpPoints[i]);
}
}
areas.push_back(tmpArea);
}else{
delete tmpArea;
}
}

void ManyCamShiftAreaDesition::timeShift(int x, int y)
{
if(targetArea > -1 && targetArea < areas.size()){
int num = areas[targetArea]->getTimePoint(x,y);
if (num > 0)cap.set(CV_CAP_PROP_POS_FRAMES ,(double)num);
}
}

void ManyCamShiftAreaDesition::createWork(int x,int y)
{
areas[targetArea]->points.push_back(cvPoint(x,y));
}

void ManyCamShiftAreaDesition::mouseEvent (int event, int x, int y, int flags, void *param)
{
    switch( event )
    {
case CV_EVENT_LBUTTONDOWN:
this->startCreateArea(x,y);
break;
case CV_EVENT_LBUTTONUP:
this->endCreateArea(x,y);
   break;
case CV_EVENT_RBUTTONUP:
if (flags & CV_EVENT_FLAG_ALTKEY){
this->timeShift(x,y);
}else{
this->createWork(x,y);
}
   break;
    }
}

void ManyCamShiftAreaDesition::keyEvent(char c)
{
switch(c){
case 'b':
backprojMode = !backprojMode;
break;
        case 'h':
            showHist = !showHist;
            if( !showHist )
                destroyWindow( "Histogram" );
            else
                namedWindow( "Histogram", 1 );
            break;
        case 'p':
            paused = !paused;
            break;
case 'n': //←
            targetArea--;
if (targetArea == -1){
targetArea = areas.size()-1;
}
            break;
case 'm': //⇒
            targetArea++;
if (areas.size() == targetArea){
targetArea = 0;
}
            break;
case 's':
            cv::imwrite("hoge.jpg",image);
cv::imwrite("positionData.jpg",positionData);
            break;
case 'q':
cv::imwrite("positionData.jpg",positionData);
            break;
case 'z':
areas.pop_back();
            break;
        default:
            break;
}
}

void ManyCamShiftAreaDesition::drawNowPoint(Mat graph,int time)
{
cv::line(graph,cvPoint(time%1200,(int)(time/1200) *10*10+ 0),cvPoint(time%1200,(int)(time/1200) *10*10+ 100),Scalar(255,255,255) , 1, CV_AA, 0);
}

void ManyCamShiftAreaDesition::init(void)
{
FILE *initfp;
initfp = fopen("ini/CamShift.txt", "r");
char fileName[256];
int mode;
fscanf(initfp,"%d",&mode);
if (mode == 1){
fscanf(initfp,"%s",&fileName);
fscanf(initfp,"%d",&speed);
this->cap.open(fileName);
int areaNum;
fscanf(initfp,"%d",&areaNum);
for(int i=0;i<areaNum;i++){
for(int j=0;j<4;j++){
int x,y;
fscanf(initfp,"%d",&x);
fscanf(initfp,"%d",&y);
this->tmpPoints.push_back(cvPoint(x,y));
}
}
}else{
cap.open(0);
}

cvNamedWindow( "Capture", 1 );
cvSetMouseCallback ("Capture", on_mouse);
}

void ManyCamShiftAreaDesition::mainloop(void)
{
    Rect trackWindow;
    int hsize = 16;
    float hranges[] = {0,180};
    const float* phranges = hranges;
this->waitSprrd = 10;

namedWindow( "Histogram", 0 );
    createTrackbar( "Vmin", "Capture", &vmin, 256, 0 );
    createTrackbar( "Vmax", "Capture", &vmax, 256, 0 );
    createTrackbar( "Smin", "Capture", &smin, 256, 0 );
    createTrackbar( "SkipSpeed", "Capture", &speed, 10, 0 );
createTrackbar( "WaitSpeed", "Capture", &this->waitSprrd, 50, 0 );

    Mat frame, histimg = Mat::zeros(200, 320, CV_8UC3),backproj,hist;
    paused = true;
cap >> frame;
frame.copyTo(positionData);
speed = 1;
   
for(;;)
    {
        if( !paused )
        {
cap >> frame;
for(int i=0;i<speed;i++){
cap >> frame;
}
            if( frame.empty() ){
                break;
}
        }

        frame.copyTo(image);
int nowNum = cap.get(CV_CAP_PROP_POS_FRAMES );
int nowTime = cap.get(CV_CAP_PROP_POS_MSEC );

if( !paused ){
if( caluculatedNum < nowNum){
for (int i=0;i<this->areas.size();i++){
areas[i]->calucurateCamShit(image,nowNum,nowTime,vmin,vmax,smin,backprojMode);
}
caluculatedNum = nowNum;
}
}
if( this->areas.size() > 0 ){
            paused = false;
}
       
     

for (int i=0;i<this->areas.size();i++) {
if (targetArea == i){
areas[i]->drawLines(image,convertSmallNum2Scalar(i),true);
areas[i]->drawArea(image);

Mat showGraph = areas[i]->graph.clone();
this->drawNowPoint(showGraph,nowNum);

imshow( "Graph", showGraph );
}else{
areas[i]->drawLines(image,convertSmallNum2Scalar(i),false);
}
areas[i]->drawTarget(image,convertSmallNum2Scalar(i),nowNum);

}

        imshow( "Capture", image );
        imshow( "Histogram", histimg );
if (this->waitSprrd == 0)this->waitSprrd=1;

char c = (char)waitKey(this->waitSprrd);
if( c == 27 ){
cv::imwrite("positionData.jpg",positionData);
for (int i=0;i<this->areas.size();i++) {
char hoge[256];
sprintf(hoge,"metyo%d.jpg",i);
cv::imwrite(hoge,this->areas[i]->positionData);
fclose(areas[i]->csvFile);
}
break;
}
this->keyEvent(c);
    }
}

void ManyCamShiftAreaDesition::term(void)
{
}