first commit for Patti

Makefile

@@ -0,0 +1,16 @@
+CXX = clang++
+
+CXXFLAGS = -I/opt/local/include/eigen3/	
+
+.PHONY: plot all
+
+all: collision_probabilty
+
+collision_probabilty: collision_probabilty.cpp
+	$(CXX) $(CXXFLAGS) collision_probabilty.cpp -o collision_probabilty
+	
+samples.dat: collision_probabilty
+	./collision_probabilty > samples.dat
+	
+plot: samples.dat
+	gnuplot -c gnuplot.script

collision_probabilty.cpp

@@ -0,0 +1,200 @@
+#include <iostream>
+#include <iomanip>
+#include <string>
+#include <map>
+#include <random>
+#include <cmath>
+#include <cstring>
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+using namespace std;
+using namespace Eigen;
+
+typedef Matrix<double, 2, Dynamic> VertexList;
+
+#define N 1000
+
+typedef bool(*CollisionFunction)(const Vector3d &a, const Vector3d &b);
+
+/* Generate N-dimensional normal distributed samples */
+class MultivariateNormalDistribution : normal_distribution<double> {
+
+protected:
+	mt19937 gen;
+	
+	Matrix<double, Dynamic, Dynamic> covar;
+	Matrix<double, Dynamic, Dynamic> transform;
+	Matrix<double, Dynamic, 1> mean;
+
+	// drawback: this creates a useless eigenSolver when using Cholesky decomposition, but it yields access to eigenvalues and vectors
+	
+
+public:
+	MultivariateNormalDistribution(const Matrix<double, Dynamic, 1> &m, const Matrix<double, Dynamic, Dynamic> &c, const mt19937 &g) :
+		gen(g),
+		mean(m),
+		covar(c),
+		normal_distribution<double>(0, 1)
+	{
+		LLT<Matrix<double, Dynamic, Dynamic> > cholSolver(covar);
+		// We can only use the cholesky decomposition if 
+		// the covariance matrix is symmetric, pos-definite.
+		// But a covariance matrix might be pos-semi-definite.
+		// In that case, we'll go to an EigenSolver
+		if (cholSolver.info() == Success) {
+			// Use cholesky solver
+			transform = cholSolver.matrixL();
+		}
+		else {
+			SelfAdjointEigenSolver<Matrix<double, Dynamic, Dynamic> > eigenSolver(covar);
+			transform = eigenSolver.eigenvectors() * eigenSolver.eigenvalues().cwiseMax(0).cwiseSqrt().asDiagonal();
+		}
+	}
+
+	Vector3d randc()
+	{
+		Vector3d p, q;
+
+		/* Get vector of standard normal distributed samples */
+		for (int i = 0; i < p.rows(); i++)
+			p[i] = operator()(gen);
+		
+		return transform * p + mean;
+	}
+};
+
+/* Is point "t" in polygon "v"?
+ * From: https://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html */
+bool pnpoly(const VertexList &v, const Vector2d &t)
+{
+	int i, j;
+	bool c = false;
+
+	for (i = 0, j = v.cols()-1; i < v.cols(); j = i++) {
+		if (
+			((v(1,i) > t(1)) != (v(1,j) > t(1))) &&
+			(t(0) < (v(0,j) - v(0,i)) * (t(1) - v(1,i)) / (v(1,j) - v(1,i)) + v(0,i))
+		)
+			c = !c;
+	}
+
+	return c;
+}
+
+/* Calculate vertices of car polygon
+ * vertx, verty will be filled */
+VertexList poly_vehicle(const Vector3d &car)
+{
+	Rotation2D<double> rot(car[2]);
+
+	VertexList vertices(2, 4);
+	Vector2d offset = car.head(2);
+	
+	double scale = 1.5;
+	
+	vertices.col(0) <<  2*scale,  1*scale;
+	vertices.col(1) << -2*scale,  1*scale;
+	vertices.col(2) << -2*scale, -1*scale;
+	vertices.col(3) <<  2*scale, -1*scale;
+	
+	for (int i = 0; i < vertices.cols(); i++)
+		vertices.col(i) = rot * vertices.col(i) + offset;
+	return vertices;
+}
+
+bool collision_vehicle(const Vector3d &vehicle, const Vector3d &obstacle)
+{
+	/* Create list of X/Y vertex points of car */
+	VertexList vertices_vehicle, vertices_obstacle;
+	
+	vertices_vehicle = poly_vehicle(vehicle);
+	vertices_obstacle = poly_vehicle(obstacle);
+	
+	/* Check if any of the vertices of the obstacle car is inside the vehicle polygon */
+	for (int i = 0; i < vertices_obstacle.cols(); i++) {
+		if (pnpoly(vertices_vehicle, vertices_obstacle.col(i)) > 0)
+			return true;
+	}
+
+	return false;
+}
+
+bool collision_circle(const Vector3d &vehicle, const Vector3d &obstacle)
+{
+	const double minDistance = 5;
+	
+	Vector3d delta = vehicle - obstacle;
+
+	return delta.norm() < minDistance; /* Collision occured if distance (r) is smaller than 1 */
+}
+
+double collision_probability(CollisionFunction cfunc, MultivariateNormalDistribution &vehicle, MultivariateNormalDistribution &obstacle)
+{
+	unsigned collisions = 0;
+	
+	for (unsigned j = 0; j < N; j++) {
+		Vector3d sample_vehicle, sample_obstacle;
+		VertexList vertices_vehicle, vertices_obstacle;
+
+		sample_vehicle = vehicle.randc();
+		sample_obstacle = obstacle.randc();
+		
+		vertices_vehicle = poly_vehicle(sample_vehicle);
+		vertices_obstacle = poly_vehicle(sample_obstacle);
+		
+		bool coll = cfunc(sample_vehicle, sample_obstacle);
+		
+		/* Print vehicle centers */
+		cout << ((coll) ? 1 : 0) << " "
+			<< sample_vehicle.transpose() << " "
+			<< sample_obstacle.transpose();
+		
+		/* Print vertices of vehicles */
+		for (int i = 0; i < 4; i++)
+			cout << " " << vertices_vehicle.col(i).transpose();
+		for (int i = 0; i < 4; i++)
+			cout << " " << vertices_obstacle.col(i).transpose();
+		
+		cout << endl;
+		
+		if (coll)
+			collisions++;
+	}
+	
+	cout << "# collisions = " << collisions << endl;
+	
+	return (double) collisions / N;
+}
+
+int main(int argc, char *argv[]) {
+	/* Initialize PRNG */
+	random_device rd;
+	mt19937 gen(rd());
+
+	Vector3d mean_vehicle, mean_obstacle;
+	Matrix3d covar_vehicle, covar_obstacle;
+		
+	mean_vehicle  <<  5, 0, 3.0/4.0 * M_PI; // x, y, heading
+	mean_obstacle <<  0, 0, 1.0/4.0 * M_PI; // x, y, heading
+	
+	covar_vehicle  << 5, -4,  0,
+	                 -4,  9,  0,
+	                  0,  0,  1.0/16.0*M_PI;
+
+	covar_obstacle << 3,  2,  0,
+	                  2,  3,  0,
+	                  0,  0,  1.0/16.0*M_PI;
+	
+	CollisionFunction cfunc = collision_vehicle;
+	
+	MultivariateNormalDistribution vehicle(mean_vehicle, covar_vehicle, gen);
+	MultivariateNormalDistribution obstacle(mean_obstacle, covar_obstacle, gen);
+	
+	cout << "# Collision Vehicle_x Vehicle_y Obstacle_x Obstacle_y" << endl;
+	
+	double prob = collision_probability(cfunc, vehicle, obstacle);
+	
+	cout << "# collision probability is: " << prob << endl;
+}

