MONTE CARLO: Updates to support Monte Carlo Localization lab

src/cs4313_utilities/geometry.py

@@ -507,7 +507,7 @@ class Polygon(Geometry):
         closest = None
         min_d = 0.0
         for edge in self.edges:
-            closest_seg_pt = rm.closest_point_on_egmentt(edge[0], edge[1], point)
+            closest_seg_pt = rm.closest_point_on_segment(edge[0], edge[1], point)
             seg_d = rm.cartesian_distance(point, closest_seg_pt)
             if (not closest) or (seg_d < min_d):
                 closest = closest_seg_pt

src/cs4313_utilities/maps.py

@@ -341,18 +341,17 @@ class GeometryMap(Map):
 
     def nearest_obstacle(self, point):
         ''' Computes distance from a test point to the nearest mapped obstacle
-        @param point: Cartesian coordiantes of the test point
-        @return: tuple containing the closest obstacle point and the distance
+        @param point: Cartesian coordinates of the test point
+        @return: tuple with the nearsest obstacle and distance from the test point
         '''
-        closest_point = None
-        min_d = 0.0
+        obstacle = None
+        min_d = -1.0
         for geometry in self._geometries:
-            pt = geometry.closest_point(point)
-            d = self.distance(point, pt)
-            if (closest_point == None) or (d < min_d):
-                closest_point = pt
+            d = geometry.edge_distance(point)
+            if (not obstacle) or (d < min_d):
+                obstacle = geometry
                 min_d = d
-        return (closest_point, min_d)
+        return (geometry, min_d)
 
 
     def distance(self, state1, state2):
@@ -380,15 +379,12 @@ class GeometryMap(Map):
                 test_pt = \
                     rm.projectSensorReturnToWorld(state, self._sensor_posit, \
                                                   rng, theta_sense)
-                distance = rm.BIGNUM
-                for obstacle in self._geometries:
-                    d_obs = obstacle.edge_distance(test_pt)
-                    if d_obs < distance:
-                        distance = d_obs
-                result *= rm.likelihoodFieldModel(distance, self._z_max, \
-                                                  self._sigma_like, \
-                                                  self._alpha_hit_like, \
-                                                  self._alpha_rand_like)
+                (_, distance) = self.nearest_obstacle(test_pt)
+                if distance > 0.0:
+                    result *= rm.likelihoodFieldModel(distance, self._z_max, \
+                                                      self._sigma_like, \
+                                                      self._alpha_hit_like, \
+                                                      self._alpha_rand_like)
         return result
 
 

src/cs4313_utilities/shared_robot_classes.py

@@ -35,6 +35,7 @@ class RobotState(object):
         @param initVelocity: initial xDot, yDot, thetaDot (default [0, 0, 0] )
         '''
         self.pose = [ initPose[0], initPose[1], initPose[2] ]
+        self.odom_prev = [ initPose[0], initPose[1], initPose[2] ]
         self.odom = [ initPose[0], initPose[1], initPose[2] ]
         self.velocity = [ initVelocity[0], initVelocity[1], initVelocity[2] ]