void blobCallback(const cmvision::Blobs::ConstPtr& msg) {

	geometry_msgs::Point output;

	//kalman prediction
	if (keepPredicting) {
		pre = kf.predict();
		output.x = pre.at<float>(0);
		output.y = pre.at<float>(1);
		output.z = pre.at<float>(2);
		pub.publish(output);
		printf("Kalman prediction: (%.3f, %.3f, %.3f)\n", output.x, output.y, output.z);
	}

	//should be an if statement, but want to break from it
	while (msg->blob_count > 0) {

		double largeArea = 0, centerX = -1, centerY = -1;

		for (int i = 0; i < msg->blob_count; i++){
					
			if(msg->blobs[i].area > largeArea && msg->blobs[i].area > IGNORE_THRESHOLD) {
				largeArea = msg->blobs[i].area;
				centerX = msg->blobs[i].x;
				centerY = msg->blobs[i].y;
			}

		}

		//if didn't find a big enough blob, pretend we saw nothing.
		if (largeArea < 1) break;
		keepPredicting = true;

		//calculate relative position -- (x,y) are wrt to the center of the camera image; z is a distance from the camera
		float x = centerX - CAMERA_CENTER_X;
		float y = CAMERA_CENTER_Y - centerY;
		float z = TARGET_SIZE / largeArea;

		printf("Blob area %.0f at (%.0f,%.0f) - correcting with (%.3f, %.3f, %.3f).\n", largeArea, centerX, centerY, x, y, z);

		//update kalman filter with new measurement
		meas.at<float>(0) = x;
		meas.at<float>(1) = y;
		meas.at<float>(2) = z;
		kf.correct(meas);

		return;
		
	}

	//no blobs, or too small to care.
	printf("No blobs.\n");
	keepPredicting = false;

}
 cv::Point updatePrediction()
 {
     cv::Mat prediction = mKF.predict();
     mPrediction = cv::Point( prediction.at<float>(0), prediction.at<float>( 1 ) );
     
     return mPrediction;
 }
Esempio n. 3
0
  int kalman_find(cv::Mat& meas,vector<cv::Rect> &ballsBox,bool &found,cv::KalmanFilter& kf,cv::Mat& state){
	meas.at<float>(0) = ballsBox[0].x + ballsBox[0].width / 2;
	meas.at<float>(1) = ballsBox[0].y + ballsBox[0].height / 2;
	meas.at<float>(2) = (float)ballsBox[0].width;
	meas.at<float>(3) = (float)ballsBox[0].height;
	if (!found) // First detection!
	  {
		// >>>> Initialization
		kf.errorCovPre.at<float>(0) = 1; // px
		kf.errorCovPre.at<float>(7) = 1; // px
		kf.errorCovPre.at<float>(14) = 1;
		kf.errorCovPre.at<float>(21) = 1;
		kf.errorCovPre.at<float>(28) = 1; // px
		kf.errorCovPre.at<float>(35) = 1; // px
		state.at<float>(0) = meas.at<float>(0);
		state.at<float>(1) = meas.at<float>(1);
		state.at<float>(2) = 0;
		state.at<float>(3) = 0;
		state.at<float>(4) = meas.at<float>(2);
		state.at<float>(5) = meas.at<float>(3);
		// <<<< Initialization
		found = true;
	  }
	else	kf.correct(meas); // Kalman Correction
  
	//cout << "Measure matrix:" << endl << meas << endl;
	return 0;
  }
