开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 图形图像 > OpenCV > 查看源码
HumanMotion.cpp
资源名称：HumanMotionTrack.rar　[点击查看]
上传用户：jsylhbnbhn
上传日期：2013-11-03
资源大小：119k
文件大小：19k
源码类别：

OpenCV

开发平台：

Visual C++

HumanMotion.cpp：源码内容
  1. // HumanMotion.cpp: implementation of the HumanMotion class.
  2. //
  3. //////////////////////////////////////////////////////////////////////
  7. #ifdef _CH_
  8. #pragma package <opencv>
  9. #endif
  12. #ifndef _EiC
  13. #include "cv.h"
  14. #include "highgui.h"
  15. #include <stdio.h>
  16. #include <ctype.h>
  17. #include "skelecton.h"
  18. #include "skeletons.h"
  19. #include "kalman.h"
  20. #include "OpenGL.h"
  21. #include "stdafx.h"
  22. #include "HumanMotion.h"
  23. #endif
  26. IplImage *image = 0, *grey = 0, *prev_grey = 0,
  27.  *pyramid = 0, *prev_pyramid = 0,*motion_track_img = 0, *swap_temp;
  29. IplImage* background_image = 0;
  31. int win_size = 10;
  32. const int MAX_COUNT = 13;  //骨架节点数
  33. CvPoint2D32f* points[2] = {0,0}, *swap_points;
  34. char* status = 0;
  35. int count = 0;
  36. int need_to_init = 1;
  37. int night_mode = 0;
  38. int flags = 0;
  39. int add_remove_pt = 0;
  40. int pause = 0;
  41. bool init_OK = false;
  43. CvPoint pt;
  44. CvSize image_sz;
  45. OpenGL *pgl;
  49. //五个U形点
  50. U_Joint ujoints[5];
  51. int ujoints_count = 0;
  53. //三个n形点，即手臂与身体两个，裆部一个
  54. U_Joint njoints[3];
  55. int njoints_count = 0;
  57. const float pi = 3.1415926f;
  59. CvMemStorage* storage = 0; // temporary storage
  61. IplImage* abs_image = 0;
  62. IplImage* add_abs_image = 0;
  63. IplImage* add_abs_image2 = 0;
  65. IplImage* dst = 0;
  66. IplImage* src = 0;
  67. CvSeq* contour = 0;
  68. IplConvKernel* element = 0;
  71. int frame_count =0;
  74. void on_mouse( int event, int x, int y, int flags, void *param )
  75. {
  77. if( !image )
  78.         return;
  80.     if( image->origin )
  81.         y = image->height - y;
  83.     if( event == CV_EVENT_LBUTTONDOWN )
  84.     {
  85.         pt = cvPoint(x,y);
  86.         add_remove_pt = 1;
  87.     } 
  88. }
  90. HumanMotion::HumanMotion()
  91. {
  93. }
  95. HumanMotion::~HumanMotion()
  96. {
  98. }
  100. int HumanMotion::Start()
  101. {
  103. main_loop(1);
  104. return 0;
  105. }
  107. int HumanMotion::main_loop( int useCAM)
  108. {
  111. skelecton *ske = new skelecton();
  112. skelectons *skes = new skelectons();
  113. kalman *bone_kalmans = new kalman[MAX_COUNT];
  115. CvCapture* capture = 0;
  116.     
  117.     if( useCAM == 1)
  118. capture = cvCaptureFromFile("e:\mse\movie\capture4.avi");
  120.     else if( useCAM == 2 )
  121.         capture = cvCaptureFromCAM(-1);
  123.     if( !capture )
  124.     {
  125.         fprintf(stderr,"Could not initialize capturing...n");
  126.         return -1;
  127.     }
  129.     printf( "Hot keys: n"
  130.             "tESC - quit the programn"
  131.             "tr - auto-initialize trackingn"
  132.             "tc - delete all the pointsn"
  133.             "tn - switch the "night" mode on/offn"
  134. "tp - pause on/offn"
  135.             "n" );
  137.     cvNamedWindow( "LkDemo", 1 );
  138. cvNamedWindow( "Contour", 1 );
  139.     cvSetMouseCallback( "LkDemo", on_mouse, 0 );
  141.     for(;;)
  142.     {
  143.         IplImage* frame = 0;
  145.         int i, k, c;
  147.         if(pause)
  148. frame = image ;
  149. else 
  150. frame = cvQueryFrame( capture );
  152.         if( !frame ) break;
  154.         if( !image )
  155.         {
  156.             /* allocate all the buffers */
  157. image_sz = cvGetSize(frame) ;
  158.             image = cvCreateImage(image_sz , 8, 3 );
  159.             image->origin = frame->origin;
  161. motion_track_img = cvCreateImage( image_sz, 8, 3 );
  162. motion_track_img->origin = frame->origin; 
  163.             grey = cvCreateImage( image_sz, 8, 1 );
  164.             prev_grey = cvCreateImage( image_sz, 8, 1 );
  165.             pyramid = cvCreateImage( image_sz, 8, 1 );
  166.             prev_pyramid = cvCreateImage( image_sz, 8, 1 );
  167.             points[0] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
  168.             points[1] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0]));
  169.             status = (char*)cvAlloc(MAX_COUNT);
  170.             flags = 0;
  172. CvSize sz = cvSize( image->width/2 & -1, image->height/2 & -1 );
  173. CvSize sz2 = cvSize( sz.width/2 & -1, sz.height/2 & -1 );
  174. abs_image = cvCreateImage( cvGetSize(image), 8, 1 );
  175. add_abs_image =cvCreateImage(sz, 8, 1 ); 
  176. add_abs_image2 =cvCreateImage(sz2, 8, 1 ); 
  177. dst = cvCreateImage( image_sz, 8, 3 );
  178. dst->origin = frame->origin;
  180. background_image = cvCreateImage( image_sz, 8, 1 );
  181. background_image->origin = frame->origin;
  183. storage = cvCreateMemStorage(0);
  185.         }
  187.         cvCopy( frame, image, 0 );
  188.         cvCvtColor( image, grey, CV_BGR2GRAY );
  189. cvZero( motion_track_img );
  191.         if( night_mode )
  192.             cvZero( image );
  193.         
  194.         if( need_to_init )
  195.         {
  196. //分析轮廓，开始自动标定
  197. analyze_contour(grey);
  198. if(init_OK) 
  199. {
  200. count = MAX_COUNT;
  201. cvFindCornerSubPix( grey, points[1], count,
  202. cvSize(win_size,win_size), cvSize(-1,-1),
  203.   cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
  205. for(int s=0;s<count;s++) 
  206. { 
  207.  ske->CalcLengthRatio (s+1,points[1][s]);
  208.  bone_kalmans[s].init_kalman (points[1][s].x,0,points[1][s].y,0);
  209. }
  210. }
  212.         }
  213.         else if( count > 0 )
  214.         {
  215.             cvCalcOpticalFlowPyrLK( prev_grey, grey, prev_pyramid, pyramid,
  216.                 points[0], points[1], count, cvSize(win_size,win_size), 3, status, 0,
  217.                 cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), flags );
  218.             flags |= CV_LKFLOW_PYR_A_READY;
  219.             for( i = k = 0; i < count; i++ )
  220.             {
  221.                 if( add_remove_pt )
  222.                 {
  223.                     double dx = pt.x - points[1][i].x;
  224.                     double dy = pt.y - points[1][i].y;
  226.                     if( dx*dx + dy*dy <= 25 )
  227.                     {
  228.                         add_remove_pt = 0;
  229.                         continue;
  230.                     }
  231.                 }
  232.                 
  233.                 if( !status[i] )
  234.                     continue;
  235.                 
  236.                 points[1][k++] = points[1][i];
  237. ske->CorrectPosition (i+1,points[1][i],bone_kalmans[i],grey);
  238.                 cvCircle( image, cvPointFrom32f(points[1][i]), 3, CV_RGB(0,255,0), -1, 8,0);
  239.             }
  240.             count = k;
  241.         }
  243. //
  244. skelecton *pske;
  245. pske = skes->get_previous_skelecton ();
  246. if(pske!=NULL)  ske->set_previous_skelecton (pske);
  247. ske->pgl = pgl;
  248. ske->drawOpenGL3D(image); 
  249. //ske->draw2D(image); 
  250. skes->add (ske);  //to do list 不需要每次都保存，应该隔单位时间保存一次
  253.         if( add_remove_pt && count < MAX_COUNT )
  254.         {
  255.             points[1][count++] = cvPointTo32f(pt);
  256.             cvFindCornerSubPix( grey, points[1] + count - 1, 1,
  257.                 cvSize(win_size,win_size), cvSize(-1,-1),
  258.                 cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
  259.             add_remove_pt = 0;
  260. bone_kalmans[count-1].init_kalman (points[1][count-1].x,0,points[1][count-1].y,0);
  261. ske->CalcLengthRatio (count,points[1][count-1]);
  262.         }
  264.         CV_SWAP( prev_grey, grey, swap_temp );
  265.         CV_SWAP( prev_pyramid, pyramid, swap_temp );
  266.         CV_SWAP( points[0], points[1], swap_points );
  267.         //need_to_init = 0;
  268.         cvShowImage( "LkDemo", image );
  269. //cvShowImage( "MotionTrackDemo", motion_track_img );
  271.         c = cvWaitKey(10);
  272. //printf("%c,%d",c,c);
  273.         if( c == 27 )
  274.             break;
  275.         switch( c )
  276.         {
  277.         case 'r':
  278.             need_to_init = 1;
  279.             break;
  280.         case 'c':
  281.             count = 0;
  282. ske->reset();
  283.             break;
  284.         case 'n':
  285.             night_mode ^= 1;
  286.             break;
  287. case 'p':
  288.             pause ^= 1;
  289.             break;
  290. case 'o':            
  291. if(ske->ske_pause)  ske->ske_pause= false;
  292. else ske->ske_pause= true;
  293.             break;
  294. case 'i':            
  295. ske->view_up();
  296. break;
  297. case 'k':            
  298. ske->view_down();
  299. break;
  300. case 'j':            
  301. ske->view_left();
  302. break;
  303. case 'l':            
  304. ske->view_right();
  305. break;
  306.         default:
  307.             ;
  308.         }
  309. frame_count++;
  311.     }
  313.     cvReleaseCapture( &capture );
  314.     cvDestroyWindow("LkDemo");
  315. cvDestroyWindow("Contour");
  318. delete ske;
  319. ske = NULL;
  320. delete skes;
  321. skes = NULL;
  322. delete []bone_kalmans;
  324.     return 0;
  325. }
  328. int HumanMotion::analyze_contour(IplImage* grey_image)
  329. {
  332. getContour(grey_image);
  333. cvShowImage( "Contour", dst );
  334. return 0;
  335. }
  338. int HumanMotion::getContour( IplImage* grey)
  339. {
  340. double d_count = frame_count;
  342. cvAddWeighted(background_image, d_count/(d_count+1),
  343.   grey, 1/(d_count+1),
  344.                       0,background_image);
  346. cvAbsDiff( grey,background_image, abs_image );
  347.         cvThreshold( abs_image, abs_image, 50, 255, CV_THRESH_BINARY );
  349. //cvPyrDown(abs_image, add_abs_image, 7);
  350. //cvPyrDown(add_abs_image, add_abs_image2, 7);
  351. //cvCanny(add_abs_image2, add_abs_image2, 20.0, 20.0*3, 3);
  355. int pos = 4;
  356. const int element_shape = CV_SHAPE_RECT;
  357. element = cvCreateStructuringElementEx( pos*2+1, pos*2+1, pos, pos, element_shape, 0 );
  358. cvDilate(abs_image,abs_image,element,1);
  359. cvErode(abs_image,abs_image,element,1);
  360. cvReleaseStructuringElement(&element);
  364.         cvFindContours( abs_image, storage, &contour, sizeof(CvContour),
  365. CV_RETR_TREE , CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0) );
  367. cvZero( dst );
  369. if(contour != NULL)
  370. {
  371. //int ApproxCount = image_sz.height / 30; //估算的简化度，待完善。
  372. int ApproxCount = 14; 
  374. contour = cvApproxPoly( contour, sizeof(CvContour), storage, CV_POLY_APPROX_DP,ApproxCount, 1 );
  376. CvSeq* contour2 = contour;
  378. int N = contour->total;
  381. CvSeqReader reader;
  382. int i;
  383. CvPoint pt;
  384. CvPoint pre_pt;
  386. //for (; contour != NULL; contour = contour->h_next)
  387. //{
  388. cvStartReadSeq(contour, &reader);
  389. CV_READ_SEQ_ELEM(pt, reader);
  390. pre_pt= pt;
  392. for (i = 1; i < N+1; i++)
  393. {
  395. CV_READ_SEQ_ELEM(pt, reader);
  396. cvLine( dst, pt,pre_pt,CV_RGB(0,255,0),2,CV_AA, 0 );
  397. pre_pt= pt;
  399. }
  400. //}
  402. contour = contour2;
  404. if(contour != NULL) 
  405. {
  406. //简化模型中只有8个节点
  407. if(N==8) 
  408. {
  410. // cvShowImage( "Contour", dst );
  411. // cvWaitKey(0);
  412. SegmentBody(contour);
  413. FindBody();
  414. ClearBodyJoints();
  416. }
  417. }
  419. }
  421.         
  422. return 0;
  424. }
  426. int HumanMotion::SegmentBody(CvSeq* contour)
  427. {
  428. const float StandBodyLength = 1.0;
  429. const float StandArc = 90;
  430. const float nStandArc = 90;
  431. float l1,l2=0;
  432. CvPoint pt1,pt2;
  433. double total_arc = 0; //内旋角度
  434. double rarc12= 0;
  435. double arc12= 0;
  436. bool Haslkvec = false; //是否存在先前记录的矢量
  437. bool HasFoundedJoint = false; //是否找到拐点
  440. CvSeqReader reader;
  441. CvSeqReader subReader;
  442. int N = contour->total;
  443. int i;
  444. CvPoint pt;
  445. CvPoint pre_pt;
  446. CvPoint pre_pre_pt;
  448. ujoints_count=0;
  449. njoints_count=0;
  452. cvStartReadSeq(contour, &reader);
  454. for (i= 0; i < N; i++)
  455. {
  456. CV_READ_SEQ_ELEM(pt, reader);
  457. pre_pt = pre_pre_pt = pt;
  458. //printf("in sub %d,%d n",pt.x,pt.y);
  459.  
  460. //计算两点之间的距离
  461. subReader = reader ;
  463. Haslkvec = false;
  464. total_arc =0;
  467. for(int j=0;j<3;j++)
  468. {
  470. l1=calc_2Point_Magnitude(pt,pre_pt);
  471. if (l1>StandBodyLength)
  472. {
  473. //是否存在先前记录的点，如果有，检查角度范围
  474. if(Haslkvec)
  475. {
  477. Vector3f vt1 =  Vector3f (pt2.x-pt1.x,pt2.y-pt1.y,0);
  478. Vector3f vt2 =  Vector3f (pt1.x-pt.x,pt1.y-pt.y,0);
  479. arc12 = AngleBetweenVectors(vt1,vt2);
  480. rarc12 = 180*arc12/pi;
  481. Vector3f vtnor=Cross(vt1,vt2); //使用叉集检查矢量是内旋还是外旋。
  482. if(vtnor.z <0) total_arc -= rarc12; 
  483. else total_arc += rarc12; 
  485. //检查角度范围合理U -200 -160
  486. if (abs(total_arc+180)<StandArc)  
  487. {
  489. //绘出U形的位置
  490. //draw_2Points(pt,pre_pt,pt1,pt2,CV_RGB(255,255,250));
  491. save_Points(pt,pre_pt,pt1,pt2,U_TYPE);
  492. HasFoundedJoint = true ;
  494. }
  496. //检查角度范围N 150 210
  497. if (abs(total_arc-180)<nStandArc)  
  498. {
  499. //记录n形点的位置
  500. //draw_2Points(pt,pre_pt,pt1,pt2,CV_RGB(230,255,0));
  501. save_Points(pt,pre_pt,pt1,pt2,N_TYPE);
  502. HasFoundedJoint = true ;
  504. }
  507. if(HasFoundedJoint)
  508. {
  510. //记录两个矢量，清空标记
  511. Haslkvec = false;
  512. total_arc = 0;
  513. HasFoundedJoint = false;
  514. break;
  515. }
  518. }
  519. else
  520. {
  521. //记录两个点
  522. pt1=pt;
  523. pt2=pre_pt;
  524. Haslkvec = true;
  525. }
  527. }
  530. pre_pre_pt = pre_pt;
  531. pre_pt= pt;
  532. CV_READ_SEQ_ELEM(pt, subReader);
  535. }
  537. }
  540. return 0;
  541. }
  545. float HumanMotion::calc_2Point_Magnitude(CvPoint pt1,CvPoint pt2)
  546. {
  547. return (float)sqrt( pow((pt1.x-pt2.x),2) + pow((pt1.y-pt2.y),2) );
  549. }
  551. int HumanMotion::draw_2Points(CvPoint pt1,CvPoint pt2,CvPoint pt3,CvPoint pt4,CvScalar color)
  552. {
  555. cvLine( dst,pt1,pt2,color,1, CV_AA, 0);
  556. cvLine( dst,pt3,pt4,color,1, CV_AA, 0);
  557. cvLine( dst,pt1,pt4,color,1, CV_AA, 0);
  558. cvLine( dst,pt2,pt3,color,1, CV_AA, 0);
  560. return 0;
  561. }
  564. //检查平行线之间构成四边形是否在同一轴线上，正方形相似度判断
  565. //方法如下，取两个矢量中心点连线，取连线与较大矢量的夹角，如果夹角在90度偏差20度内认为是正方形。
  566. bool HumanMotion::Is_Closed_Vectors(CvPoint pt1,CvPoint pt2,CvPoint pt3,CvPoint pt4)
  567. {
  569. CvPoint pt5,pt6;
  570. const float StandSqArc = 40;
  571. pt5.x= (pt1.x + pt2.x ) /2;
  572. pt5.y= (pt1.y + pt2.y ) /2;
  573. pt6.x= (pt3.x + pt4.x ) /2;
  574. pt6.y= (pt3.y + pt4.y ) /2;
  576. Vector3f vt1 = Vector3f (pt1.x-pt2.x,pt1.y-pt2.y,0);
  577. Vector3f vt2 = Vector3f (pt3.x-pt4.x,pt3.y-pt4.y,0);
  578. Vector3f vtc = Vector3f (pt6.x-pt5.x,pt6.y-pt5.y,0);
  580. Vector3f vtmax;
  581. if(Magnitude(vt1) > Magnitude(vt2)) 
  582. vtmax= vt1;
  583. else
  584. vtmax= vt2;
  586. double arc12 = AngleBetweenVectors(vtc,vtmax);
  587. double rarc12 = 180*arc12/pi;
  588. if (abs(abs(rarc12)-90)<StandSqArc) 
  589. return true;
  590. else
  591. return false;
  592. }
  594. //排除的第二种情况
  595. //1.两矢量长度不等，差距相差4倍时，认为无效
  596. //2.两矢量错位，不协调，即对角线长度大于两矢量长度的两倍。
  597. bool HumanMotion::Is_Apar_Vectors(CvPoint pt1,CvPoint pt2,CvPoint pt3,CvPoint pt4)
  598. {
  599. bool result = false;
  600. const float StandLenRatio = 4.0;  
  601. const float StandCrossLenRatio = 2.0;  
  603. Vector3f vt1 = Vector3f (pt1.x-pt2.x,pt1.y-pt2.y,0);
  604. Vector3f vt2 = Vector3f (pt3.x-pt4.x,pt3.y-pt4.y,0);
  607. //1.两矢量长度不等，差距相差4倍时，认为无效
  608. double lenvct1,lenvct2,raido12;
  609. lenvct1 = Magnitude(vt1);
  610. lenvct2 = Magnitude(vt2);
  611. raido12 = lenvct1/lenvct2;
  612. if( raido12<StandLenRatio && raido12 > 1/StandLenRatio )
  613. result = true ;
  615. //2.两矢量错位，不协调，即对角线长度大于两矢量长度的两倍。
  616. if(result)
  617. {
  618. Vector3f vtc1 = Vector3f (pt1.x-pt3.x,pt1.y-pt3.y,0); //对角线1
  619. Vector3f vtc2 = Vector3f (pt1.x-pt3.x,pt1.y-pt3.y,0); //对角线2
  620. double c1,c2,ratio12;
  621. double vct_min,cross_vct_max;
  622. c1 = Magnitude(vtc1);
  623. c2 = Magnitude(vtc2);
  624. if (lenvct1>lenvct2) 
  625. vct_min = lenvct2; 
  626. else
  627. vct_min = lenvct1; 
  628. if(c1>c2)
  629. cross_vct_max = c1;
  630. else
  631. cross_vct_max = c2;
  632. ratio12 = cross_vct_max/vct_min;
  633. if (ratio12<StandCrossLenRatio) 
  634. result = true ;
  635. else
  636. result = false;
  637. }
  639. return result;
  640. }
  642. int HumanMotion::FindBody()
  643. {
  644. init_OK = false;
  645. if(ujoints_count==5 && njoints_count==3)
  646. {
  650. CvPoint pt1,pt2,pt3,pthead;
  651. //右腋下
  652. pt1 = njoints[0].pt;
  654. //裆部
  655. pt2 = njoints[1].pt;
  657. //左腋下
  658. pt3 = njoints[2].pt;
  660. //寻找head 节点
  661. int Head_i;
  662. int temp = 0;
  663. for(int i= 0; i<5 ;i++)
  664. {
  665. pthead.y = ujoints[i].pt.y ;
  666. if (pthead.y>temp) Head_i =i;
  667. }
  668. pthead=ujoints[Head_i].pt ;
  671. CvPoint pt_neck;
  672. pt_neck.x = ( pthead.x - pt2.x )*0.75 + pt2.x; 
  673. pt_neck.y = ( pthead.y - pt2.y )*0.75 + pt2.y;
  675. CvPoint pt_waist;
  676. pt_waist.x = ( pthead.x - pt2.x )*0.3 + pt2.x; 
  677. pt_waist.y = ( pthead.y - pt2.y )*0.3 + pt2.y;
  679. CvPoint pt_lhand;
  680. pt_lhand = ujoints[3].pt ;
  682. CvPoint pt_rhand;
  683. pt_rhand = ujoints[0].pt ;
  687. CvPoint pt_rAnkle;
  688. pt_rAnkle = ujoints[1].pt ;
  690. CvPoint pt_lAnkle;
  691. pt_lAnkle = ujoints[2].pt ;
  693. CvPoint pt_rknee;
  694. pt_rknee.x  =( pt_waist.x +  pt_rAnkle.x ) /2;
  695. pt_rknee.y  =( pt_waist.y +  pt_rAnkle.y ) /2;
  697. CvPoint pt_lknee;
  698. pt_lknee.x  =( pt_waist.x +  pt_lAnkle.x ) /2;
  699. pt_lknee.y  =( pt_waist.y +  pt_lAnkle.y ) /2;
  701. CvPoint pt_lshoulder;
  702. //pt_lshoulder.x = ( pthead.x - pt_lhand.x )*(pt_neck.y - pt_lhand.y)/(pthead.y - pt_lhand.y) + pt_lhand.x; 
  703. //pt_lshoulder.y = pt_neck.y;
  705. CvPoint pt_rshoulder;
  706. //pt_rshoulder.x =( pthead.x - pt_rhand.x )*(pt_neck.y - pt_rhand.y)/(pthead.y - pt_rhand.y) + pt_rhand.x; 
  707. //pt_rshoulder.y = pt_neck.y;
  709. CvPoint pt_relbow;
  710. pt_relbow.x  =( pt_rshoulder.x +  pt_rhand.x ) /2;
  711. pt_relbow.y  =( pt_rshoulder.y +  pt_rhand.y ) /2;
  713. CvPoint pt_lelbow;
  714. pt_lelbow.x  =( pt_lshoulder.x +  pt_lhand.x ) /2;
  715. pt_lelbow.y  =( pt_lshoulder.y +  pt_lhand.y ) /2;
  719. ZoomLine(pthead,pt_neck);
  721. ZoomLine(pt_neck,pt_waist);
  722. ZoomLine(pt_neck,pt_rshoulder);
  723. ZoomLine(pt_neck,pt_lshoulder);
  724. ZoomLine(pt_rshoulder,pt_relbow);
  725. ZoomLine(pt_lshoulder,pt_lelbow);
  726. ZoomLine(pt_relbow,pt_rhand);
  727. ZoomLine(pt_lelbow,pt_lhand);
  728. ZoomLine(pt_waist,pt_rAnkle);
  729. ZoomLine(pt_waist,pt_lAnkle);
  731. points[1][0] = Point_to_32f(pt_neck) ;
  732. points[1][1] = Point_to_32f(pthead) ;
  733. points[1][2] = Point_to_32f(pt_rshoulder) ;
  734. points[1][3] = Point_to_32f(pt_lshoulder) ;
  735. points[1][4] = Point_to_32f(pt_relbow) ;
  736. points[1][5] = Point_to_32f(pt_lelbow) ;
  737. points[1][6] = Point_to_32f(pt_rhand) ;
  738. points[1][7] = Point_to_32f(pt_lhand) ;
  739. points[1][8] = Point_to_32f(pt_waist) ;
  740. points[1][9] = Point_to_32f(pt_rknee) ;
  741. points[1][10] = Point_to_32f(pt_lknee) ;
  742. points[1][11] = Point_to_32f(pt_rAnkle) ;
  743. points[1][12] = Point_to_32f(pt_lAnkle) ;
  746. need_to_init = 0 ;
  747. init_OK = true ;
  748. cvShowImage( "LkDemo", image );
  749. cvWaitKey(0);
  752. }
  754. return 0;
  755. }
  757. int  HumanMotion::ClearBodyJoints()
  758. {
  759. for(int i= 0; i<5 ;i++) ujoints[i].HasSet = false;
  760. for(i= 0; i<3 ;i++) njoints[i].HasSet = false;
  761. ujoints_count = 0;
  762. njoints_count = 0;
  763. return 0;
  764. }
  765. int HumanMotion::save_Points(CvPoint pt1,CvPoint pt2,CvPoint pt3,CvPoint pt4,JointType upoint_type)
  766. {
  767. if(upoint_type == U_TYPE)
  768. {
  769. ujoints_count = ujoints_count % 5;
  770. ujoints[ujoints_count].next_pt = pt1;
  771. ujoints[ujoints_count].pt = pt2;
  772. ujoints[ujoints_count].pre_pt = pt4;
  773. ujoints[ujoints_count].HasSet = true;
  774. ujoints_count ++;
  775. }
  777. if(upoint_type == N_TYPE)
  778. {
  779. njoints_count = njoints_count % 3;
  780. njoints[njoints_count].next_pt = pt1;
  781. njoints[njoints_count].pt = pt2;
  782. njoints[njoints_count].pre_pt = pt4;
  783. njoints[njoints_count].HasSet = true;
  784. njoints_count ++;
  785. }
  787. return 0;
  788. }
  791. int HumanMotion::ZoomLine(CvPoint pt1,CvPoint pt2)
  792. {
  793. CvPoint pts1,pts2;
  794. int zoom = 1;
  795. pts1.x = pt1.x * zoom ;
  796. pts1.y = pt1.y * zoom ;
  797. pts2.x = pt2.x * zoom ;
  798. pts2.y = pt2.y * zoom ;
  800. cvCircle( image,pts1,5, CV_RGB(0,233,250),3, CV_AA, 0 );
  801. cvCircle( image,pts2,5, CV_RGB(0,233,250),3, CV_AA, 0 );
  802. cvLine( image,pts1,pts2,CV_RGB(240,210,0),1, CV_AA, 0);
  804. cvShowImage( "LkDemo", image );
  805. cvWaitKey(0);
  808. return 0;
  810. }
  812. CvPoint2D32f HumanMotion::Point_to_32f(CvPoint pt)
  813. {
  814. CvPoint2D32f pt32;
  815. int zoom = 1;
  816. pt32.x = pt.x * zoom;
  817. pt32.y = pt.y * zoom;
  818. return pt32;
  819. }