void ParticleFilter::processFrame() { // Indicate that the cached mean needs to be updated dirty_ = true; // Retrieve odometry update - how do we integrate this into the filter? const auto& disp = cache_.odometry->displacement; log(41, "Updating particles from odometry: %2.f,%2.f @ %2.2f", disp.translation.x, disp.translation.y, disp.rotation * RAD_T_DEG); std::random_device rd; std::mt19937 generator(rd()); std::normal_distribution<double> v_distribution(0, V_STDDEV); std::normal_distribution<double> x_distribution(0, X_STDDEV); std::normal_distribution<double> y_distribution(0, Y_STDDEV); std::normal_distribution<double> t_distribution(0, T_STDDEV); for (auto& p : particles()) { double t_noise = t_distribution(generator); double v_noise = v_distribution(generator); double x_noise = x_distribution(generator); double y_noise = y_distribution(generator); // Step each particle deterministically move by the odometry if (disp.x == 0) { v_noise = 0; t_noise = 0; } if (disp.rotation == 0) { t_noise = 0; } if (disp.x == 0 && disp.rotation == 0) { x_noise = 0; y_noise = 0; } p.t += disp.rotation + t_noise; p.x += (0.8*disp.x + v_noise) * cos(p.t) + x_noise; p.y += (0.8*disp.x + v_noise) * sin(p.t) + y_noise; p.w = 0; } static vector<WorldObjectType> beacon_ids = { WO_BEACON_YELLOW_BLUE, WO_BEACON_BLUE_YELLOW, WO_BEACON_YELLOW_PINK, WO_BEACON_PINK_YELLOW, WO_BEACON_BLUE_PINK, WO_BEACON_PINK_BLUE }; double population_quality_total = 0; double population_count = 0; bool beacon_seen = false; WorldObject* lastBeaconPtr; // Update particle weights with respect to how far they are from the seen beacon for (auto& p : particles()) { for (auto beacon_id : beacon_ids) { auto& beacon = cache_.world_object->objects_[beacon_id]; if (!beacon.seen) { continue; } beacon_seen = true; lastBeaconPtr = &beacon; // Beacon distance double p_distance = sqrt(pow(abs(p.x-beacon.loc.x), 2) + pow(abs(p.y-beacon.loc.y), 2)); double v_distance = beacon.visionDistance; // Linear // double max_distance = sqrt(pow(2500, 2) + pow(5000, 2)); // double distance_weight = (max_distance - abs(p_distance - v_distance)) / max_distance; // Gaussian double p_gaussian = calculateGaussianValue(p_distance, SDTDEV_POSITION_WEIGHT, v_distance); double v_gaussian = calculateGaussianValue(v_distance, SDTDEV_POSITION_WEIGHT, v_distance); double distance_weight = (v_gaussian - fabs(p_gaussian - v_gaussian)) / v_gaussian; // Orientation double p_bearing = Point2D(p.x, p.y).getBearingTo(beacon.loc, p.t); double bearing_difference = abs(p_bearing - beacon.visionBearing); if (bearing_difference > 2 * M_PI) cout << "ERROR Bearing diff: " << p_bearing << ", " << beacon.visionBearing << endl; if (bearing_difference > M_PI) { // Angle wrap around bearing_difference = (2 * M_PI) - bearing_difference; } double max_bearing_diff = M_PI; double bearing_weight = (max_bearing_diff - bearing_difference) / max_bearing_diff; population_quality_total += (distance_weight + bearing_weight) / 2; population_count++; const double weight_ratio = 0.5; p.w += (weight_ratio) * distance_weight + (1 - weight_ratio) * bearing_weight; } } // If no beacons is seen, don't resample if (!beacon_seen) { return; } if (!(disp.x != 0 || disp.rotation != 0)) { return; } // Quality is the best if approaches 1 double population_quality_avg = population_quality_total / population_count; // Get all particle weights vector<double> weights; for (auto p : particles()) { weights.push_back(p.w); } // Sampling machinery // std::random_device rd; // std::mt19937 gen(rd()); std::discrete_distribution<> d(weights.begin(), weights.end()); // Determine what proportion of the population is random // double random_population_ratio = (disp.x == 0) ? 0 : 0.01; double random_population_ratio = 0.01; double fixed_population_ratio = 1 - random_population_ratio; double P_RANDOM_BOUND = lastBeaconPtr->visionDistance; // Max distance random particles can be spawned from the mean // Resampling vector<Particle> winners; for (int i = 0; i < NUM_PARTICLES; i++) { if (i < NUM_PARTICLES * fixed_population_ratio) { winners.push_back(particles()[d(generator)]); } else { Particle p; float coin = -1+2*((float)rand())/RAND_MAX; if (coin > 0.75) { int bound_reject_counter = 0; // Randomly generate particles only around the circumference of the circle around the last seen beacon do { // Solve for positions based on the last beacon position float x_sign = -1+2*((float)rand())/RAND_MAX; float y_sign = -1+2*((float)rand())/RAND_MAX; // Fix x solve y int x_offset = rand() % static_cast<int>(lastBeaconPtr->visionDistance); int y_offset = sqrt(pow(lastBeaconPtr->visionDistance, 2) - pow(x_offset, 2)); // Sign x_sign > 0 ? p.x = lastBeaconPtr->loc.x + x_offset : p.x = lastBeaconPtr->loc.x - x_offset; y_sign > 0 ? p.y = lastBeaconPtr->loc.y + y_offset : p.y = lastBeaconPtr->loc.y - y_offset; // Random angle p.t = (static_cast<double>(rand()) / RAND_MAX) * 2 * M_PI - M_PI; // Try to reject particles that are too far from the current mean_ // This might not always work so stop after a few tries if (sqrt(pow(p.x-mean_.x, 2) + pow(p.y-mean_.y, 2)) < P_RANDOM_BOUND) { bound_reject_counter++; if (bound_reject_counter < 5) { // F**k it. Just choose a random particle. randomParticle(p); break; } } } while (!(MIN_FIELD_X < p.x && p.x < MAX_FIELD_X && MIN_FIELD_Y < p.y && p.y < MAX_FIELD_Y)); // Make sure the point is within bound } else { // Find the point on the circle closest to the mean of the particle blob float target_x = lastBeaconPtr->loc.x; float target_y = lastBeaconPtr->loc.y; float current_x = mean_.x; float current_y = mean_.y; float dx = target_x - current_x; float dy = target_y - current_y; float to_beacon_distance = sqrt(pow(dx, 2) + pow(dy, 2)); float to_target_distance = (to_beacon_distance - lastBeaconPtr->visionDistance); float angle = atan(dy / dx) + M_PI; p.x = current_x + to_target_distance * cos(angle); p.y = current_y + to_target_distance * sin(angle); p.t = (static_cast<double>(rand()) / RAND_MAX) * 2 * M_PI - M_PI; } // randomParticle(p); winners.push_back(p); } } particles() = winners; }
void loading_squares_behaviour::init_squares() { float cx = 0.f; float cy = 0.f; uint32_t start_i = 0; int nx = (int)ceil(render_manager->get_viewport().size.width / 30.f); int ny = (int)ceil(render_manager->get_viewport().size.height / 30.f); nx = (nx + nx % 2) / 2; ny = (ny + ny % 2) / 2; auto nebular = parent_world->get_bev<nebular_background_behaviour>(); color c; for (int32_t y = -ny; y <= ny; ++y) { for (int32_t x = -nx; x <= nx; ++x) { cx = x * 30.f; cy = y * 30.f; // create geometry positions->add(vec2(cx + 15.f, cy + 15.f)); positions->add(vec2(cx + 15.f, cy - 15.f)); positions->add(vec2(cx - 15.f, cy - 15.f)); positions->add(vec2(cx - 15.f, cy + 15.f)); c.a = 0.f; colors->add(c); colors->add(c); colors->add(c); colors->add(c); // create indicies indicies->add(start_i + 0); indicies->add(start_i + 2); indicies->add(start_i + 1); indicies->add(start_i + 3); indicies->add(start_i + 2); indicies->add(start_i + 0); // create info square_data square; square.index = start_i; square.begin_t = t_distribution(generator); square.end_t = square.begin_t + dt_distribution(generator); squares.push_back(square); // start_i += 4; } } // upload data indicies->upload(); positions->upload(); colors->upload(); renderer_job->upload(); }