calcOpticalFlowPyrLK and goodFeaturesToTrack doesn't work properly [closed]

edit

I trying to port some code i written in C++ non ROS into a Ros environment, which causing some problems.. The purpose of this code is to track the movement of a persons face, a bit like the pi_face_tracker The problem though, is that the opticalFlow isn't working properly.. The imshow screen doesn't say where the new points are, but just keep the old points. Which makes me suspect if it even tracking the movements of the point or not?..

here is the code:

#include "image_converter.h"
#include <termios.h>
int getch()
{
  static struct termios oldt, newt;
  tcgetattr( STDIN_FILENO, &oldt);           // save old settings
  newt = oldt;
  newt.c_cc[VMIN] = 0; newt.c_cc[VTIME] = 0;
  newt.c_lflag &= ~(ICANON);                 // disable buffering      
  tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings

  int c = getchar();  // read character (non-blocking)

  tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
  return c;
}


ImageConverter::ImageConverter()
    : it_(nh_),pos(2),  vel(2) 
  {
    // Subscrive to input video feed and publish output video feed

        image_sub_ = it_.subscribe("/image/left/image_rect_color", 1, 
        &ImageConverter::imageDetect, this);
        image_pub_ = it_.advertise("/image_converter/output_video", 1);
        state = false;
        reinit = false;
        redetect = false;
        frames = 0;
        cout << "start over" << endl;
  }

 ImageConverter::~ImageConverter()
  {
    //cv::destroyAllWindows();
  }

  void ImageConverter::imageDetect(const sensor_msgs::ImageConstPtr& msg)
  {  
     facedetect_pub = nh_.advertise<sensor_msgs::JointState>("/ptu/cmd", 1);
     sub = nh_.subscribe("/joint_states",1,&ImageConverter::chatterCallback,this); 
     cv_bridge::CvImagePtr cv_ptr;
     sensor_msgs::JointState ms;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    if(!(state))
    {
        vector<Rect> faces(1);
        Point center_of_frame(cv_ptr->image.size().width/2,cv_ptr->image.size().height/2);
        pair<Point, Point> corners;
        pair<double,double> displacement;
        double displacement_pixel_x;
        double displacement_pixel_y;
        pair<Rect, bool> response;

            if (cv_ptr->image.type()!= 8) {
                cvtColor(cv_ptr->image, cv_ptr->image, CV_8U);
            }

    //-- 1. Load the cascades
            if( !face_cascade.load( face_cascade_XML ) ){
                cout << "Cascade Error" << endl;
            };

            circle(cv_ptr->image, center_of_frame, 1, CV_RGB(0,255,255),8,8,0);

    //-- Detect faces
            face_cascade.detectMultiScale( cv_ptr->image, faces, 1.1, 2, 0, Size(100, 100) );
            if(faces.empty())
            {
                if( !face_cascade.load( face_cascade_XML1 ) )
                {
                    cout << "Cascade Error" << endl;
                };
                face_cascade.detectMultiScale( cv_ptr->image, faces, 1.1, 2, 0, Size(100, 100) );


                if( !face_cascade.load( nose_xml ) )
                {
                    cout << "Cascade Error" << endl;
                };
                face_cascade.detectMultiScale( cv_ptr->image, faces, 1.1, 2, 0, Size(100, 100) );

            }
            for( int i = 0; i<faces.size() ; i++)
            {
                Point center_position_of_face((faces[i].br().x+faces[i].tl().x)/2,(faces[i].tl().y+faces[i].br().y)/2);
                Point corner_1(faces[i].br().x,faces[i].tl().y);
                Point corner_2 = faces[i].tl();
                Point corner_3 = faces[i].br();
                Point corner_4(faces[i].tl().x,faces[i].br().y);
                rectangle(cv_ptr->image, faces[i], CV_RGB(0,255,0),4,8,0);
                circle(cv_ptr->image, center_position_of_face, 8, CV_RGB(128,128,128),8,8,0);
                circle(cv_ptr->image, corner_1, 1, CV_RGB(128,128,128),8,8,0);
                circle(cv_ptr->image, corner_2, 1, CV_RGB(128,128,128),8,8,0);
                circle(cv_ptr->image, corner_3, 1, CV_RGB(128,128,128),8,8,0);
                circle(cv_ptr->image, corner_4, 1, CV_RGB(128,128,128),8,8,0);
                line(cv_ptr->image, center_position_of_face ...