✨

selflocalization/selflocalize.py

@@ -258,6 +258,8 @@ try:
         print("est_weight:", est_weight)
         print("certainty:", certainty)
         if est_weight > 1:
+            draw_world(est_pose, particles, world)
+            cv2.imwrite("test.png", world)
             break
         else:
             arlo.go_diff(POWER, POWER, 0, 1)