ME597 ROS particle filtering for localization

gistfile1.txt

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <gazebo_msgs/ModelStates.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf/transform_datatypes.h>

#include <Eigen/Dense>
#include <math.h>
#include <time.h>
#include <iostream>
#include <vector>
#include <algorithm>
#include <limits>

using namespace Eigen;
using namespace visualization_msgs;

// Evaluates the normal PDF with provided mean and covariance at a given x
template<int k>
double normpdf(Matrix<double, k, 1> pose, Matrix<double, k, 1> mu, Matrix<double, k, k> sigma) {
    auto det = sigma.determinant();
    
    if (det == 0) {
        return pose == mu ? std::numeric_limits<double>::infinity() : 0;
    }
    
    auto diff = pose - mu;
    
    return (1 / sqrt(pow(2*M_PI, k) * det)) * exp(-0.5 * diff.transpose() * sigma.inverse() * diff);
}

// Given previous belief particle set, control signal, measurement and state transition function, compute new particle set
template<int k, int m>
Matrix<double, k, m> estimate(Matrix<double, k, m> belief, Matrix<double, k, 1> u, Matrix<double, k, 1> z, Matrix<double, k, k> Q, Matrix<double, k, 1> (*g)(Matrix<double, k, 1>, Matrix<double, k, 1>)) {
    // Initialize particle set matrices for prediction and final current belief
    auto x_p = Matrix<double, k, m>();
    auto x = Matrix<double, k, m>();
    auto w = std::vector<double>(m);

    // Iterate over previous belief particles
    for(int i = 0; i < m; i++) {
        // Use motion model to calculate new prediction
        Matrix<double, k, 1> x_p_i = g(belief.col(i), u);
        x_p.col(i) = x_p_i;
        
        // Work out non-normalized weights using gaussian pdf to estimate p(z | x_p)
        w[i] = normpdf(z, x_p_i, Q);
    }

    // Accumulate weights so it's easy to choose a random index
    std::partial_sum(w.begin(), w.end(), w.begin());
    // double max_w = *std::max_element(std::begin(w), std::end(w)); // Can probably get away with w.back() here, better perf
    double max_w = w.back();

    // Resample
    for(int i = 0; i < m; i++) {
        double rnd = ((double) rand() / RAND_MAX) * max_w;
        int j = *std::lower_bound(w.begin(), w.end(), rnd); // Find first element with weight greater than rnd

        x.col(i) = x_p.col(j);
    }
    return x;
}

// Our motion model. Simply adds the appropriately rotated velocity to the current position
Vector3d motion_model(Vector3d x, Vector3d u) {
    auto dt = 1;
    
    auto R = Matrix<double, 3, 3>();
    R << cos(x[2]), -sin(x[2]), 0, sin(x[2]), cos(x[2]), 0, 0, 0, 1;
    
    return x + dt * R * u;
}

// Initialize variables for u and z
Vector3d u;
Vector3d z;

// For actual IPS messages
// void pose_callback_live(const geometry_msgs::PoseWithCovarianceStamped msg, Vector3d& z) {
//     auto lin = msg.pose.pose.position;
//     auto yaw = tf::getYaw(msg.pose.pose.orientation);
//     z << lin.x, lin.y, yaw;
// }

// For gazebo "fake IPS" messages
void pose_callback_sim(const gazebo_msgs::ModelStates msg) {
    int i;
    
    for(i = 0; i < msg.name.size(); i++) if (msg.name[i] == "mobile_base") break;

    double ips_x = msg.pose[i].position.x;
    double ips_y = msg.pose[i].position.y;
    double ips_yaw = tf::getYaw(msg.pose[i].orientation);
    
    z << ips_x, ips_y, ips_yaw;
}


void odom_callback(const nav_msgs::Odometry msg) {
    auto lin = msg.twist.twist.linear;
    auto ang = msg.twist.twist.angular;
    u << lin.x, lin.y, ang.z;
};

template<int k, int m>
MarkerArray build_particleset_viz(Matrix<double, k, m> pset) {
    MarkerArray msg;
    msg.markers = std::vector<Marker>(m);
    for (int i = 0; i < m; i++) {
        auto marker = msg.markers[i];
        auto col = pset.col(i);
        
        marker.id = 0;
        marker.header.frame_id = "base_link";
        marker.header.stamp = ros::Time();
        marker.type = visualization_msgs::Marker::ARROW;
        marker.color.a = 1.0; // Don't forget to set the alpha!
        marker.color.r = 0.0;
        marker.color.g = 1.0;
        marker.color.b = 0.0;
        
        auto pos = marker.pose.position;
        auto ori = marker.pose.orientation;
        pos.x = col[0];
        pos.y = col[1];
        ori.z = col[2];       
    }
    
    return msg;
}

int main(int argc, char **argv)
{    
    // Double rand function for convenience
    srand (time(NULL));
    auto drand = []() -> double { return (double)rand() / RAND_MAX; };
    
    // Parameters for localization
    const auto m = 1000;
    const auto k = 3;
    auto Q = Matrix<double, 3, 3>();
    Q << 0.01, 0,    0,
         0,    0.01, 0,
         0,    0,    1e-20; // No idea what theta variance actually is
    
    // Initialize our belief (no clue where we are, just guess wildly)
    auto belief = Matrix<double, k, m>();
    for (int i = 0; i < m; i++) {
        // Going for a 10m x 10m world
        belief.col(i) << drand() * 10, drand() * 10, drand() * 2 * M_PI;
    }
    
    // Initialize the ROS framework
    ros::init(argc, argv, "localization");
    ros::NodeHandle n;
    
    // Publisher for belief particle set (queue size 0, don't really need msgs to queue up)
    auto particles_pub = n.advertise<MarkerArray>("/visualization_marker_array", 0);
    
    // Subscribers for odometry and IPS updates
    auto odo_sub = n.subscribe("/odom", 1, odom_callback);
    // auto ips_sub = n.subscribe("/indoor_pos", pos_callback_live);
    auto ips_sub = n.subscribe("/gazebo/model_states", 1, pose_callback_sim);

    // Set the loop rate
    ros::Rate loop_rate(1);

    while (ros::ok())
    {        
    	loop_rate.sleep(); //Maintain the loop rate
    	ros::spinOnce();   //Check for new messages

    	//Main loop code goes here
        belief = estimate(belief, u, z, Q, motion_model);
        
        particles_pub.publish(build_particleset_viz(belief));
    }

    return 0;
}