我正在尝试实施粒子滤波器，基于编程汽车和其他一些在线视频的udacity课程。
通过减去连续的帧来找到汽车的粒子滤波器的测量值，即（x，y）。
代码是
//motionTracking.cpp
#include <opencv\cv.h>
#include <opencv\highgui.h>
#include"particle filter.h"
#include<Windows.h>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include<stdio.h>
#include<iostream>
#include<fstream>
#include<stdlib.h>
#include<math.h>
using namespace cv;
using namespace std;
#define max 5
float dt = 1.0;
static Mat A = (Mat_<float>(4, 4) << 1.0, 0.0, dt, 0, 0, 1.0, 0.0, dt, 0.0, 0.0, 1.0, 0.0, 0, 0, 0, 1.0);
static Mat Prior = (Mat_<float>(4, 1) << 0, 0, 0, 0);
static Mat Predict = (Mat_<float>(4, 1) << 0, 0, 0, 0);
Mat Ex = (Mat_<float>(4, 4) << dt*dt / 4.0, 0, dt*dt*dt / 2.0, 0, 0, dt*dt / 4.0, 0, dt*dt*dt / 2.0, dt*dt*dt / 2.0, 0.0, dt*dt, 0, 0, dt*dt*dt / 2.0, 0, dt*dt);
float X_R = 0.001;
static Mat Particle[max];
Mat Particle2[max];
float weight[max];
void intialise_particles(int a,int b)
{
int i;
for (i = 0; i < max; i++)
{
Particle[i] = (Mat_<float>(4, 1) << rand() % a, rand() % b, 0, 0);
weight[i] = 1;
}
}
void filter(float *a, float *b)
{
float x_m = *a;
float y_m = *b;
//for the car
Predict = A*Prior;
//updating the particles
float sum = 0;
float maximum = 0;
for (int i = 0; i < max; i++)
{
Particle[i] = A*Particle[i];
Particle[i].at<float>(0, 0) += uniformRandom();
Particle[i].at<float>(1, 0) += uniformRandom();
float xW, yW;
xW = (1 / sqrt(2 * 3.14*X_R))*exp((-1 * (x_m - Particle[i].at<float>(0, 0)) * (x_m - Particle[i].at<float>(0, 0))) / 2.0*X_R);
yW = (1 / sqrt(2 * 3.14*X_R))*exp((-1 * (y_m - Particle[i].at<float>(1, 0)) * (x_m - Particle[i].at<float>(1, 0))) / 2.0*X_R);
weight[i] = sqrt(xW*xW + yW*yW);
sum += weight[i];
if (weight[i] > maximum)
maximum = weight[i];
}
//normalizing the weights;
for (int i = 0; i < max; i++)
weight[i] = weight[i] / sum;
//resampling
//index = int(random.random() * N)
int index = rand() % max;
float beta = 0.0;
//mw = max(W)
float mw = maximum;
for (int i = 0; i < max; i++)
{
beta += rand()*2.0*mw;
while (beta>weight[index])
{
beta -= weight[index];
index = (index + 1) % max;
}
Particle2[i] = Particle[index];
}
x_m = 0;
y_m = 0;
for (int i = 0; i < max; i++)
{
Particle[i] = Particle2[i];
x_m += weight[i] * Particle[i].at<float>(0, 0);
y_m += weight[i] * Particle[i].at<float>(1, 0);
*a = x_m;
*b = y_m;
}
cout << "\n";
cout << Particle;
}
//our sensitivity value to be used in the absdiff() function
const static int SENSITIVITY_VALUE = 20;
//size of blur used to smooth the intensity image output from absdiff() function
const static int BLUR_SIZE = 10;
//we'll have just one object to search for
//and keep track of its position.
int theObject[2] = { 0, 0 };
//bounding rectangle of the object, we will use the center of this as its position.
Rect objectBoundingRectangle = Rect(0, 0, 0, 0);
//int to string helper function
string intToString(int number){
//this function has a number input and string output
std::stringstream ss;
ss << number;
return ss.str();
}
void searchForMovement(Mat thresholdImage, Mat &cameraFeed){
static unsigned int time = 0;
bool objectDetected = false;
Mat temp;
thresholdImage.copyTo(temp);
//these two vectors needed for output of findContours
vector< vector<Point> > contours;
vector<Vec4i> hierarchy;
//find contours of filtered image using openCV findContours function
//findContours(temp,contours,hierarchy,CV_RETR_CCOMP,CV_CHAIN_APPROX_SIMPLE );// retrieves all contours
findContours(temp, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);// retrieves external contours
//if contours vector is not empty, we have found some objects
if (contours.size()>0)objectDetected = true;
else objectDetected = false;
if (objectDetected){
//the largest contour is found at the end of the contours vector
//we will simply assume that the biggest contour is the object we are looking for.
vector< vector<Point> > largestContourVec;
largestContourVec.push_back(contours.at(contours.size() - 1));
//make a bounding rectangle around the largest contour then find its centroid
//this will be the object's final estimated position.
objectBoundingRectangle = boundingRect(largestContourVec.at(0));
int xpos = objectBoundingRectangle.x + objectBoundingRectangle.width / 2;
int ypos = objectBoundingRectangle.y + objectBoundingRectangle.height / 2;
//update the objects positions by changing the 'theObject' array values
theObject[0] = xpos, theObject[1] = ypos;
}
//make some temp x and y variables so we dont have to type out so much
int x = theObject[0];
int y = theObject[1];
float ParX, ParY;
ParX = x;
ParY = y;
filter(&ParX, &ParY);
ParX = (int)ParX;
ParY = (int)ParY;
//Particle filter estimate
circle(cameraFeed, Point(ParX, ParY), 20, Scalar(0, 0,255), 2);
line(cameraFeed, Point(ParX, ParY), Point(ParX, ParY - 25), Scalar(0, 0,255), 2);
line(cameraFeed, Point(ParX, ParY), Point(ParX, ParY + 25), Scalar(0, 0,255), 2);
line(cameraFeed, Point(ParX, ParY), Point(ParX - 25, ParY), Scalar(0, 0,255), 2);
line(cameraFeed, Point(ParX, ParY), Point(ParX + 25, ParY), Scalar(0, 0,255), 2);
//draw some crosshairs around the object
circle(cameraFeed, Point(x, y), 20, Scalar(0, 255, 0), 2);
line(cameraFeed, Point(x, y), Point(x, y - 25), Scalar(0, 255, 0), 2);
line(cameraFeed, Point(x, y), Point(x, y + 25), Scalar(0, 255, 0), 2);
line(cameraFeed, Point(x, y), Point(x - 25, y), Scalar(0, 255, 0), 2);
line(cameraFeed, Point(x, y), Point(x + 25, y), Scalar(0, 255, 0), 2);
//write the position of the object to the screen
putText(cameraFeed,"Tracking object at (" + intToString(x)+","+intToString(y)+")",Point(x,y),1,1,Scalar(255,0,0),2);
}
int main(){
//some boolean variables for added functionality
bool objectDetected = false;
//these two can be toggled by pressing 'd' or 't'
bool debugMode = false;
bool trackingEnabled = false;
//pause and resume code
bool pause = false;
//set up the matrices that we will need
//the two frames we will be comparing
Mat frame1, frame2;
//their grayscale images (needed for absdiff() function)
Mat grayImage1, grayImage2;
//resulting difference image
Mat differenceImage;
//thresholded difference image (for use in findContours() function)
Mat thresholdImage;
//video capture object.
// Mat frame1, frame2;
string a = "f (";
string b = ").png";
unsigned int f = 1;
frame1 = imread(a + intToString(f) + b);
intialise_particles(frame1.rows, frame1.cols);
for (int k = 0; k < max; k++)
{
cout << Particle[k];
cout << "\n";
}
while (1){
//we can loop the video by re-opening the capture every time the video reaches its last frame
while (1){
//cout << f;
//read first frame
//capture.read(frame1);
string abcd = a + intToString(f) + b;
//cout << abcd;
//char abc = getchar();
frame1 = imread(a + intToString(f) + b);
//convert frame1 to gray scale for frame differencing
cv::cvtColor(frame1, grayImage1, COLOR_BGR2GRAY);
//grayImage1.convertTo(grayImage2, CV_32F);
//copy second frame
//capture.read(frame2);
if (f == 449)
f = 0;
frame2 = imread(a + intToString(f + 1) + b);
f++;
//convert frame2 to gray scale for frame differencing
cv::cvtColor(frame2, grayImage2, COLOR_BGR2GRAY);
/*capture.set(CV_CAP_PROP_POS_FRAMES, position);
position = position + 1;
if (position > t)
position = 0;*/
//grayImage2.convertTo(grayImage2, CV_32F);
//perform frame differencing with the sequential images. This will output an "intensity image"
//do not confuse this with a threshold image, we will need to perform thresholding afterwards.
cv::absdiff(grayImage1, grayImage2, differenceImage);
//threshold intensity image at a given sensitivity value
cv::threshold(differenceImage, thresholdImage, SENSITIVITY_VALUE, 255, THRESH_BINARY);
if (debugMode == true){
//show the difference image and threshold image
cv::imshow("Difference Image", differenceImage);
cv::imshow("Threshold Image", thresholdImage);
}
else{
//if not in debug mode, destroy the windows so we don't see them anymore
cv::destroyWindow("Difference Image");
cv::destroyWindow("Threshold Image");
}
//blur the image to get rid of the noise. This will output an intensity image
cv::blur(thresholdImage, thresholdImage, cv::Size(BLUR_SIZE, BLUR_SIZE));
//threshold again to obtain binary image from blur output
cv::threshold(thresholdImage, thresholdImage, SENSITIVITY_VALUE, 255, THRESH_BINARY);
if (debugMode == true){
//show the threshold image after it's been "blurred"
imshow("Final Threshold Image", thresholdImage);
}
else {
//if not in debug mode, destroy the windows so we don't see them anymore
cv::destroyWindow("Final Threshold Image");
}
//if tracking enabled, search for contours in our thresholded image
if (trackingEnabled){
searchForMovement(thresholdImage, frame1);
for (int k = 0; k < max; k++)
{
int a = (int)Particle[k].at<float>(0, 0);
int b = (int)Particle[k].at<float>(1, 0);
circle(frame1, Point(a, b), 1, Scalar(0, 0,255), 2);
}
}
//show our captured frame
imshow("Frame1", frame1);
//waitKey(10);
//imshow("Frame1", frame2);
//check to see if a button has been pressed.
//this 10ms delay is necessary for proper operation of this program
//if removed, frames will not have enough time to referesh and a blank
//image will appear.
switch (waitKey(10*2 )){
case 27: //'esc' key has been pressed, exit program.
return 0;
case 116: //'t' has been pressed. this will toggle tracking
trackingEnabled = !trackingEnabled;
if (trackingEnabled == false) cout << "Tracking disabled." << endl;
else cout << "Tracking enabled." << endl;
break;
case 100: //'d' has been pressed. this will debug mode
debugMode = !debugMode;
if (debugMode == false) cout << "Debug mode disabled." << endl;
else cout << "Debug mode enabled." << endl;
break;
case 112: //'p' has been pressed. this will pause/resume the code.
pause = !pause;
if (pause == true){
cout << "Code paused, press 'p' again to resume" << endl;
while (pause == true){
//stay in this loop until
switch (waitKey()){
//a switch statement inside a switch statement? Mind blown.
case 112:
//change pause back to false
pause = false;
cout << "Code Resumed" << endl;
break;
}
}
}
}
}
//release the capture before re-opening and looping again.
// capture.release();
}
return 0;
}
正态分布的随机数生成器由头文件中的函数完成。
double uniformRandom()
{
return ((double)(rand()) + 1.) / ((double)(RAND_MAX)+1.);
}
// return a normally distributed random number
double normalRandom()
{
double u1 = uniformRandom();
double u2 = uniformRandom();
return cos(8.*atan(1.)*u2)*sqrt(-2.*log(u1));
}
代码工作正常，但是当我打开跟踪时，它会运行一些帧并在此之后挂起。
输入文件在input files