void initKalmanFilter(cv::KalmanFilter &KF, int nStates, int nMeasurements, int nInputs, double dt)
{
  KF.init(nStates, nMeasurements, nInputs, CV_64F);                 // init Kalman Filter
  cv::setIdentity(KF.processNoiseCov, cv::Scalar::all(8e-6));       // set process noise
  cv::setIdentity(KF.measurementNoiseCov, cv::Scalar::all(6e-5));   // set measurement noise
  cv::setIdentity(KF.errorCovPost, cv::Scalar::all(1));             // error covariance
  /* DYNAMIC MODEL */
  //  [1 0 0 dt  0  0 dt2   0   0 0 0 0  0  0  0   0   0   0]
  //  [0 1 0  0 dt  0   0 dt2   0 0 0 0  0  0  0   0   0   0]
  //  [0 0 1  0  0 dt   0   0 dt2 0 0 0  0  0  0   0   0   0]
  //  [0 0 0  1  0  0  dt   0   0 0 0 0  0  0  0   0   0   0]
  //  [0 0 0  0  1  0   0  dt   0 0 0 0  0  0  0   0   0   0]
  //  [0 0 0  0  0  1   0   0  dt 0 0 0  0  0  0   0   0   0]
  //  [0 0 0  0  0  0   1   0   0 0 0 0  0  0  0   0   0   0]
  //  [0 0 0  0  0  0   0   1   0 0 0 0  0  0  0   0   0   0]
  //  [0 0 0  0  0  0   0   0   1 0 0 0  0  0  0   0   0   0]
  //  [0 0 0  0  0  0   0   0   0 1 0 0 dt  0  0 dt2   0   0]
  //  [0 0 0  0  0  0   0   0   0 0 1 0  0 dt  0   0 dt2   0]
  //  [0 0 0  0  0  0   0   0   0 0 0 1  0  0 dt   0   0 dt2]
  //  [0 0 0  0  0  0   0   0   0 0 0 0  1  0  0  dt   0   0]
  //  [0 0 0  0  0  0   0   0   0 0 0 0  0  1  0   0  dt   0]
  //  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  1   0   0  dt]
  //  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  0   1   0   0]
  //  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  0   0   1   0]
  //  [0 0 0  0  0  0   0   0   0 0 0 0  0  0  0   0   0   1]
  // position
  KF.transitionMatrix.at<double>(0,3) = dt;
  KF.transitionMatrix.at<double>(1,4) = dt;
  KF.transitionMatrix.at<double>(2,5) = dt;
  KF.transitionMatrix.at<double>(3,6) = dt;
  KF.transitionMatrix.at<double>(4,7) = dt;
  KF.transitionMatrix.at<double>(5,8) = dt;
  KF.transitionMatrix.at<double>(0,6) = 0.5*pow(dt,2);
  KF.transitionMatrix.at<double>(1,7) = 0.5*pow(dt,2);
  KF.transitionMatrix.at<double>(2,8) = 0.5*pow(dt,2);
  // orientation
  KF.transitionMatrix.at<double>(9,12) = dt;
  KF.transitionMatrix.at<double>(10,13) = dt;
  KF.transitionMatrix.at<double>(11,14) = dt;
  KF.transitionMatrix.at<double>(12,15) = dt;
  KF.transitionMatrix.at<double>(13,16) = dt;
  KF.transitionMatrix.at<double>(14,17) = dt;
  KF.transitionMatrix.at<double>(9,15) = 0.5*pow(dt,2);
  KF.transitionMatrix.at<double>(10,16) = 0.5*pow(dt,2);
  KF.transitionMatrix.at<double>(11,17) = 0.5*pow(dt,2);
  /* MEASUREMENT MODEL */
  //  [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
  //  [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
  //  [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
  //  [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0]
  //  [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0]
  //  [0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0]
  KF.measurementMatrix.at<double>(0,0) = 1;  // x
  KF.measurementMatrix.at<double>(1,1) = 1;  // y
  KF.measurementMatrix.at<double>(2,2) = 1;  // z
  KF.measurementMatrix.at<double>(3,9) = 1;  // roll
  KF.measurementMatrix.at<double>(4,10) = 1; // pitch
  KF.measurementMatrix.at<double>(5,11) = 1; // yaw
}
Esempio n. 5
0
cv::Mat kalmanPredict(cv::KalmanFilter &kf, cv::Mat control) //prediction = kf.predict(control);
{
	cv::Mat prediction(6, 1, CV_32F);
	prediction = kf.predict(control);
	//prediction = kf.predict();
	kf.statePre.copyTo(kf.statePost);
	kf.errorCovPre.copyTo(kf.errorCovPost);
	return prediction;
}
void updateKalmanFilter( cv::KalmanFilter &KF, cv::Mat &measurement, cv::Mat &translation_estimated, cv::Mat &rotation_estimated )
{
  // First predict, to update the internal statePre variable
  cv::Mat prediction = KF.predict();
  // The "correct" phase that is going to use the predicted value and our measurement
  cv::Mat estimated = KF.correct(measurement);

  // Estimated translation
  translation_estimated.at<double>(0) = estimated.at<double>(0);
  translation_estimated.at<double>(1) = estimated.at<double>(1);
  translation_estimated.at<double>(2) = estimated.at<double>(2);
  // Estimated euler angles
  cv::Mat eulers_estimated(3, 1, CV_64F);
  eulers_estimated.at<double>(0) = estimated.at<double>(9);
  eulers_estimated.at<double>(1) = estimated.at<double>(10);
  eulers_estimated.at<double>(2) = estimated.at<double>(11);
  // Convert estimated quaternion to rotation matrix
  rotation_estimated = euler2rot(eulers_estimated);
}
 cv::Point correct( cv::Point pt )
 {
     // update point
     mMeasurement( 0 ) = pt.x;
     mMeasurement( 1 ) = pt.y;
     
     cv::Point measPt( mMeasurement( 0 ), mMeasurement( 1 ) );
     
     // The "correct" phase that is going to use the predicted value and our measurement
     cv::Mat estimated = mKF.correct( mMeasurement );
     cv::Point statePt( estimated.at<float>( 0 ), estimated.at<float>( 1 ) );
     
     mPrediction = statePt;
     
     return mPrediction;
 }
