Skip to content

Commit

Permalink
Merge pull request oreillymedia#43 from oreillymedia/chap17
Browse files Browse the repository at this point in the history 
Chap17
  • Loading branch information
@garybradski
garybradski authored Jun 28, 2017
2 parents c96e726 + 7cc3d8d commit ed4dea2
Show file tree
Hide file tree
Showing 4 changed files with 498 additions and 392 deletions.
181 changes: 93 additions & 88 deletions example_17-01.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,101 +1,106 @@
//Example 17-1. Kalman filter example code
#include <opencv2/opencv.hpp>
// Example 17-1. Kalman filter example code

#include <vector>
#include <iostream>
#include <cstdlib>
#include <fstream>
#include <opencv2/opencv.hpp>

using namespace cv;
using namespace std;
using std::cout;
using std::endl;

#define phi2xy( mat ) \
cv::Point( cvRound( img.cols/2 + img.cols/3*cos(mat.at<float>(0)) ), \
cvRound( img.rows/2 - img.cols/3*sin(mat.at<float>(0)) ) )
#define phi2xy(mat) \
cv::Point(cvRound(img.cols / 2 + img.cols / 3 * cos(mat.at<float>(0))), \
cvRound(img.rows / 2 - img.cols / 3 * sin(mat.at<float>(0))))


void help(char** argv ) {
cout << "\n"
<< "Example code for using cv::KalmanFilter\n"
<< argv[0] << "\n\n"
<< "For example:\n"
<< argv[0] <<"\n\n"
<< "Esc to quit\n"
<< endl;
cout << "\n"
<< "Example code for using cv::KalmanFilter\n"
<< argv[0] << "\n\n"
<< "For example:\n"
<< argv[0] <<"\n\n"
<< "Esc to quit\n"
<< endl;
}

int main(int argc, char** argv) {
help(argv);

// Initialize, create Kalman filter object, window, random number
// generator etc.
//
cv::Mat img( 500, 500, CV_8UC3 );
cv::KalmanFilter kalman(2, 1, 0);

// state is (phi, delta_phi) - angle and angular velocity
// Initialize with random guess.
//
cv::Mat x_k( 2, 1, CV_32F );
randn( x_k, 0., 0.1 );

// process noise
//
cv::Mat w_k( 2, 1, CV_32F );

// measurements, only one parameter for angle
//
cv::Mat z_k = cv::Mat::zeros( 1, 1, CV_32F );

// Transition matrix 'F' describes relationship between
// model parameters at step k and at step k+1 (this is
// the "dynamics" in our model.
//
float F[] = { 1, 1, 0, 1 };
kalman.transitionMatrix = cv::Mat( 2, 2, CV_32F, F ).clone();

// Initialize other Kalman filter parameters.
//
cv::setIdentity( kalman.measurementMatrix, cv::Scalar(1) );
cv::setIdentity( kalman.processNoiseCov, cv::Scalar(1e-5) );
cv::setIdentity( kalman.measurementNoiseCov, cv::Scalar(1e-1) );
cv::setIdentity( kalman.errorCovPost, cv::Scalar(1) );

// choose random initial state
//
randn(kalman.statePost, 0., 0.1);

for(;;) {

// predict point position
//
cv::Mat y_k = kalman.predict();

// generate measurement (z_k)
//
cv::randn(z_k, 0., sqrt((double)kalman.measurementNoiseCov.at<float>(0,0)));
z_k = kalman.measurementMatrix*x_k + z_k;

// plot points (e.g., convert
//
img = cv::Scalar::all(0);
cv::circle( img, phi2xy(z_k), 4, cv::Scalar(128,255,255) ); // observed
cv::circle( img, phi2xy(y_k), 4, cv::Scalar(255,255,255), 2 ); // predicted
cv::circle( img, phi2xy(x_k), 4, cv::Scalar(0,0,255) ); // actual to planar co-ordinates and draw)

cv::imshow( "Kalman", img );

// adjust Kalman filter state
//
kalman.correct( z_k );

// Apply the transition matrix 'F' (e.g., step time forward)
// and also apply the "process" noise w_k
//
cv::randn(w_k, 0., sqrt((double)kalman.processNoiseCov.at<float>(0,0)));
x_k = kalman.transitionMatrix*x_k + w_k;

// exit if user hits 'Esc'
if( (cv::waitKey( 100 ) & 255) == 27 ) break;
}
return 0;
help(argv);

// Initialize, create Kalman filter object, window, random number
// generator etc.
//
cv::Mat img(500, 500, CV_8UC3);
cv::KalmanFilter kalman(2, 1, 0);

// state is (phi, delta_phi) - angle and angular velocity
// Initialize with random guess.
//
cv::Mat x_k(2, 1, CV_32F);
randn(x_k, 0.0, 0.1);

// process noise
//
cv::Mat w_k(2, 1, CV_32F);

// measurements, only one parameter for angle
//
cv::Mat z_k = cv::Mat::zeros(1, 1, CV_32F);

// Transition matrix 'F' describes relationship between
// model parameters at step k and at step k+1 (this is
// the "dynamics" in our model.
//
float F[] = {1, 1, 0, 1};
kalman.transitionMatrix = cv::Mat(2, 2, CV_32F, F).clone();

// Initialize other Kalman filter parameters.
//
cv::setIdentity(kalman.measurementMatrix, cv::Scalar(1));
cv::setIdentity(kalman.processNoiseCov, cv::Scalar(1e-5));
cv::setIdentity(kalman.measurementNoiseCov, cv::Scalar(1e-1));
cv::setIdentity(kalman.errorCovPost, cv::Scalar(1));

// choose random initial state
//
randn(kalman.statePost, 0.0, 0.1);

for (;;) {
// predict point position
//
cv::Mat y_k = kalman.predict();

// generate measurement (z_k)
//
cv::randn(z_k, 0.0,
sqrt(static_cast<double>(kalman.measurementNoiseCov.at<float>(0, 0))));
z_k = kalman.measurementMatrix*x_k + z_k;

// plot points (e.g., convert
//
img = cv::Scalar::all(0);
cv::circle(img, phi2xy(z_k), 4, cv::Scalar(128, 255, 255)); // observed
cv::circle(img, phi2xy(y_k), 4, cv::Scalar(255, 255, 255), 2); // predicted
cv::circle(img, phi2xy(x_k), 4, cv::Scalar(0, 0, 255)); // actual to
// planar co-ordinates and draw

cv::imshow("Kalman", img);

// adjust Kalman filter state
//
kalman.correct(z_k);

// Apply the transition matrix 'F' (e.g., step time forward)
// and also apply the "process" noise w_k
//
cv::randn(w_k, 0.0, sqrt(static_cast<double>(kalman.processNoiseCov.at<float>(0, 0))));
x_k = kalman.transitionMatrix*x_k + w_k;

// exit if user hits 'Esc'
if ((cv::waitKey(100) & 255) == 27) {
break;
}
}

return 0;
}
90 changes: 90 additions & 0 deletions example_17-02.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,90 @@
// Example 17-2. Farneback optical flow example code

