gistfile1.txt

/*
 * particle_filter.cpp
 *
 *  Created on: Dec 12, 2016
 *      Author: Tiffany Huang
 */
#define _USE_MATH_DEFINES

#include <random>
#include <algorithm>
#include <iostream>
#include <numeric>
#include <math.h>
#include <iostream>
#include <sstream>
#include <string>
#include <iterator>

#include "particle_filter.h"

using namespace std;

void ParticleFilter::init(double x, double y, double theta, double std[]) {
    // TODO: 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.
    // NOTE: Consult particle_filter.h for more information about this method (and others in this file).
    default_random_engine gen;
    num_particles = 1;

    //Create a Gaussian distribution for x, y, and theta
    normal_distribution<double> dist_x(x, std[0]);
    normal_distribution<double> dist_y(y, std[0]);
    normal_distribution<double> dist_theta(theta, std[0]);

    for (int i = 0; i < num_particles; ++i) {
        Particle particle;
        particle.id = i;
        particle.x = dist_x(gen);
        particle.y = dist_y(gen);
        particle.theta = dist_theta(gen);
        particle.weight = 1;

        particles.push_back(particle);
        weights.push_back(1);

    }

    is_initialized = true;


}

void ParticleFilter::prediction(double delta_t, double std_pos[], double velocity, double yaw_rate) {
    // TODO: Add measurements to each particle and add random Gaussian noise.
    // NOTE: When adding noise you may find std::normal_distribution and std::default_random_engine useful.
    //  http://en.cppreference.com/w/cpp/numeric/random/normal_distribution
    //  http://www.cplusplus.com/reference/random/default_random_engine/
    default_random_engine gen;
    for (int i = 0; i < num_particles; ++i) {

        double new_x;
        double new_y;
        double new_theta;

        // adding mearsurements for each particle
        if (yaw_rate == 0) {
            new_x = particles[i].x + velocity * delta_t * cos(particles[i].theta);
            new_y = particles[i].y + velocity * delta_t * sin(particles[i].theta);
            new_theta = particles[i].theta;

        } else {
            new_x = particles[i].x +
                    (velocity / yaw_rate) * (sin(particles[i].theta + yaw_rate * delta_t) - sin(particles[i].theta));
            new_y = particles[i].y +
                    (velocity / yaw_rate) * (cos(particles[i].theta) - cos(particles[i].theta + yaw_rate * delta_t));
            new_theta = particles[i].theta + yaw_rate * delta_t;

        }

        //Adding random Gaussian noise to ensure particles don't collapse to the same measurement
        normal_distribution<double> dist_x(new_x, std_pos[0]);
        normal_distribution<double> dist_y(new_y, std_pos[0]);
        normal_distribution<double> dist_theta(new_theta, std_pos[0]);

        particles[i].x = dist_x(gen);
        particles[i].y = dist_y(gen);
        particles[i].theta = dist_theta(gen);

    }
}

//void ParticleFilter::dataAssociation(std::vector<LandmarkObs> predicted, std::vector<LandmarkObs> &observations) {
//    // TODO: Find the predicted measurement that is closest to each observed measurement and assign the
//    //   observed measurement to this particular landmark.
//    // NOTE: this method will NOT be called by the grading code. But you will probably find it useful to
//    //   implement this method and use it as a helper during the updateWeights phase.
//}

