✨

selflocalization/camera.py

@@ -100,7 +100,7 @@ class CaptureThread(threading.Thread):
 
 
 class Camera(object):
-    """This class is responsible for doing the image processing. It detects known landmarks and 
+    """This class is responsible for doing the image processing. It detects known landmarks and
     measures distances and orientations to these."""
     
     def __init__(self, camidx, robottype='arlo', useCaptureThread=False):
@@ -309,7 +309,10 @@ class Camera(object):
 
 
         if not isinstance(self.ids, type(None)):
-            dists = np.linalg.norm(self.tvecs, axis=len(self.tvecs.shape) - 1) * 100
+            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)
+
             # Make sure we always return properly shaped arrays
             dists = dists.reshape((dists.shape[0],))
             ids = self.ids.reshape((self.ids.shape[0],))