Multilateration improvments

drivers/mil_passive_sonar/scripts/multilateration

@@ -13,7 +13,7 @@ from tf2_geometry_msgs import Vector3Stamped, PointStamped
 from std_msgs.msg import Header
 from geometry_msgs.msg import PointStamped, Vector3
 from mil_passive_sonar.srv import FindPinger, FindPingerResponse
-from std_srvs.srv import SetBool, SetBoolResponse
+from std_srvs.srv import SetBool, SetBoolResponse, Trigger, TriggerResponse
 
 
 class MultilaterationNode(object):
@@ -24,30 +24,40 @@ class MultilaterationNode(object):
     def __init__(self):
         self.tfBuffer = tf2_ros.Buffer()
         self.listener = tf2_ros.TransformListener(self.tfBuffer)
-        buffer_len = rospy.get_param('~buffer_size', default=10)
+        buffer_len = rospy.get_param('~buffer_size', default=15)
+        self.minimum_delta_dist = rospy.get_param('~min_delta', default=0.2)
+        self.filter_dist = rospy.get_param('~filter_dist', default=5)
+        self.filter_count = rospy.get_param('~filter_count', default=4)
+        self.buffer_min = rospy.get_param('~buffer_min', default=10)
         self.vec_samples = deque(maxlen=buffer_len)
         self.enabled = False
-        self.global_fame = 'enu'
+        self.last_pos = None
+        self.global_frame = rospy.get_param('~global_frame', default='enu')
         self.heading_sub = rospy.Subscriber('/hydrophones/direction',
                 Vector3Stamped, self.heading_cb, queue_size=10)
         self.position_pub = rospy.Publisher('/hydrophones/position',
                 PointStamped, queue_size=1)
         self.enable_srv = rospy.Service('~enable', SetBool,
                 self.enable_cb)
+        self.reset_srv = rospy.Service('~reset', Trigger,
+                                       self.reset_cb)
 
     def enable_cb(self, req):
-        if req.data is not self.enabled:
-            self.vec_samples.clear()
         self.enabled = req.data
         return SetBoolResponse(success=True)
 
+    def reset_cb(self, req):
+        self.vec_samples.clear()
+        return {'success': True}
+
     def heading_cb(self, p_message):
         if not self.enabled:
             return
         try:
+            # Transform ping into gloabl frame
             transformed_vec = self.tfBuffer.transform(p_message,
-                    self.global_fame, rospy.Duration(2))
-            transformed_origin = self.tfBuffer.lookup_transform(self.global_fame,
+                    self.global_frame, rospy.Duration(2))
+            transformed_origin = self.tfBuffer.lookup_transform(self.global_frame,
                     p_message.header.frame_id, p_message.header.stamp,
                     rospy.Duration(2))
             vec = rosmsg_to_numpy(transformed_vec.vector)
@@ -57,25 +67,85 @@ class MultilaterationNode(object):
         except tf2_ros.TransformException, e:
             rospy.logwarn('TF Exception: {}'.format(e))
             return
+
+        # Check if two samples were taken too close to each other
+        if self.last_pos is not None and np.linalg.norm(self.last_pos - origin) < self.minimum_delta_dist:
+            rospy.loginfo("Observation too close to previous, ignoring")
+            return
+        self.last_pos = origin
+
+        # Add ping to samples list
         self.vec_samples.append((origin, origin + vec))
-        if len(self.vec_samples) == self.vec_samples.maxlen:
+
+        # If there are enough samples, calculate position
+        if len(self.vec_samples) >= self.buffer_min:
             self.publish_position()
 
     def publish_position(self):
-        line_array = np.array([(p[0][0], p[0][1], p[1][0], p[1][1]) for p in self.vec_samples])
+        # Setup lines from samples
+        line_array = np.array([(np.array(p[0][0:2]), np.array(p[1][0:2])) for p in self.vec_samples])
+
+        # Calculate least squares intersection
         where = self.ls_line_intersection2d(line_array)
+
+        # If no point could be determined, don't publish
+        if where is None:
+            rospy.logwarn_throttle(10.0, 'Could not determine point; likely because too many lobs were filtered out due to intersections close to their origins')
+            return
+
+        # Publish point
         ps = PointStamped()
+        ps.header.frame_id = self.global_frame
         ps.header.stamp = rospy.Time.now()
         ps.point = numpy_to_point(where)
         self.position_pub.publish(ps)
 
-    def ls_line_intersection2d(self, line_array):
+    def ls_line_intersection2d(self, lines_array):
         """
         Find the intersection of lines in the least-squares sense.
-        start - Nx3 numpy array of start points
-        end - Nx3 numpy array of end points
-        https://en.wikipedia.org/wiki/Line-line_intersection#In_two_dimensions_2
+        Filter lobs such that no lob can have more than a number of intersections with other lines within a radius of the boat
         """
+
+        # Filter out lobs that intersect with many other lobs close to their origin
+
+        line_array = np.array([[0, 0, 0, 0]])
+        line_intersecting_array = np.array([])
+
+        for line1 in lines_array:
+            line_intersections = 0
+            for line2 in lines_array:
+                if np.array_equal(line1, line2):
+                    continue
+
+                boat_pos1 = line1[0]
+                boat_pos2 = line2[0]
+
+                # Calculate the intersection of the two lines
+                # https://stackoverflow.com/a/47823499
+                t, s = np.linalg.solve(np.array([line1[1]-line1[0], line2[0]-line2[1]]).T, line2[0]-line1[0])
+                intersection = (1-t)*line1[0] + t*line1[1]
+
+                # Calculate the distance to the boat's position from the intersection
+                distance1 = np.linalg.norm(intersection - boat_pos1)
+                distance2 = np.linalg.norm(intersection - boat_pos2)
+
+                # If the distances are within the filter radius, increment the intersections count
+                if distance1 < self.filter_dist or distance2 < self.filter_dist:
+                    line_intersections += 1
+
+            if line_intersections < self.filter_count:
+                line_array = np.vstack([line_array, np.array([line1[0][0], line1[0][1], line1[1][0], line1[1][1]])])
+
+
+        line_array = line_array[1:]
+
+        # If we don't have at least 3 lines left, return no results
+        if len(line_array) < 3:
+            return None
+
+
+        # https://en.wikipedia.org/wiki/Line-line_intersection#In_two_dimensions_2
+
         def line_segment_norm(line_end_pts):
             assert len(line_end_pts) == 4
             return np.linalg.norm(line_end_pts[2:] - line_end_pts[:2])
@@ -103,4 +173,3 @@ if __name__ == '__main__':
     rospy.init_node('multilateration')
     MultilaterationNode()
     rospy.spin()
-