merging

lib-visualodo/src/duckietown_visualodo/algo/data_plotter.py

@@ -22,13 +22,6 @@ def __init__(self, train_image, query_image, parameters):
         self.query_image_manager = query_image
         self.image_bridge = CvBridge()
 
-        # if parameters.plot_ransac:
-        #     self.ransac_publisher = rospy.Publisher("ransac/image/compressed", CompressedImage, queue_size=1)
-        # if parameters.plot_histogram_filtering:
-        #     self.histogram_publisher = rospy.Publisher("histograms/image/compressed", CompressedImage, queue_size=1)
-        # if parameters.plot_masking:
-        #     self.mask_publisher = rospy.Publisher("masking/image/compressed", CompressedImage, queue_size=1)
-
     def plot_histogram_filtering(self, good_matches, best_matches, histogram_filter):
         """
         Plots the result of the match histogram filtering
@@ -83,8 +76,7 @@ def plot_histogram_filtering(self, good_matches, best_matches, histogram_filter)
             np.append(initial_histogram_img, final_histogram_img, axis=0), new_height=matches_img.shape[0])
         final_img = np.append(matches_img, final_img, axis=1)
 
-        # self.histogram_publisher.publish(self.image_bridge.cv2_to_compressed_imgmsg(final_img))
-        return self.image_bridge.cv2cv2_to_compressed_imgmsg(final_img)
+        return self.image_bridge.cv2_to_compressed_imgmsg(final_img)
 
     def plot_point_correspondences(self, train_pts, query_pts, proximity_mask):
         """
@@ -117,7 +109,6 @@ def plot_point_correspondences(self, train_pts, query_pts, proximity_mask):
         # plt.imshow(img3)
         # plt.show()
 
-        # self.mask_publisher.publish(self.image_bridge.cv2_to_compressed_imgmsg(img3))
         return img3
 
     def plot_displacements_from_distance_mask(self, match_distance_filter):
@@ -148,7 +139,6 @@ def plot_displacements_from_distance_mask(self, match_distance_filter):
         # plt.imshow(img)
         # plt.show()
 
-        # self.mask_publisher.publish(self.image_bridge.cv2_to_compressed_imgmsg(img))
         return img
 
     def plot_query_bounding_box(self, bounding_box):
@@ -204,7 +194,6 @@ def plot_ransac_homography(self, matches, h_matrix, matches_mask):
 
             img3 = cv2.drawMatches(img1, kp1, img2, kp2, matches, None, **draw_params)
 
-            # self.ransac_publisher.publish(self.image_bridge.cv2_to_compressed_imgmsg(img3))
             return img3
 
     @staticmethod

lib-visualodo/src/duckietown_visualodo/algo/visual_odometry.py

@@ -1,5 +1,4 @@
 #!/usr/bin/env python
-
 # -*- coding: utf-8 -*-
 
 from __future__ import division
@@ -50,10 +49,8 @@ def __init__(self):
         self.mask_params = [0.5, 0.7, 0.4]
         self.stingray_mask = []
 
