✨

robot/robot.py

@@ -85,7 +85,7 @@ class Robot(object):
 
     def _init_camera(self):
         def gstreamer_pipeline(
-            capture_width=1024, capture_height=720, framerate=30):
+            capture_width=1280, capture_height=720, framerate=30):
             """
             Utility function for setting parameters for the gstreamer camera
             pipeline

selflocalization/camera.py

@@ -309,10 +309,7 @@ class Camera(object):
 
 
         if not isinstance(self.ids, type(None)):
-            dists = np.zeros(len(self.tvects), dtype=self.tvecs.dtype)
-            for i, tvec in enumerate(self.tvecs):
-                dists[i] = np.sqrt(tvec[0][0]**2 + tvec[0][2]**2)
-
+            dists = np.linalg.norm(self.tvecs, axis=len(self.tvecs.shape) - 1) * 100
             # Make sure we always return properly shaped arrays
             dists = dists.reshape((dists.shape[0],))
             ids = self.ids.reshape((self.ids.shape[0],))