convert c++ code to java below particle_filtercpp Created on Feb 29 2020 Author Kamiar Kordari #include particle_filterh#include mathh#include algorithm#include iostream#include iterator#include n

/**

• particle_filter.java
• Created on: Feb 29, 2020
• Author: Kamiar Kordari */

import java.util.ArrayList; import java.util.List; import java.util.Random;

public class ParticleFilter {

private int num_particles;
private boolean is_initialized;
private List<Particle> particles;
private List<Double> weights;

public class Particle {
    int id;
    double x;
    double y;
    double theta;
    double weight;
    List<Integer> associations;
    List<Double> sense_x;
    List<Double> sense_y;
    
    Particle() {
        associations = new ArrayList<Integer>();
        sense_x = new ArrayList<Double>();
        sense_y = new ArrayList<Double>();
    }
}

public class LandmarkObs {
    int id;
    double x;
    double y;
}

public class Map {
    List<LandmarkObs> landmark_list;
    
    Map() {
        landmark_list = new ArrayList<LandmarkObs>();
    }
}

public void init(double x, double y, double theta, double[] std) {
    /**
     * Set the number of particles. Initialize all particles to
     *   first position (based on estimates of x, y, theta and their uncertainties
     *   from GPS) and all weights to 1.
     * Add random Gaussian noise to each particle.
     */
    num_particles = 100;
    
    Random rand = new Random();
    
    double N_x, N_y, N_theta;
    
    for (int i = 0; i < num_particles; i++) {
        Particle particle = new Particle();
        particle.id = i;
        N_x = rand.nextGaussian() * std[0] + x;
        N_y = rand.nextGaussian() * std[1] + y;
        N_theta = rand.nextGaussian() * std[2] + theta;
        particle.x = N_x;
        particle.y = N_y;
        particle.theta = N_theta;
        particle.weight = 1;
        
        particles.add(particle);
        weights.add(1.0);
    }
    
    is_initialized = true;
}

public void prediction(double delta_t, double[] std_pos, double velocity, double yaw_rate) {
    /**
     * Add measurements to each particle and add random Gaussian noise.
     */
    
    Random rand = new Random();
    
    double new_x, new_y, new_theta;
    
    for (int i = 0; i < num_particles; i++) {
        if (Math.abs(yaw_rate) < 0.0001) {
            new_theta = particles.get(i).theta;
            new_x = particles.get(i).x + velocity * delta_t * Math.cos(new_theta);
            new_y = particles.get(i).y + velocity * delta_t * Math.sin(new_theta);
        } else {
            new_theta = particles.get(i).theta + yaw_rate * delta_t;
            new_x = particles.get(i).x + velocity / yaw_rate * (Math.sin(new_theta) - Math.sin(particles.get(i).theta));
            new_y = particles.get(i).y + velocity / yaw_rate * (Math.cos(particles.get(i).theta) - Math.cos(new_theta));
        }
        
        Random rand_pos = new Random();
        
        double N_x = rand_pos.nextGaussian() * std_pos[0] + new_x;
        double N_y = rand_pos.nextGaussian() * std_pos[1] + new_y;
        double N_theta = rand_pos.nextGaussian() * std_pos[2] + new_theta;
        
        particles.get(i).x = N_x;
        particles.get(i).y = N_y;
        particles.get(i).theta = N_theta;
    }
}

public void updateWeights(double sensor_range, double[] std_landmark, List<LandmarkObs> observations, Map map_landmarks) {
    /**
     * Update weights of particles
     */
    
    LandmarkObs converted_obs;
    LandmarkObs best_landmark;
    
    weights.clear();
    
    for (int i = 0; i < particles.size(); i++) {
        double prob = 1.0;
        
        for (int j = 0; j < observations.size(); j++) {
            // Convert observation to map coordinate
            converted_obs = transformCoords(particles.get(i), observations.get(j));
            
            // Associate observation to a landmark
            best_landmark = dataAssociation(converted_obs, map_landmarks, std_landmark);
            
            // Calculate weight
            double w = calculateWeights(converted_obs, best_landmark, std_landmark);
            
            // Get final weight by multiplying all the probabilities
            prob *= w;
        }
        
        particles.get(i).weight = prob;
        weights.add(prob);
    }
}

public LandmarkObs transformCoords(Particle p, LandmarkObs obs) {
    /**
     * Convert coordinates from particle to map
     */
    LandmarkObs transformed_coords = new LandmarkObs();
    
    transformed_coords.id = obs.id;
    transformed_coords.x = obs.x * Math.cos(p.theta) - obs.y * Math.sin(p.theta) + p.x;
    transformed_coords.y = obs.x * Math.sin(p.theta) + obs.y * Math.cos(p.theta) + p.y;
    
    return transformed_coords;
}

public LandmarkObs dataAssociation(LandmarkObs converted_obs, Map map_landmarks, double[] std_landmark) {
    /**
     * Associate measurement with a landmark
     */
    
    LandmarkObs best_landmark = new LandmarkObs();
    double min_dist = 0.0;
    
    for (int i = 0; i < map_landmarks.landmark_list.size(); i++) {
        double distance = dist(converted_obs.x, converted_obs.y, map_landmarks.landmark_list.get(i).x, map_landmarks.landmark_list.get(i).y);
        if (i == 0) {
            min_dist = distance;
            best_landmark.id = map_landmarks.landmark_list.get(i).id;
            best_landmark.x = map_landmarks.landmark_list.get(i).x;
            best_landmark.y = map_landmarks.landmark_list.get(i).y;
        } else if (distance < min_dist) {
            min_dist = distance;
            best_landmark.id = map_landmarks.landmark_list.get(i).id;
            best_landmark.x = map_landmarks.landmark_list.get(i).x;
            best_landmark.y = map_landmarks.landmark_list.get(i).y;
        }
    }
    
    return best_landmark;
}

public double calculateWeights(LandmarkObs obs, LandmarkObs best_landmark, double[] std_landmark) {
    /**
     * Calculate the weight value of the particle
     */
    
    double sigma_x = std_landmark[0];
    double sigma_y = std_landmark[1];
    double gauss_norm = 1 / (2 * Math.PI * sigma_x * sigma_y);
    
    double exponent = (Math.pow(obs.x - best_landmark.x, 2) / (2 * Math.pow(sigma_x, 2)))
            + (Math.pow(obs.y - best_landmark.y, 2) / (2 * Math.pow(sigma_y, 2)));
    
    double weight = gauss_norm * Math.exp(-exponent);
    
    return weight;
}

public void resample() {
    /**
     * Resample particles with replacement with probability proportional to their weight
     */
    Random rand = new Random();
    List<Particle> resample_particles = new ArrayList<Particle>();
    
    double[] weights_arr = new double[weights.size()];
    for (int i = 0; i < weights.size(); i++) {
        weights_arr[i] = weights.get(i);
    }
    
    int[] indices = new int[num_particles];
    for (int i = 0; i < num_particles; i++) {
        indices[i] = sample(weights_arr, rand);
    }
    
    for (int i = 0; i < num_particles; i++) {
        resample_particles.add(particles.get(indices[i]));
    }
    
    particles = resample_particles;
    
}

private int sample(double[] weights, Random rand) {
    double sum = 0.0;
    for (double weight : weights) {
        sum += weight;
    }
    double random_value = rand.nextDouble() * sum;
    
    int index = 0;
    sum = weights[0];
    while (sum < random_value) {
        index++;
        sum += weights[index];
    }
    
    return index;
}

public void SetAssociations(Particle particle, List<Integer> associations, List<Double> sense_x, List<Double> sense_y) {
    // particle: the particle to which assign each listed association,
    //   and association's (x,y) world coordinates mapping
    // associations: The landmark id that goes along with each listed association
    // sense_x: the associations x mapping already converted to world coordinates
    // sense_y: the associations y mapping already converted to world coordinates
    particle.associations = associations;
    particle.sense_x = sense_x;
    particle.sense_y = sense_y;
}

public String getAssociations(Particle best) {
    List<Integer> v = best.associations;
    StringBuilder sb = new StringBuilder();
    for (int i = 0; i < v.size(); i++) {
        sb.append(v.get(i));
        sb.append(" ");
    }
    String s = sb.toString();
    s = s.substring(0, s.length() - 1);  // get rid of the trailing space
    return s;
}

public String getSenseCoord(Particle best, String coord) {
    List<Double> v;
    
    if (coord.equals("X")) {
        v = best.sense_x;
    } else {
        v = best.sense_y;
    }
    
    StringBuilder sb = new StringBuilder();
    for (int i = 0; i < v.size(); i++) {
        sb.append(v.get(i));
        sb.append(" ");
    }
    String s = sb.toString();
    s = s.substring(0, s.length() - 1);  // get rid of the trailing space
    return s;
}

private double dist(double x1, double y1, double x2, double y2) {
    return Math.sqrt(Math.pow(x2 - x1, 2) + Math.pow(y2 - y1, 2));
}

}