void id_bot(struct RoboAI *ai, struct blob *blobs)
{
// Drive a simple pattern forward/reverse and call the particle filter to
// find the blobs that are consistent with this in order to identify the
// bot as well as the opponent.
double noiseV=5; // Expected motion magnitude due to noise for a static object
// (it's just a guess - haven't measured it)
double oppSizeT=2500; // Opponent's blob nimimum size threshold
static double blobData[10][4]; // Keep track of up to 10 blobs that could be
// the bot. Unlikely there are more since
// these have to be active tracked blobs.
// blobData[i][:]=[blobID mx my p]
static int bdataidx=0;
double psum;
double px,siz,len,vx,vy,wx,wy;
int ownID,i;
struct blob *oppID;
struct blob *p;
static double IDstep=0; // Step tracker
double frameInc=1.0/10; // 1 over the number of frames each step should get
// Get a handle on the ball
id_ball(ai,blobs);
// Forward motion for a few frames
if (IDstep<3*frameInc)
{
drive_speed(35);
clear_motion_flags(ai);
ai->st.mv_fwd=1;
}
else if (IDstep<1.0)
{
drive_speed(35);
clear_motion_flags(ai);
ai->st.mv_fwd=1;
particle_filter(ai,blobs);
}
else if (IDstep<1.0+(3*frameInc)) // Reverse drive
{
drive_speed(-35);
clear_motion_flags(ai);
ai->st.mv_back=1;
}
else if (IDstep<2.0)
{
drive_speed(-35);
clear_motion_flags(ai);
ai->st.mv_back=1;
particle_filter(ai,blobs);
}
else if (IDstep<2.0+(5*frameInc)) {clear_motion_flags(ai); all_stop();} // Stop and clear blob motion history
else {IDstep=0; ai->st.state=1;} // For debug...
IDstep+=frameInc;
}
// [ref] ${IVT_HOME}/examples/ParticleFilterDemo/src/main.cpp
void particle_filter_example()
{
const int NUMBER_OF_PARTICLES = 200;
const int width = 400;
const int height = 300;
CByteImage image(width, height, CByteImage::eGrayScale);
CByteImage color(width, height, CByteImage::eRGB24);
MyRegion region;
int k = 40;
#ifdef TRACK_3D
double result_configuration[DIMENSION_3D];
CParticleFilter3D particle_filter(NUMBER_OF_PARTICLES, width, height, k);
#else
double result_configuration[DIMENSION_2D];
CParticleFilter2D particle_filter(NUMBER_OF_PARTICLES, width, height, k);
#endif
// gui
CApplicationHandlerInterface *pApplicationHandler = CreateApplicationHandler();
pApplicationHandler->Reset();
CMainWindowInterface *pMainWindow = CreateMainWindow(0, 0, width, height, "Particle Filter Demo");
WIDGET_HANDLE pImageWidget = pMainWindow->AddImage(0, 0, width, height);
pMainWindow->Show();
for (int i = 0; i < 600; ++i)
{
unsigned int t = get_timer_value(true);
local::SimulateMovement(&image);
particle_filter.SetImage(&image);
particle_filter.ParticleFilter(result_configuration);
#ifdef TRACK_3D
k = int(result_configuration[2] + 0.5);
#endif
region.min_x = int(result_configuration[0] - k + 0.5);
region.min_y = int(result_configuration[1] - k + 0.5);
region.max_x = int(result_configuration[0] + k + 0.5);
region.max_y = int(result_configuration[1] + k + 0.5);
ImageProcessor::ConvertImage(&image, &color);
PrimitivesDrawer::DrawRegion(&color, region, 255, 0, 0, 2);
pMainWindow->SetImage(pImageWidget, &color);
if (pApplicationHandler->ProcessEventsAndGetExit())
break;
while (get_timer_value() - t < 1000000.0 / 30);
}
delete pMainWindow;
delete pApplicationHandler;
}
int main(int argc, char *argv[]) {
/* Settings */
int use_smoothing = 0;
int use_flow = 1;
int use_multiple_predictions = 0;
int use_variance = 0;
if (argc < 2) {
printf("Please specify: test, train, or preds and data set size");
return -1;
}
int SIZE_SIFT = atoi(argv[2]);
/* Create arrays for measurements */
struct measurement measurements[SIZE_SIFT];
struct measurement measurements_2[SIZE_SIFT]; /* If multiply regressors are used */
struct measurement measurements_3[SIZE_SIFT]; /* If multiply regressors are used */
struct measurement opticalflow[SIZE_SIFT];
char *filename_out;
/* For test set */
if (strcmp(argv[1], "test") == 0) {
filename_out = "sift_filtered_test_2.csv";
//char filename_in[] = "/home/pold/Documents/Internship/datasets/board_test_pos.csv";
char filename_in[] = "/home/pold/Documents/Internship/datasets/board_test_2_pos.csv";
read_measurements_from_csv(measurements, filename_in, SIZE_SIFT);
}
/* For training set */
else if (strcmp(argv[1], "train") == 0) {
filename_out = "sift_filtered_train_vel.csv";
char filename_in[] = "/home/pold/Documents/Internship/datasets/board_train_pos.csv";
read_measurements_from_csv(measurements, filename_in, SIZE_SIFT);
}
/* For predictions */
else if (strcmp(argv[1], "preds") == 0) {
filename_out = "predictions_filtered_lasso.csv";
char filename_in[] = "/home/pold/Documents/treXton/predictions_cross.csv";
char filename_in_2[] = "/home/pold/Documents/treXton/predictions.csv";
char filename_optical_flow[] = "/home/pold/Documents/trexton_pprz/edgeflow_diff.csv";
//char filename_in_3[] = "/home/pold/Documents/Internship/treXton/predictions_3.csv";
read_predictions_from_csv(measurements, filename_in, SIZE_SIFT, 1);
read_predictions_from_csv(measurements_2, filename_in_2, SIZE_SIFT, 0);
read_predictions_from_csv(opticalflow, filename_optical_flow, SIZE_SIFT, 0);
/* read_predictions_from_csv(measurements_3, filename_in_3, SIZE_SIFT); */
} else {
printf("No argument specified");
return -1;
}
/* Create and initialize particles */
struct particle particles[N];
init_particles(particles);
struct particle particles_backward[N];
init_particles(particles_backward);
/* Run particle filter for every measurement */
int i = 0, j = 0, k;
FILE *fp = fopen(filename_out, "w");
fprintf(fp, "x,y\n");
/* Tables for dynamic programming */
struct particle ps_forward[SIZE_SIFT];
struct particle ps_backward[SIZE_SIFT];
struct particle uncertainties[SIZE_SIFT];
/* Fill in dynamic programming table */
for (i = 0; i < SIZE_SIFT; i++) {
printf(" iteration: %d\n", i);
fflush(stdout);
/* Use predictions form multiple regressors */
if (use_multiple_predictions) {
particle_filter_multiple(particles, &measurements[i], &measurements_2[i], use_variance);
} else {
particle_filter(particles, &measurements[i], &opticalflow[i], use_variance, use_flow);
}
/* Forward-backward smoothing */
if (use_smoothing) {
k = SIZE_SIFT - i;
particle_filter(particles_backward, &measurements[k], &opticalflow[i], use_variance, use_flow);
struct particle p_backward = weighted_average(particles_backward, N);
ps_backward[k] = p_backward;
}
struct particle p_forward = weighted_average(particles, N);
uncertainties[i] = calc_uncertainty(particles, p_forward, N);
printf("Uncertainty: x: %f y: %f", uncertainties[i].x, uncertainties[i].y);
ps_forward[i] = p_forward;
}
for (i = 0; i < SIZE_SIFT; i++) {
struct particle p;
printf("measurement %f %f\n", measurements[i].x, measurements[i].y);
if (use_smoothing)
p = weight_forward_backward(ps_forward[i], ps_backward[i], i, (SIZE_SIFT - i));
else
p = ps_forward[i];
printf("x: %f y: %f", p.x, p.y);
fprintf(fp, "%f,%f\n" , p.x, p.y);
}
fclose(fp);
return 0;
}