#include <vector>
#include <iostream>
#include <cstdlib>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

using std::cout;
using std::cerr;
using std::string;

// Argument values for calcOpticalFlowFarneback
//
const double pyr_scale = 0.85; // Scale between pyramid levels (< 1.0)
const int levels = 7; // Number of pyramid levels
const int winsize = 13; // Size of window for pre-smoothing pass
const int iterations = 10; // Iterations for each pyramid level
const int poly_n = 5; // Area over which polynomial will be fit
const double poly_sigma = 1.1; // Width of fit polygon

// Function returns cv::Mat object with optical flow visualization
//
cv::Mat get_optflow_image(cv::Mat& optflow, cv::Mat& img) {
cv::Scalar arrow_color(0, 0, 255);
cv::Mat res = img.clone();
res /= 2; // making image darker
int rows = res.rows;
int cols = res.cols;
const int step = 12;
for (int x = (step >> 1); x < rows; x += step)
for (int y = (step >> 1); y < cols; y += step) {
float vx = optflow.at<cv::Vec2f>(x, y)[0];
float vy = optflow.at<cv::Vec2f>(x, y)[1];
cv::Point pt1(y, x);
cv::Point pt2(y + vx, x + vy);
cv::arrowedLine(res, pt1, pt2, arrow_color, 1);
}
return res;
}

int main(int argc, char** argv) {
// Program expects at least one argument that is path to video file
//
if (argc < 2) {
cerr << "Use:\n" << argv[0] << " <video_file>\n"
<< "to run this demo\n";
exit(1);
}

string file_name = string(argv[1]);
cv::VideoCapture capture(file_name);

if (!capture.isOpened()) {
cerr << "Cannot open file \"" << file_name << "\"\n";
exit(-1);
}

cv::Mat optflow; // optical flow result
cv::Mat optflow_image; // optical flow visualization
cv::Mat prev_frame; // previous frame grayscale image
cv::Mat frame; // current frame grayscale image
cv::Mat colored_frame; // current frame RGB-image

cv::namedWindow("video");

// User can terminate program with hitting ESC
//
while ((cv::waitKey(10) & 255) != 27) {
capture >> colored_frame;
if (!colored_frame.rows || !colored_frame.cols) {
break;
}
if (colored_frame.type() == CV_8UC3) {
cvtColor(colored_frame, frame, CV_BGR2GRAY);
}
if (prev_frame.rows) {
calcOpticalFlowFarneback(prev_frame, frame, optflow, pyr_scale, levels, winsize,
iterations, poly_n, poly_sigma, cv::OPTFLOW_FARNEBACK_GAUSSIAN);
optflow_image = get_optflow_image(optflow, colored_frame);
cv::imshow("video", optflow_image);
}
prev_frame = frame.clone();
}
cv::destroyAllWindows();

return 0;
}
Loading

0 comments on commit ed4dea2

Please sign in to comment.