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;
}
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";
}
Update( ParticleFilter& filter, const PEBC& eval_base, double& weight_sum ):
filter_(filter), eval_base_(eval_base),
prev_particles_( filter.particles_ ), weight_sum_(weight_sum)
{
filter.GetParticleNumList( particle_num_list_ );
filter.particles_.clear();
filter.weights_.clear();
filter.particles_.reserve( filter.particle_num_ * 2 );
filter.weights_.reserve( filter.particle_num_ * 2 );
weight_sum_ = 0.0;
}
//Body tracking Single Threaded
int mainSingleThread(string path, int cameras, int frames, int particles, int layers, bool OutputBMP)
{
cout << endl << "Running Single Threaded" << endl << endl;
TrackingModel model;
if(!model.Initialize(path, cameras, layers)) //Initialize model parameters
{ cout << endl << "Error loading initialization data." << endl;
return 0;
}
model.GetObservation(0); //load data for first frame
ParticleFilter<TrackingModel> pf; //particle filter instantiated with body tracking model type
pf.SetModel(model); //set the particle filter model
pf.InitializeParticles(particles); //generate initial set of particles and evaluate the log-likelihoods
cout << "Using dataset : " << path << endl;
cout << particles << " particles with " << layers << " annealing layers" << endl << endl;
ofstream outputFileAvg((path + "poses.txt").c_str());
vector<float> estimate; //expected pose from particle distribution
#if defined(ENABLE_PARSEC_HOOKS)
__parsec_roi_begin();
#endif
clock_t start_t1 = clock();
for(int i = 0; i < frames; i++) //process each set of frames
{ cout << "Processing frame " << i << endl;
if(!pf.Update((float)i)) //Run particle filter step
{ cout << "Error loading observation data" << endl;
return 0;
}
pf.Estimate(estimate); //get average pose of the particle distribution
WritePose(outputFileAvg, estimate);
if(OutputBMP)
pf.Model().OutputBMP(estimate, i); //save output bitmap file
}
#if defined(ENABLE_PARSEC_HOOKS)
__parsec_roi_end();
#endif
clock_t end_t2 = clock();
long double diff_ticks = ((long double)end_t2 - (long double)start_t1);
cout << "ROI It took me " << diff_ticks << " clicks and " << diff_ticks/CLOCKS_PER_SEC << " seconds." << endl;
return 1;
}
//Body tracking Single Threaded
int mainSingleThread(string path, int cameras, int frames, int particles, int layers, bool OutputBMP)
{
cout << endl << "Running Single Threaded" << endl << endl;
TrackingModel model;
if(!model.Initialize(path, cameras, layers)) //Initialize model parameters
{ cout << endl << "Error loading initialization data." << endl;
return 0;
}
model.GetObservation(0); //load data for first frame
ParticleFilter<TrackingModel> pf; //particle filter instantiated with body tracking model type
pf.SetModel(model); //set the particle filter model
pf.InitializeParticles(particles); //generate initial set of particles and evaluate the log-likelihoods
cout << "Using dataset : " << path << endl;
cout << particles << " particles with " << layers << " annealing layers" << endl << endl;
ofstream outputFileAvg((path + "poses.txt").c_str());
vector<float> estimate; //expected pose from particle distribution
for(int i = 0; i < frames; i++) //process each set of frames
{ cout << "Processing frame " << i << endl;
if(!pf.Update((float)i)) //Run particle filter step
{ cout << "Error loading observation data" << endl;
return 0;
}
pf.Estimate(estimate); //get average pose of the particle distribution
WritePose(outputFileAvg, estimate);
if(OutputBMP)
pf.Model().OutputBMP(estimate, i); //save output bitmap file
}
return 1;
}
int main() {
uWS::Hub h;
// Set up parameters here
double delta_t = 0.1; // Time elapsed between measurements [sec]
double sensor_range = 50; // Sensor range [m]
double sigma_pos[3] = {
0.3, 0.3,
0.01}; // GPS measurement uncertainty [x [m], y [m], theta [rad]]
double sigma_landmark[2] = {
0.3, 0.3}; // Landmark measurement uncertainty [x [m], y [m]]
// Read map data
Map map;
if (!read_map_data("../data/map_data.txt", map)) {
cout << "Error: Could not open map file" << endl;
return -1;
}
// Create particle filter
ParticleFilter pf;
h.onMessage([&pf, &map, &delta_t, &sensor_range, &sigma_pos,
&sigma_landmark](uWS::WebSocket<uWS::SERVER> ws, char *data,
size_t length, uWS::OpCode opCode) {
// "42" at the start of the message means there's a websocket message event.
// The 4 signifies a websocket message
// The 2 signifies a websocket event
if (length && length > 2 && data[0] == '4' && data[1] == '2') {
auto s = hasData(std::string(data));
if (s != "") {
auto j = json::parse(s);
std::string event = j[0].get<std::string>();
if (event == "telemetry") {
// j[1] is the data JSON object
if (!pf.initialized()) {
// Sense noisy position data from the simulator
double sense_x = std::stod(j[1]["sense_x"].get<std::string>());
double sense_y = std::stod(j[1]["sense_y"].get<std::string>());
double sense_theta =
std::stod(j[1]["sense_theta"].get<std::string>());
pf.init(sense_x, sense_y, sense_theta, sigma_pos);
} else {
// Predict the vehicle's next state from previous (noiseless
// control) data.
double previous_velocity =
std::stod(j[1]["previous_velocity"].get<std::string>());
double previous_yawrate =
std::stod(j[1]["previous_yawrate"].get<std::string>());
pf.prediction(delta_t, sigma_pos, previous_velocity,
previous_yawrate);
}
// receive noisy observation data from the simulator
// sense_observations in JSON format
// [{obs_x,obs_y},{obs_x,obs_y},...{obs_x,obs_y}]
vector<LandmarkObs> noisy_observations;
string sense_observations_x = j[1]["sense_observations_x"];
string sense_observations_y = j[1]["sense_observations_y"];
std::vector<float> x_sense;
std::istringstream iss_x(sense_observations_x);
std::copy(std::istream_iterator<float>(iss_x),
std::istream_iterator<float>(),
std::back_inserter(x_sense));
std::vector<float> y_sense;
std::istringstream iss_y(sense_observations_y);
std::copy(std::istream_iterator<float>(iss_y),
std::istream_iterator<float>(),
std::back_inserter(y_sense));
for (int i = 0; i < x_sense.size(); i++) {
LandmarkObs obs;
obs.x = x_sense[i];
obs.y = y_sense[i];
noisy_observations.push_back(obs);
}
// Update the weights and resample
pf.updateWeights(sensor_range, sigma_landmark, noisy_observations,
map);
pf.resample();
// Calculate and output the average weighted error of the particle
// filter over all time steps so far.
vector<Particle> particles = pf.particles;
int num_particles = particles.size();
double highest_weight = -1.0;
Particle best_particle;
double weight_sum = 0.0;
for (int i = 0; i < num_particles; ++i) {
if (particles[i].weight > highest_weight) {
highest_weight = particles[i].weight;
best_particle = particles[i];
}
weight_sum += particles[i].weight;
}
cout << "highest w " << highest_weight << endl;
cout << "average w " << weight_sum / num_particles << endl;
json msgJson;
msgJson["best_particle_x"] = best_particle.x;
msgJson["best_particle_y"] = best_particle.y;
msgJson["best_particle_theta"] = best_particle.theta;
// Optional message data used for debugging particle's sensing and
// associations
msgJson["best_particle_associations"] =
pf.getAssociations(best_particle);
msgJson["best_particle_sense_x"] = pf.getSenseX(best_particle);
msgJson["best_particle_sense_y"] = pf.getSenseY(best_particle);
auto msg = "42[\"best_particle\"," + msgJson.dump() + "]";
// std::cout << msg << std::endl;
ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
}
} else {
std::string msg = "42[\"manual\",{}]";
ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
}
}
});
// We don't need this since we're not using HTTP but if it's removed the
// program
// doesn't compile :-(
h.onHttpRequest([](uWS::HttpResponse *res, uWS::HttpRequest req, char *data,
size_t, size_t) {
const std::string s = "<h1>Hello world!</h1>";
if (req.getUrl().valueLength == 1) {
res->end(s.data(), s.length());
} else {
// i guess this should be done more gracefully?
res->end(nullptr, 0);
}
});
h.onConnection([&h](uWS::WebSocket<uWS::SERVER> ws, uWS::HttpRequest req) {
std::cout << "Connected!!!" << std::endl;
});
h.onDisconnection([&h](uWS::WebSocket<uWS::SERVER> ws, int code,
char *message, size_t length) {
ws.close();
std::cout << "Disconnected" << std::endl;
});
int port = 4567;
if (h.listen(port)) {
std::cout << "Listening to port " << port << std::endl;
} else {
std::cerr << "Failed to listen to port" << std::endl;
return -1;
}
h.run();
}
int main(int argc, char** argv) {
/*if (argc < 1) {
cout << "Need 1 argument: input_file" << endl;
return -1;
}*/
//ifstream infile(argv[1]);
//declare particle filter and Location Visualizer for processing:
ParticleFilter pf;
pf.set_params(); //TODO: CHANGE!
pf.add_particles(200,-300,0,0,1);
//pf.add_particles(200);
InputGenerator ig;
LocationVisualizer lv;
lv.draw_particle(pf.particles);
lv.create_window();
lv.clear_buffer();
ifstream infile("straight_processed_patrick(1).txt");//"straight_processed_patrick_experimental.txt");//"straightwalk_binary_IPM_RnB.txt");//"straight_processed_patrick.txt");
string line;
istringstream iss;
vector<string> tokens;
int counter = 0;
if (infile.is_open()) {
while (infile.good()) {
getline(infile, line);
tokens = split(line, ' ');
cout<<"line size:"<<tokens.size()<<endl;
for(int i = 0; i< tokens.size();i++){
cout<<tokens[i]<<endl;
}
if ((tokens.size() - 1) % 4 == 0 && tokens.size() > 1) {
////FILE PROCESSING
///new frame !!!
string filename;
vector<VisualFeature> vf;
processed_line(&filename,&vf,tokens);
filename += ".png";
filename = "./dataset_straightwalk/" + filename;
cout<<"processing:"<<filename<<"coutner:"<<counter<<endl;
counter++;
//read file
Mat image;
image = imread(filename,1);
namedWindow( "Display window", CV_WINDOW_AUTOSIZE );// Create a window for display.
imshow( "Display window", image ); // Show our image inside it.
/*
vector<VisualFeature> vis_feat;
ig.generate_features(counter * 20,0, 0,pf.feature_map, &vis_feat);
ig.calculate_range_bearing(pf.feature_map,counter*20, 0, 0, vis_feat);
for(int i = 0; i< vis_feat.size();i++){
cout<<"visual feature: "<<vis_feat[i].range<<" "<<vis_feat[i].bearing<<endl;
}
counter++;*/
////////DO PARTICLE PROCESSING
//shity implementation of odometry data:
OdometryInformation of;
of.rot = 0;
of.x = 20;
of.y = 0;
pf.dynamic(of,vf);
pf.resample();
lv.draw_particle(pf.particles);
lv.create_window();
//make clean window (footballfield);
lv.clear_buffer();
}
}
infile.close();
}
return 0;
}