gnuplot.script

@@ -0,0 +1,45 @@
+#reset
+#set term gif animate
+#set output "animate.gif"
+
+
+unset key
+set size ratio -1
+s = 0.4
+
+samples = `egrep -v ^# samples.dat | wc -l`
+collisions = `egrep ^1 samples.dat | wc -l`
+
+do for [tt=1:10] {
+  system "./collision_probabilty > samples.dat"
+  reread
+  
+  do for [pp=1:1] {
+    do for [ii=1:collisions-1] {
+      plot \
+        "samples.dat"            using 2:3 t "Vehicle"                                                lc rgb "red"  lt 7, \
+        "samples.dat"            using 5:6 t "Obstacle"                                               lc rgb "blue" lt 7, \
+        "samples.dat"            using 2:3:(s*cos($4)):(s*sin($4))                with vectors nohead lc rgb "red", \
+        "samples.dat"            using 5:6:(s*cos($7)):(s*sin($7))                with vectors nohead lc rgb "blue", \
+        "< egrep ^1 samples.dat" using 2:3:($5-$2):($6-$3)         every ::ii::ii with vectors nohead lw 2 lc rgb "green" t "Collisions", \
+        \
+        "< egrep ^1 samples.dat" using 8:9:($10-$8):($11-$9)       every ::ii::ii with vectors nohead lw 2 lc rgb "black", \
+        "< egrep ^1 samples.dat" using 10:11:($12-$10):($13-$11)   every ::ii::ii with vectors nohead lw 2 lc rgb "black", \
+        "< egrep ^1 samples.dat" using 12:13:($14-$12):($15-$13)   every ::ii::ii with vectors nohead lw 2 lc rgb "black", \
+        "< egrep ^1 samples.dat" using 14:15:($8-$14):($9-$15)     every ::ii::ii with vectors nohead lw 2 lc rgb "black", \
+        \
+        "< egrep ^1 samples.dat" using 16:17:($18-$16):($19-$17)   every ::ii::ii with vectors nohead lw 2 lc rgb "black", \
+        "< egrep ^1 samples.dat" using 18:19:($20-$18):($21-$19)   every ::ii::ii with vectors nohead lw 2 lc rgb "black", \
+        "< egrep ^1 samples.dat" using 20:21:($22-$20):($23-$21)   every ::ii::ii with vectors nohead lw 2 lc rgb "black", \
+        "< egrep ^1 samples.dat" using 22:23:($16-$22):($17-$23)   every ::ii::ii with vectors nohead lw 2 lc rgb "black"
+        
+        unset label 1
+        unset label 2
+        unset label 3
+        set label 1 "Total Samples = " . samples at graph 0.1,0.1
+        set label 2 "Total Collisions = " . collisions at graph 0.1,0.15
+        set label 3 "Showing Collision = " . ii at graph 0.1,0.2
+    }
+  }
+}
+set output