int main()
{
// Setup for random number generator
srand(time(NULL));
// Initialize our particle filter
ParticleFilter filter;
// Load occupancy map of wean hall and data from log
filter.readMap();
filter.loadMapImage();
filter.readLog();
// Draw initial particles
filter.drawParticles();
// Start the filter!
for (int i = 1; i < filter.timestamps.size(); i++) {
filter.motionModel(i);
filter.updateWeights(i);
filter.resampleParticles();
filter.visualize();
}
std::cout << "Done!\n";
}
int main( int argc, char** argv )
{
Mat frame;
namedWindow("Tracking window", WINDOW_NORMAL);
VideoCapture video(0);
if (!video.isOpened()) {
cout << "Failed to open video stream" << endl;
return -1;
}
video >> frame;
resize(frame, frame, Size(), 0.35, 0.35);
cout << frame.cols << " " << frame.rows << endl;
imshow("d", frame);
waitKey(0);
Mat objectToTrack = imread(argv[1], CV_LOAD_IMAGE_COLOR);
if (!objectToTrack.data) {
cout << "Failed to open template image" << endl;
return -1;
}
cout << objectToTrack.cols << " " << objectToTrack.rows << endl;
imshow("d", objectToTrack);
waitKey(0);
ParticleFilter particleFilter;
bool initalised = false;
while (true) {
char key = waitKey(1);
if (key == 'q') break;
video >> frame;
resize(frame, frame, Size(), 0.35, 0.35);
if (!initalised) {
particleFilter.initalise(frame, objectToTrack, 1000);
initalised = true;
}
particleFilter.update(frame);
// Draw predicted location
PerspectiveTransform est = particleFilter.getEstimateTransform();
Point tl(0, 0), tr(objectToTrack.cols, 0), bl(0, objectToTrack.rows),
br(objectToTrack.cols, objectToTrack.rows);
line(frame, est.transformPoint(tl), est.transformPoint(tr),
CV_RGB(255, 0, 0));
line(frame, est.transformPoint(tr), est.transformPoint(br),
CV_RGB(255, 0, 0));
line(frame, est.transformPoint(br), est.transformPoint(bl),
CV_RGB(255, 0, 0));
line(frame, est.transformPoint(bl), est.transformPoint(tl),
CV_RGB(255, 0, 0));
cout << est.getTranslation() << endl;
cout << est.getRotation() << endl;
particleFilter.drawParticles(frame, Scalar::all(100));
imshow("Tracking window", frame);
}
return 0;
}