Esempio n. 8
0
  int kalman_if_found(cv::KalmanFilter& kf,cv::Mat& state,cv::Mat& res){
	state = kf.predict();
	//  cout << "State post:" << endl << state << endl;

	cv::Rect predRect;
	predRect.width = state.at<float>(4);
	predRect.height = state.at<float>(5);
	predRect.x = state.at<float>(0) - predRect.width / 2;
	predRect.y = state.at<float>(1) - predRect.height / 2;

	cv::Point center;
	center.x = state.at<float>(0);
	center.y = state.at<float>(1);
	cv::circle(res, center, 2, CV_RGB(255,0,0), -1);
	cv::rectangle(res, predRect, CV_RGB(255,0,0), 2);

	return 0;
  }
//カルマンフィルタ
void kalmanFilter()
{
	cv::VideoCapture capture(0);

	//x,y,vx,vy
	//cv::KalmanFilter kalman(4, 2, 0);
	cv::setIdentity(kalman.measurementMatrix, cv::Scalar(1.0));
	cv::setIdentity(kalman.processNoiseCov, cv::Scalar::all(1e-5));
	cv::setIdentity(kalman.measurementNoiseCov, cv::Scalar::all(0.1));
	cv::setIdentity(kalman.errorCovPost, cv::Scalar::all(1.0));

    // 等速直線運動モデル
	kalman.transitionMatrix = *(cv::Mat_<float>(4, 4) << 1,0,1,0,   0,1,0,1,  0,0,1,0,  0,0,0,1);
	//観測値を格納するもの
	cv::Mat_<float> measurement(2,1); measurement.setTo(cv::Scalar(0));
	//初期値
	kalman.statePre.at<float>(0) = initx;
	kalman.statePre.at<float>(1) = inity;
	kalman.statePre.at<float>(2) = 0;
	kalman.statePre.at<float>(3) = 0;

	cv::namedWindow("KalmanFilter", CV_WINDOW_AUTOSIZE);
	cv::setMouseCallback("KalmanFilter", _onMouse, 0);

	// メインループ
    while (!GetAsyncKeyState(VK_ESCAPE)) {
        // 画像を取得
		capture >> capframe;
		cv::Mat image = capframe.clone();
 
        // HSVに変換
		//cv::Mat hsv = image.clone();
		//cv::cvtColor(image, hsv, CV_BGR2HSV);

		std::cout << "HSV: (" << h << ", " << s << ", " << v << ")\n";

		//クリックした点のRGBと近い画素を探索し、その重心を求める
		int sumX =0, sumY = 0, counter = 0;

		for(int y = 0; y < image.rows; y++)
			for(int x = 0; x < image.cols; x++)
			{
				//RGBの差分が閾値以内ならカウント
				if(sqrt((red - image.at<cv::Vec3b>(y,x)[2]) * (red - image.at<cv::Vec3b>(y,x)[2]) +
						  (green - image.at<cv::Vec3b>(y,x)[1]) * (green - image.at<cv::Vec3b>(y,x)[1]) +
						  (blue - image.at<cv::Vec3b>(y,x)[0]) * (blue - image.at<cv::Vec3b>(y,x)[0])) <= thresh)
				{
					//色付け
					image.at<cv::Vec3b>(y,x)[2] = 255;

					sumX += x;
					sumY += y;
					counter++;
				}
			}
 
		if(counter > 0)
		{
			//観測値
			int mx = (int)(sumX/counter);
			int my = (int)(sumY/counter);
			measurement(0) = mx;
			measurement(1) = my;
			cv::Point measPt(measurement(0), measurement(1));
			//表示
			cv::circle(image, measPt, 10, cv::Scalar(0, 0, 255));

			//修正フェーズ
			cv::Mat estimated = kalman.correct(measurement);
			cv::Point statePt(estimated.at<float>(0),estimated.at<float>(1));

        }

		//予測フェーズ
		cv::Mat prediction = kalman.predict();
		cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
		// 表示
		cv::circle(image, predictPt, 10, cv::Scalar(0, 255, 0));
		cv::circle(image, cv::Point(20, 20), 10, CV_RGB(red, green, blue), 5);
		cv::putText(image,"target", cv::Point(0, 50), cv::FONT_HERSHEY_SIMPLEX, 0.7, CV_RGB(red, green, blue));
  
		std::cout << (int)red << ", " << (int)green << ", " <<  (int)blue << std::endl;
		cv::imshow("KalmanFilter", image);
        cvWaitKey(1);
 
    }
}
Esempio n. 10
0
void detectionsCallback(const hannrs_msgs::VisionDetection::ConstPtr& msg){
  if(msg->positions.size() == 1){
    if(people.empty()){
      if(msg->positions.size() == 1){
// 	double dist = sqrt((msg->positions[0].x - robot.back().position.x)*(msg->positions[0].x - robot.back().position.x) + 
// 	(msg->positions[0].y - robot.back().position.y)*(msg->positions[0].y - robot.back().position.y));
// 	if(dist < 0.35){
// 	}else{
	  KF = cv::KalmanFilter(4,2,0,CV_64F);
	  KF.transitionMatrix = *(cv::Mat_<double>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1);
	  // init...
	  KF.statePre.at<double>(0) = msg->positions[0].x;
	  KF.statePre.at<double>(1) = msg->positions[0].y;
	  KF.statePre.at<double>(2) = 0;
	  KF.statePre.at<double>(3) = 0;
  // 	setIdentity(KF.measurementMatrix);
	  KF.measurementMatrix = *(cv::Mat_<double>(2, 4) << 1,0,0,0, 0,1,0,0);
	  setIdentity(KF.processNoiseCov, cv::Scalar::all(0.25));
	  setIdentity(KF.measurementNoiseCov, cv::Scalar::all(0.05));
	  setIdentity(KF.errorCovPost, cv::Scalar::all(.1));
	  	  
	  hannrs_msgs::Person person;
	  person.pose.position = msg->positions[0];
	  person.pose.orientation.w = 1.0;
	  person.status = "standing"; //person cannot enter the scene sitted
	  person.name = "Andre";
	  people.push_back(person);
	  headers.push_back(msg->header);
// 	}
      }
    }else{
      cv::Mat_<double> measurement(2,1); measurement.setTo(cv::Scalar(0));
      double dt = msg->header.stamp.toSec() - headers.back().stamp.toSec();
      
      KF.transitionMatrix = *(cv::Mat_<double>(4, 4) << 1,0,dt,0, 0,1,0,dt, 0,0,1,0, 0,0,0,1);
      
      // First predict, to update the internal statePre variable
      cv::Mat prediction = KF.predict();

      double vel_module;
      hannrs_msgs::Person person;
      if(msg->positions.size() == 1){
// 	double dist = sqrt((msg->positions[0].x - robot.back().position.x)*(msg->positions[0].x - robot.back().position.x) + 
// 	(msg->positions[0].y - robot.back().position.y)*(msg->positions[0].y - robot.back().position.y));
// 	
// 	if(dist < 0.35){
// 	}else{
	  // Get detection
	  measurement(0) = msg->positions[0].x;
	  measurement(1) = msg->positions[0].y;
		      
	  cv::Point measPt(measurement(0),measurement(1));
	  
	  // The "correct" phase that is going to use the predicted value and our measurement
	  cv::Mat estimated = KF.correct(measurement);
	  
	  
	  person.pose.position.x = estimated.at<double>(0);
	  person.pose.position.y = estimated.at<double>(1);
	  if(msg->classifications[0] == 2.0){
	    person.pose.orientation = tf::createQuaternionMsgFromYaw(msg->positions[0].z);
	    person.velocity.linear.x = 0;
	    person.velocity.linear.y = 0;
	    vel_module = -1;
	  }else{
	    person.pose.orientation = tf::createQuaternionMsgFromYaw(atan2(estimated.at<double>(3), estimated.at<double>(2)));
	    person.velocity.linear.x = estimated.at<double>(2);
	    person.velocity.linear.y = estimated.at<double>(3);
	    vel_module = sqrt(estimated.at<double>(2)*estimated.at<double>(2) + estimated.at<double>(3)*estimated.at<double>(3));
	  }
// 	}
      }else{
	person.pose.position.x = prediction.at<double>(0);
	person.pose.position.y = prediction.at<double>(1);
	if(people.back().status.compare("sitting") == 0){
	  person.pose.orientation = people.back().pose.orientation;
	  person.velocity.linear.x = 0;
	  person.velocity.linear.y = 0;
	  vel_module = -1;
	}else{
	  person.pose.orientation = tf::createQuaternionMsgFromYaw(atan2(prediction.at<double>(3), prediction.at<double>(2)));
	  person.velocity.linear.x = prediction.at<double>(2);
	  person.velocity.linear.y = prediction.at<double>(3);
	  vel_module = sqrt(prediction.at<double>(2)*prediction.at<double>(2) + prediction.at<double>(3)*prediction.at<double>(3));
	}
      }
      
      if(vel_module == -1){
	person.velocity.linear.x = 0;
	person.velocity.linear.y = 0;
	person.status = "sitting"; //person cannot enter the scene sitted
      }else if(vel_module < 0.15){
	person.velocity.linear.x = 0;
	person.velocity.linear.y = 0;
	person.status = "standing"; //person cannot enter the scene sitted
      }else{
	person.status = "walking";
      }
      
      person.name = "Andre";
      people.push_back(person);
      headers.push_back(msg->header);
    }
  }
}