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; }
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 }
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; }
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); } }
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); } } }