-
-
-        self.histogram_img = None
-        self.mask_img = None
+        self.mask_img=None
+        self.histogram_img=None
 
     def set_parameter(self, param_name, param_val, string_param_val):
         """
@@ -241,7 +238,7 @@ def visual_odometry_core(self):
 
             # Publish the results of histogram filtering
             if parameters.plot_histogram_filtering:
-                self.histogram_img = processed_data_plotter.plot_histogram_filtering(unfiltered_matches, matches, histogram_filter)
+                self.histogram_img = processed_data_plotter.plot_histogram_filtering(unfiltered_matches, matches, histogram_filter)
 
         n_final_matches = len(matches)
 

lib-visualodo/src/duckietown_visualodo_tests/test-utils.py

@@ -1,23 +1,20 @@
 # coding=utf-8
 import numpy as np
 from comptests import comptest, run_module_tests, comptest_fails
-from duckietown_visualodo.algo.utils import second_largest
-import duckietown_visualodo
+from duckietown_visualodo.algo.utils import second_largest, is_rotation_matrix
 
 
 
 
 @comptest
-def check_util():
+def check_second_largest():
     a = [1,2,3,4]
     assert(second_largest(a)==3)
 
-
 @comptest
 def check_is_rotation_matrix():
     M = np.array([[1,0,0],[0,1,0],[0,0,1]])
     assert is_rotation_matrix(M)
 
-
 if __name__ == '__main__':
     run_module_tests()

ros-visualodo/src/visual_odometry_node.py

@@ -134,75 +134,78 @@ def cb_image(self, cv_image):
 
         # Run configured visual odometry with input image
         vo_result = self.visual_odometer.get_image_and_trigger_vo(cv_image)
-        vo_transform = vo_result[0]
-        histogram_img = vo_result[1]
-        mask_img = vo_result[2]
-
-        if vo_transform is not None:
-            try:
-                t_vec = vo_transform.translation
-                z_quaternion = vo_transform.rotation
-                current_time = rospy.Time.now()
-
-                t = TransformStamped()
-                t.header.frame_id = "world"
-                t.child_frame_id = "axis"
-
-                # Rotate displacement vector by duckiebot rotation wrt world frame and add it to stacked displacement
-                t_vec = np.squeeze(qv_multiply(self.stacked_rotation, [t_vec.x, t_vec.y, t_vec.z]))
-                translation = Vector3(t_vec[0], t_vec[1], t_vec[2])
-                self.stacked_position = Vector3(
-                    self.stacked_position.x + translation.x,
-                    self.stacked_position.y + translation.y,
-                    self.stacked_position.z + translation.z)
-
-                # Add quaternion rotation to stacked rotation to obtain the rotation transformation between world and
-                # duckiebot frames
-                quaternion = tf.transformations.quaternion_multiply(
-                    self.stacked_rotation, [z_quaternion.x, z_quaternion.y, z_quaternion.z, z_quaternion.w])
-
-                # Store quaternion and transform it to geometry message
-                self.stacked_rotation = quaternion
-                quaternion = Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3])
-
-                # Broadcast transform
-                t.transform.translation = self.stacked_position
-                t.transform.rotation = quaternion
-                self.transform_broadcaster.sendTransform(t)
-
-                # Create odometry and Path msgs
-                odom = Odometry()
-                odom.header.stamp = current_time
-                odom.header.frame_id = "world"
-
-                self.path.header = odom.header
-
-                # Set the position
-                odom.pose.pose = Pose(self.stacked_position, quaternion)
-                pose_stamped = PoseStamped()
-                pose_stamped.header = odom.header
-                pose_stamped.pose = odom.pose.pose
-                self.path.poses.append(pose_stamped)
-
-                odom.child_frame_id = "base_link"
-
-                # Publish the messages
-                self.odom_publisher.publish(odom)
-                self.path_publisher.publish(self.path)
-
-                rospy.logwarn("TIME: Total time: %s", time.time() - start)
-                rospy.logwarn("===================================================")
-
-            except AssertionError as e:
-                rospy.logwarn("Error in estimated rotation matrix")
-                rospy.logerr(e)
-                raise
-        if histogram_img is not None:
-            self.histogram_publisher.publish(histogram_img)
-
-        if mask_img is not None:
-            self.mask_publisher.publish(histogram_img)
 
+        if vo_result is not None:
+
+            if vo_result[0] is not None:
+                vo_transform = vo_result[0]
+
+            if vo_transform is not None:
+                try:
+                    t_vec = vo_transform.translation
+                    z_quaternion = vo_transform.rotation
+                    current_time = rospy.Time.now()
+
+                    t = TransformStamped()
+                    t.header.frame_id = "world"
+                    t.child_frame_id = "axis"
+
+                    # Rotate displacement vector by duckiebot rotation wrt world frame and add it to stacked displacement
+                    t_vec = np.squeeze(qv_multiply(self.stacked_rotation, [t_vec.x, t_vec.y, t_vec.z]))
+                    translation = Vector3(t_vec[0], t_vec[1], t_vec[2])
+                    self.stacked_position = Vector3(
+                        self.stacked_position.x + translation.x,
+                        self.stacked_position.y + translation.y,
+                        self.stacked_position.z + translation.z)
+
+                    # Add quaternion rotation to stacked rotation to obtain the rotation transformation between world and
+                    # duckiebot frames
+                    quaternion = tf.transformations.quaternion_multiply(
+                        self.stacked_rotation, [z_quaternion.x, z_quaternion.y, z_quaternion.z, z_quaternion.w])
+
+                    # Store quaternion and transform it to geometry message
+                    self.stacked_rotation = quaternion
+                    quaternion = Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3])
+
+                    # Broadcast transform
+                    t.transform.translation = self.stacked_position
+                    t.transform.rotation = quaternion
+                    self.transform_broadcaster.sendTransform(t)
+
+                    # Create odometry and Path msgs
+                    odom = Odometry()
+                    odom.header.stamp = current_time
+                    odom.header.frame_id = "world"
+
+                    self.path.header = odom.header
+
+                    # Set the position
+                    odom.pose.pose = Pose(self.stacked_position, quaternion)
+                    pose_stamped = PoseStamped()
+                    pose_stamped.header = odom.header
+                    pose_stamped.pose = odom.pose.pose
+                    self.path.poses.append(pose_stamped)
+
+                    odom.child_frame_id = "base_link"
+
+                    # Publish the messages
+                    self.odom_publisher.publish(odom)
+                    self.path_publisher.publish(self.path)
+
+                    rospy.logwarn("TIME: Total time: %s", time.time() - start)
+                    rospy.logwarn("===================================================")
+
+                except AssertionError as e:
+                    rospy.logwarn("Error in estimated rotation matrix")
+                    rospy.logerr(e)
+                    raise
+            if vo_result[1] is not None:
+                histogram_img = vo_result[1]
+                self.histogram_publisher.publish(histogram_img)
+
+            if vo_result[2] is not None:
+                mask_img = vo_result[2]
+                self.mask_publisher.publish(histogram_img)
 
         self.thread_working = False