void ParticleFilter::updateWeights(double sensor_range, double std_landmark[],
                                   std::vector<LandmarkObs> observations, Map map_landmarks) {
    // TODO: Update the weights of each particle using a mult-variate Gaussian distribution. You can read
    //   more about this distribution here: https://en.wikipedia.org/wiki/Multivariate_normal_distribution
    // NOTE: The observations are given in the VEHICLE'S coordinate system. Your particles are located
    //   according to the MAP'S coordinate system. You will need to transform between the two systems.
    //   Keep in mind that this transformation requires both rotation AND translation (but no scaling).
    //   The following is a good resource for the theory:
    //   https://www.willamette.edu/~gorr/classes/GeneralGraphics/Transforms/transforms2d.htm
    //   and the following is a good resource for the actual equation to implement (look at equation
    //   3.33
    //   http://planning.cs.uiuc.edu/node99.html

    //Denominator of multivariate gaussian does not change, keep outside of for loops

    double mg_denominator = 1 / (2 * M_PI * std_landmark[0] * std_landmark[1]);
    //loop through all particles
    for (int i = 0; i < num_particles; ++i) {
        double multi_gauss = 1;
        cout<<observations.size()<<endl;

        //loop through all observations
        for (int j = 0; j < observations.size(); ++j) {

            //convert observations from car coordinate system to map coordinate system
            double obs_map_x = particles[i].x + (cos(particles[i].theta) * observations[j].x) -
                               (sin(particles[i].theta) * observations[j].y);
            double obs_map_y = particles[i].y + (sin(particles[i].theta) * observations[j].x) +
                               (cos(particles[i].theta) * observations[j].y);

            //find the closest landmark
            vector<Map::single_landmark_s> landmarks = map_landmarks.landmark_list;
            vector<double> landmark_object_distance(landmarks.size());

            for (int k = 0; k < landmarks.size(); ++k) {

                double landmark_particle_distance = dist(landmarks[k].x_f, landmarks[k].y_f, particles[i].x,
                                                         particles[i].y);

                if (landmark_particle_distance <= sensor_range) {

                    landmark_object_distance[k] = dist(landmarks[k].x_f, landmarks[k].y_f, obs_map_x, obs_map_y);
                } else {
                    //large number to ensure that objects that are too far are considered closest
                    landmark_object_distance[k] = numeric_limits<double>::max();

                }

            }

            //nearest landmark neighbor

            int nearest = distance(landmark_object_distance.begin(),
                                   min_element(landmark_object_distance.begin(), landmark_object_distance.end()));

            double mu_x = landmarks[nearest].x_f;
            double mu_y = landmarks[nearest].y_f;


            //calculate gaussian
            double gauss_x = obs_map_x - mu_x;
            double gauss_y = obs_map_y - mu_y;
            multi_gauss *= (1 / mg_denominator) * exp(-(pow(gauss_x, 2) / (2 * pow(std_landmark[0], 2)) +
                                                        pow(gauss_y, 2) / (2 * pow(std_landmark[1], 2))));

        }


        particles[i].weight = multi_gauss;
        weights[i] = particles[i].weight;

    }
}

void ParticleFilter::resample() {
    // TODO: Resample particles with replacement with probability proportional to their weight.
    // NOTE: You may find std::discrete_distribution helpful here.
    //   http://en.cppreference.com/w/cpp/numeric/random/discrete_distribution
    default_random_engine gen;
    discrete_distribution<int> distribution(weights.begin(), weights.end());

    vector<Particle> resample_particles;

    for (int i = 0; i < num_particles; ++i) {
        resample_particles.push_back(particles[distribution(gen)]);
    }
    particles = resample_particles;

}

Particle ParticleFilter::SetAssociations(Particle particle, std::vector<int> associations, std::vector<double> sense_x,
                                         std::vector<double> sense_y) {
    //particle: the particle to assign each listed association, and association's (x,y) world coordinates mapping to
    // 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

    //Clear the previous associations
    particle.associations.clear();
    particle.sense_x.clear();
    particle.sense_y.clear();

    particle.associations = associations;
    particle.sense_x = sense_x;
    particle.sense_y = sense_y;

    return particle;
}

string ParticleFilter::getAssociations(Particle best) {
    vector<int> v = best.associations;
    stringstream ss;
    copy(v.begin(), v.end(), ostream_iterator<int>(ss, " "));
    string s = ss.str();
    s = s.substr(0, s.length() - 1);  // get rid of the trailing space
    return s;
}

string ParticleFilter::getSenseX(Particle best) {
    vector<double> v = best.sense_x;
    stringstream ss;
    copy(v.begin(), v.end(), ostream_iterator<float>(ss, " "));
    string s = ss.str();
    s = s.substr(0, s.length() - 1);  // get rid of the trailing space
    return s;
}

string ParticleFilter::getSenseY(Particle best) {
    vector<double> v = best.sense_y;
    stringstream ss;
    copy(v.begin(), v.end(), ostream_iterator<float>(ss, " "));
    string s = ss.str();
    s = s.substr(0, s.length() - 1);  // get rid of the trailing space
    return s;
}