In my attempt to implement particle filter, I have first manually drawn a rectangle(x,y,w,h) around the car in my image (in red color), then i took 50 particles, and assigned them noise i.e x=x+noise(0,15) and y=y+noise(0,15).
Then i wanted to dram all the rectangles for each particle in green color, but instead of showing 50 rectangles, it is only showing one rectangle.
#include<opencv2\core\core.hpp>
#include<opencv2\imgproc\imgproc.hpp>
#include<opencv2\highgui\highgui.hpp>
#include<stdio.h>
#include<iostream>
#include<random>
using namespace cv;
using namespace std;
const int N = 50;// no of particles
string intToString(int number){
//this function has a number input and string output
std::stringstream ss;
ss << number;
return ss.str();
}
int main()
{
Mat frame;
frame = imread("f (1).png");
namedWindow("Out");
//locating the car manually
Rect car(175, 210, 42, 31);
//making a rectangle around the car
rectangle(frame, car, Scalar(0, 0,255), 1, 8, 0);
//getting tht height and width of the frame
const int FRAME_HEIGHT = frame.rows;
const int FRAME_WIDTH = frame.cols;
//Particle filter initialization
Mat Init = (Mat_<float>(4, 1) << car.x, car.y, 0, 0);
//for a gaussian noise distribution
std::default_random_engine generator;
std::normal_distribution<double> distribution(0, 15);
//Initializing the particles
std::vector<cv::Mat> particle(N, cv::Mat(4, 1, CV_32F));
cout << car.x << " " << car.y << "\n";
for (int i = 0; i < N; i++)
{
particle[i].at<float>(0, 0) = Init.at<float>(0, 0) + distribution(generator);
particle[i].at<float>(1, 0) = Init.at<float>(1, 0) + distribution(generator);
particle[i].at<float>(2, 0) = 0.0;
particle[i].at<float>(3, 0) = 0.0;
cout << particle[i] << "\n";
}
for (int i = 0; i < N; i++)
{
int x = particle[i].at<float>(0, 0);
int y = particle[i].at<float>(1, 0);
rectangle(frame, Rect(x, y, 42, 31), Scalar(0, 255, 0), 1, 8, 0);
}
imshow("Out", frame);
waitKey();
return 0;
}
The output looks like this
The particle x y coordinates are in the following image, The first entry is of the the car that was manually hard coded, rest all are the particle coordinates.
Unfortunately, the problem might be in vector initialization. In the line:
//Initializing the particles
std::vector<cv::Mat> particle(N, cv::Mat(4, 1, CV_32F));
What you do is to create N Mat objects initialized to size 4 by 1 of type float. So this seems to be okay...Wait! What if all Mat objects in the vector share the same matrix data? We all know that Mat container has a header and data. Data is not copied when the headers are assigned to each other or copy constructor is executed. Like in our case. What exactly Let's imagine how the vector constructor might work:
template<class T>
vector::vector (size_type n, const value_type& val = value_type(),
const allocator_type& alloc = allocator_type()) {
// how it can be written? Probably in the following way:
this->resize(n); // allocates using allocator
for(int i=0;i<n;i++)
this->innerBuffer[i] = alloc(val); // runs copy constructor with argument val
// in our case, this is cv::Mat(const cv::Mat &)
}
And if you think about copying of cv::Mat container, only header is copied. If you would like to clone the data itself, you have to specify it directly, e.g. using Mat::clone() method. And vector constructor definitely does not use clone(). So the data is not cloned and all Mat objects share the same data, although headers are different!
You can easy check if this is true (I would be surprised if it wasn't): Change the fragment of code from:
for (int i = 0; i < N; i++)
{
particle[i].at<float>(0, 0) = Init.at<float>(0, 0) + distribution(generator);
particle[i].at<float>(1, 0) = Init.at<float>(1, 0) + distribution(generator);
particle[i].at<float>(2, 0) = 0.0;
particle[i].at<float>(3, 0) = 0.0;
cout << particle[i] << "\n"; // it is obvius that HERE particles are different
// beacuse in the next loop run you overwrite them!
}
for (int i = 0; i < N; i++)
{
int x = particle[i].at<float>(0, 0);
int y = particle[i].at<float>(1, 0);
rectangle(frame, Rect(x, y, 42, 31), Scalar(0, 255, 0), 1, 8, 0);
}
to
for (int i = 0; i < N; i++)
{
particle[i].at<float>(0, 0) = Init.at<float>(0, 0) + distribution(generator);
particle[i].at<float>(1, 0) = Init.at<float>(1, 0) + distribution(generator);
particle[i].at<float>(2, 0) = 0.0;
particle[i].at<float>(3, 0) = 0.0;
}
for (int i = 0; i < N; i++)
{
cout << particle[i] << "\n";
int x = particle[i].at<float>(0, 0);
int y = particle[i].at<float>(1, 0);
rectangle(frame, Rect(x, y, 42, 31), Scalar(0, 255, 0), 1, 8, 0);
}
I expect that all particles are the same.
Reference: (here)
vector(size_type __n, const value_type& __value,
00199 const allocator_type& __a = allocator_type())
00200 : _Base(__n, __a)
00201 { this->_M_impl._M_finish = std::uninitialized_fill_n(this->
00202 _M_impl._M_start,
00203 __n, __value); }
00204