convert c++ to java code below helper_functionsh Some helper functions for the 2D particle filter Created on Dec 13 2016 Author Tiffany Huang #ifndef HELPER_FUNCTIONS_H_#define HELPER_FUNCTIONS_H

/**

• helper_functions.java
• Some helper functions for the 2D particle filter.
• Created on: Dec 13, 2016
• Author: Tiffany Huang */

import java.io.BufferedReader; import java.io.FileReader; import java.io.IOException; import java.util.ArrayList; import java.util.List;

public class HelperFunctions {

public static final double M_PI = 3.14159265358979323846;

/**
 * Struct representing one position/control measurement.
 */
public static class Control {
    public double velocity;  // Velocity [m/s]
    public double yawrate;   // Yaw rate [rad/s]
}

/**
 * Struct representing one ground truth position.
 */
public static class GroundTruth {
    public double x;     // Global vehicle x position [m]
    public double y;     // Global vehicle y position
    public double theta; // Global vehicle yaw [rad]
}

/**
 * Struct representing one landmark observation measurement.
 */
public static class LandmarkObs {
    public int id;     // Id of matching landmark in the map.
    public double x;   // Local (vehicle coords) x position of landmark observation [m]
    public double y;   // Local (vehicle coords) y position of landmark observation [m]
}

/**
 * Computes the Euclidean distance between two 2D points.
 * @param x1, y1 x and y coordinates of first point
 * @param x2, y2 x and y coordinates of second point
 * @return Euclidean distance between two 2D points
 */
public static double dist(double x1, double y1, double x2, double y2) {
    return Math.sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
}

/**
 * Computes the error between ground truth and particle filter data.
 * @param gt_x, gt_y, gt_theta x, y and theta of ground truth
 * @param pf_x, pf_y, pf_theta x, y and theta of particle filter
 * @return Error between ground truth and particle filter data.
 */
public static double[] getError(double gt_x, double gt_y, double gt_theta, double pf_x,
                                double pf_y, double pf_theta) {
    double[] error = new double[3];
    error[0] = Math.abs(pf_x - gt_x);
    error[1] = Math.abs(pf_y - gt_y);
    error[2] = Math.abs(pf_theta - gt_theta);
    error[2] = error[2] % (2.0 * M_PI);
    if (error[2] > M_PI) {
        error[2] = 2.0 * M_PI - error[2];
    }
    return error;
}

/**
 * Reads map data from a file.
 * @param filename Name of file containing map data.
 * @return True if opening and reading file was successful
 */
public static boolean readMapData(String filename, Map map) {
    try {
        BufferedReader br = new BufferedReader(new FileReader(filename));
        String line;
        while ((line = br.readLine()) != null) {
            String[] values = line.split(" ");
            double landmark_x_f = Double.parseDouble(values[0]);
            double landmark_y_f = Double.parseDouble(values[1]);
            int id_i = Integer.parseInt(values[2]);
            Map.SingleLandmark single_landmark_temp = new Map.SingleLandmark(id_i, landmark_x_f, landmark_y_f);
            map.landmark_list.add(single_landmark_temp);
        }
        br.close();
        return true;
    } catch (IOException e) {
        e.printStackTrace();
        return false;
    }
}

/**
 * Reads control data from a file.
 * @param filename Name of file containing control measurements.
 * @return True if opening and reading file was successful
 */
public static boolean readControlData(String filename, List<Control> position_meas) {
    try {
        BufferedReader br = new BufferedReader(new FileReader(filename));
        String line;
        while ((line = br.readLine()) != null) {
            String[] values = line.split(" ");
            double velocity = Double.parseDouble(values[0]);
            double yawrate = Double.parseDouble(values[1]);
            Control meas = new Control();
            meas.velocity = velocity;
            meas.yawrate = yawrate;
            position_meas.add(meas);
        }
        br.close();
        return true;
    } catch (IOException e) {
        e.printStackTrace();
        return false;
    }
}

/**
 * Reads ground truth data from a file.
 * @param filename Name of file containing ground truth.
 * @return True if opening and reading file was successful
 */
public static boolean readGtData(String filename, List<GroundTruth> gt) {
    try {
        BufferedReader br = new BufferedReader(new FileReader(filename));
        String line;
        while ((line = br.readLine()) != null) {
            String[] values = line.split(" ");
            double x = Double.parseDouble(values[0]);
            double y = Double.parseDouble(values[1]);
            double azimuth = Double.parseDouble(values[2]);
            GroundTruth single_gt = new GroundTruth();
            single_gt.x = x;
            single_gt.y = y;
            single_gt.theta = azimuth;
            gt.add(single_gt);
        }
        br.close();
        return true;
    } catch (IOException e) {
        e.printStackTrace();
        return false;
    }
}

/**
 * Reads landmark observation data from a file.
 * @param filename Name of file containing landmark observation measurements.
 * @return True if opening and reading file was successful
 */
public static boolean readLandmarkData(String filename, List<LandmarkObs> observations) {
    try {
        BufferedReader br = new BufferedReader(new FileReader(filename));
        String line;
        while ((line = br.readLine()) != null) {
            String[] values = line.split(" ");
            double local_x = Double.parseDouble(values[0]);
            double local_y = Double.parseDouble(values[1]);
            LandmarkObs meas = new LandmarkObs();
            meas.x = local_x;
            meas.y = local_y;
            observations.add(meas);
        }
        br.close();
        return true;
    } catch (IOException e) {
        e.printStackTrace();
        return false;
    }
}

}