Add stereo image

README.md

@@ -76,3 +76,21 @@ pip install -e .[pc]
 ```bash
 irg createcar --path ~/ROAR
 ```
+
+## Commands to re-install the package
+* Under IRG directory, reinstall the package
+```bash
+pip install -e .[nano]
+```
+* Remove mark1 folder
+```bash
+rm -r mark1/
+```
+* Create car (executable)
+```bash
+irg createcar --path ~/mark1
+```
+* Run drive script
+```bash
+python manage.py drive
+```

irmark1/config.py

@@ -73,10 +73,15 @@ def load_config(config_path=None):
 
 
     #derivative settings
-    if hasattr(cfg, 'IMAGE_H') and hasattr(cfg, 'IMAGE_W'): 
-        cfg.TARGET_H = cfg.IMAGE_H - cfg.ROI_CROP_TOP - cfg.ROI_CROP_BOTTOM
-        cfg.TARGET_W = cfg.IMAGE_W
-        cfg.TARGET_D = cfg.IMAGE_DEPTH
+    if not hasattr(cfg, 'DNN_IMAGE_H'):
+        cfg.DNN_IMAGE_H = cfg.IMAGE_H
+    if not hasattr(cfg, 'DNN_IMAGE_W'):
+        cfg.DNN_IMAGE_W = cfg.IMAGE_W
+    if not hasattr(cfg, 'DNN_IMAGE_DEPTH'):
+        cfg.DNN_IMAGE_DEPTH = cfg.IMAGE_DEPTH
+    cfg.TARGET_H = cfg.DNN_IMAGE_H - cfg.ROI_CROP_TOP - cfg.ROI_CROP_BOTTOM
+    cfg.TARGET_W = cfg.DNN_IMAGE_W
+    cfg.TARGET_D = cfg.DNN_IMAGE_DEPTH
 
     print()
 

irmark1/parts/controller.py

@@ -890,8 +890,9 @@ def chaos_monkey_off(self):
         self.chaos_monkey_steering = None
 
 
-    def run_threaded(self, img_arr=None):
-        self.img_arr = img_arr
+    def run_threaded(self, img_arr_a=None, img_arr_b=None):
+        self.img_arr_a = img_arr_a
+        self.img_arr_b = img_arr_b
 
         '''
         process E-Stop state machine
@@ -920,7 +921,7 @@ def run_threaded(self, img_arr=None):
         return self.angle, self.throttle, self.mode, self.recording
 
 
-    def run(self, img_arr=None):
+    def run(self, img_arr_a=None, image_arr_b=None):
         raise Exception("We expect for this part to be run with the threaded=True argument.")
         return None, None, None, None
 

irmark1/parts/datastore.py

@@ -235,6 +235,12 @@ def put_record(self, data):
                 name = self.make_file_name(key, ext='.jpg')
                 img.save(os.path.join(self.path, name))
                 json_data[key]=name
+            elif typ == 'lossless_image_array':
+                img = Image.fromarray(np.uint8(val))
+                name = self.make_file_name(key, ext='.png')
+                # https://pillow.readthedocs.io/en/3.1.x/handbook/image-file-formats.html
+                img.save(os.path.join(self.path, name), compress_level=0)
+                json_data[key]=name
 
             else:
                 msg = 'Tub does not know what to do with this type {}'.format(typ)

irmark1/parts/image.py

@@ -3,6 +3,7 @@
 from PIL import Image
 import numpy as np
 from irmark1.utils import img_to_binary, binary_to_img, arr_to_img, img_to_arr
+import irmark1
 
 class ImgArrToJpg():
 
@@ -36,8 +37,8 @@ def run(self, image_a, image_b):
         '''
         if image_a is not None and image_b is not None:
             width, height, _ = image_a.shape
-            grey_a = dk.utils.rgb2gray(image_a)
-            grey_b = dk.utils.rgb2gray(image_b)
+            grey_a = irmark1.utils.rgb2gray(image_a)
+            grey_b = irmark1.utils.rgb2gray(image_b)
             grey_c = grey_a - grey_b
 
             stereo_image = np.zeros([width, height, 3], dtype=np.dtype('B'))

irmark1/parts/realsense2.py

@@ -84,9 +84,8 @@ def shutdown(self):
 
 class RS_D435i(object):
     '''
-    The Intel Realsense T265 camera is a device which uses an imu, twin fisheye cameras,
-    and an Movidius chip to do sensor fusion and emit a world space coordinate frame that 
-    is remarkably consistent.
+    Intel RealSense depth camera D435i combines the robust depth sensing capabilities of the D435 with the addition of an inertial measurement unit (IMU).
+    ref: https://www.intelrealsense.com/depth-camera-d435i/
     '''
 
     def __init__(self, image_w=640, image_h=480, image_d=3, image_output=True, framerate=30):
@@ -102,16 +101,17 @@ def __init__(self, image_w=640, image_h=480, image_d=3, image_output=True, frame
 
         if self.image_output:
             cfg.enable_stream(rs.stream.color, image_w, image_h, rs.format.rgb8, framerate) # color camera
-            # cfg.enable_stream(rs.stream.depth, image_w, image_h, rs.format.z16, framerate) # depth camera
+            cfg.enable_stream(rs.stream.depth, image_w, image_h, rs.format.z16, framerate) # depth camera
 
         # Start streaming with requested config
         self.pipe.start(cfg)
         self.running = True
 
         zero_vec = (0.0, 0.0, 0.0)
-        self.gyro = zero_vec
-        self.acc = zero_vec
+        self.gyr = zero_vec
+        self.acl = zero_vec
         self.img = None
+        self.dimg = None
 
     def poll(self):
         try:
@@ -122,23 +122,25 @@ def poll(self):
 
         if self.image_output:
             color_frame = frames.get_color_frame()
+            depth_frame = frames.get_depth_frame()
             self.img = np.asanyarray(color_frame.get_data())
+            self.dimg = np.asanyarray(depth_frame.get_data())
 
         # Fetch IMU frame
         accel = frames.first_or_default(rs.stream.accel)
         gyro = frames.first_or_default(rs.stream.gyro)
         if accel and gyro:
-            self.acc = accel.as_motion_frame().get_motion_data()
-            self.gyro = gyro.as_motion_frame().get_motion_data()
-            # print('realsense accel(%f, %f, %f)' % (self.acc.x, self.acc.y, self.acc.z))
-            # print('realsense gyro(%f, %f, %f)' % (self.gyro.x, self.gyro.y, self.gyro.z))
+            self.acl = accel.as_motion_frame().get_motion_data()
+            self.gyr = gyro.as_motion_frame().get_motion_data()
+            # print('realsense accel(%f, %f, %f)' % (self.acl.x, self.acl.y, self.acl.z))
+            # print('realsense gyro(%f, %f, %f)' % (self.gyr.x, self.gyr.y, self.gyr.z))
 
     def update(self):
         while self.running:
             self.poll()
 
     def run_threaded(self):
-        return self.img
+        return self.img, self.dimg, self.acl.x, self.acl.y, self.acl.z, self.gyr.x, self.gyr.y, self.gyr.z
 
     def run(self):
         self.poll()

irmark1/parts/web_controller/templates/static/style.css

@@ -51,6 +51,9 @@
 #mpeg-image {
 	width:100%;
 }
+#rear-mpeg-image {
+	width:100%;
+}
 
 #joystick_container {
 	text-align: center;
@@ -117,4 +120,4 @@ body {
   /* Set the fixed height of the footer here */
   height: 60px;
   background-color: #f5f5f5;
-}
+}

irmark1/parts/web_controller/templates/vehicle.html

@@ -103,7 +103,10 @@
 
       <div class="col-xs-8 col-sm-5 col-md-5"><!-- center column -->
         <div class="thumbnail">
-          <img id='mpeg-image', class='img-responsive' src="/video"/> </img>
+          <img id='mpeg-image', class='img-responsive' src="/video_a"/> </img>
+        </div>
+        <div class="thumbnail">
+          <img id='rear-mpeg-image', class='img-responsive' src="/video_b"/> </img>
         </div>
       </div><!-- end center column -->
 
@@ -168,4 +171,4 @@ <h4 class="modal-title" id="myModalLabel">About Control Modes</h4>
   });
 </script>
 
-{% end %}
+{% end %}

irmark1/parts/web_controller/web.py

@@ -123,7 +123,7 @@ def __init__(self):
         handlers = [
             (r"/", tornado.web.RedirectHandler, dict(url="/drive")),
             (r"/drive", DriveAPI),
-            (r"/video",VideoAPI),
+            (r"/video_(.)",VideoAPI),
             (r"/static/(.*)", tornado.web.StaticFileHandler, {"path": self.static_file_path}),
             ]
 
@@ -139,12 +139,22 @@ def update(self, port=8887):
         self.listen(self.port)
         tornado.ioloop.IOLoop.instance().start()
 
-    def run_threaded(self, img_arr=None):
-        self.img_arr = img_arr
+    def run_threaded(self, disp_img_arr_a=None, disp_img_arr_b=None):
+        if disp_img_arr_a is not None:
+            self.disp_img_arr_a = disp_img_arr_a
+        if disp_img_arr_b is not None:
+            self.disp_img_arr_b = disp_img_arr_b
+        else:
+            self.disp_img_arr_b = self.disp_img_arr_a
         return self.angle, self.throttle, self.mode, self.recording
 
-    def run(self, img_arr=None):
-        self.img_arr = img_arr
+    def run(self, disp_img_arr_a=None, disp_img_arr_b=None):
+        if disp_img_arr_a is not None:
+            self.disp_img_arr_a = disp_img_arr_a
+        if disp_img_arr_b is not None:
+            self.disp_img_arr_b = disp_img_arr_b
+        else:
+            self.disp_img_arr_b = self.disp_img_arr_a
         return self.angle, self.throttle, self.mode, self.recording
 
     def shutdown(self):
@@ -174,7 +184,7 @@ class VideoAPI(tornado.web.RequestHandler):
     '''
     Serves a MJPEG of the images posted from the vehicle. 
     '''
-    async def get(self):
+    async def get(self, src):
 
         self.set_header("Content-type", "multipart/x-mixed-replace;boundary=--boundarydonotcross")
 
@@ -185,8 +195,12 @@ async def get(self):
             interval = .1
             if self.served_image_timestamp + interval < time.time():
 
-
-                img = utils.arr_to_binary(self.application.img_arr)
+                if src == 'a':
+                    img = utils.arr_to_binary(self.application.disp_img_arr_a)
+                elif src == 'b':
+                    img = utils.arr_to_binary(self.application.disp_img_arr_b)
+                else:
+                    raise
 
                 self.write(my_boundary)
                 self.write("Content-type: image/jpeg\r\n")
@@ -198,4 +212,4 @@ async def get(self):
                 except tornado.iostream.StreamClosedError:
                     pass
             else:
-                await tornado.gen.sleep(interval)
+                await tornado.gen.sleep(interval)

irmark1/templates/complete.py

@@ -78,8 +78,7 @@ def drive(cfg, model_path=None, use_joystick=False, model_type=None, camera_type
         V.add(camB, outputs=['cam/image_array_b'], threaded=True)
 
         from irmark1.parts.image import StereoPair
-
-        V.add(StereoPair(), inputs=['cam/image_array_a', 'cam/image_array_b'], 
+        V.add(StereoPair(), inputs=['cam/image_array_a', 'cam/image_array_b'],
             outputs=['cam/image_array'])
 
     else:
@@ -117,9 +116,13 @@ def drive(cfg, model_path=None, use_joystick=False, model_type=None, camera_type
             cam = RS_D435i(image_w=cfg.IMAGE_W, image_h=cfg.IMAGE_H, image_d=cfg.IMAGE_DEPTH, framerate=cfg.CAMERA_FRAMERATE)
         else:
             raise(Exception("Unkown camera type: %s" % cfg.CAMERA_TYPE))
-
-        V.add(cam, inputs=inputs, outputs=['cam/image_array'], threaded=threaded)
-
+
+        if cfg.CAMERA_TYPE == "D435i":
+            V.add(cam, inputs=inputs, outputs=['cam/image_array_a', 'cam/image_array_b', 'imu/acl_x', 'imu/acl_y', 'imu/acl_z',
+                'imu/gyr_x', 'imu/gyr_y', 'imu/gyr_z'], threaded=threaded)
+        else:
+            V.add(cam, inputs=inputs, outputs=['cam/image_array'], threaded=threaded)
+
     if use_joystick or cfg.USE_JOYSTICK_AS_DEFAULT:
         #modify max_throttle closer to 1.0 to have more power
         #modify steering_scale lower than 1.0 to have less responsive steering
@@ -140,7 +143,7 @@ def drive(cfg, model_path=None, use_joystick=False, model_type=None, camera_type
 
 
     V.add(ctr, 
-          inputs=['cam/image_array'],
+          inputs=['cam/image_array_a', 'cam/image_array_b'],
           outputs=['user/angle', 'user/throttle', 'user/mode', 'recording'],
           threaded=True)
 
@@ -254,7 +257,7 @@ def show_record_acount_status():
         ctr.set_button_down_trigger('circle', show_record_acount_status)
 
     #IMU
-    if cfg.HAVE_IMU:
+    if cfg.HAVE_IMU and cfg.CAMERA_TYPE != "D435i":
         from irmark1.parts.imu import Mpu6050
         imu = Mpu6050()
         V.add(imu, outputs=['imu/acl_x', 'imu/acl_y', 'imu/acl_z',
@@ -499,16 +502,24 @@ def run(self, mode, recording):
         V.add(motor, inputs=["throttle"])
 
 
-    #add tub to save data
-
-    inputs=['cam/image_array',
+    #add tub to save data    
+    inputs=['cam/image_array', 
             'user/angle', 'user/throttle', 
             'user/mode']
+
 
     types=['image_array',
            'float', 'float',
            'str']
 
+    if cfg.CAMERA_TYPE == "D435i":
+        # remove 'cam/image_array'
+        inputs.pop(0)
+        types.pop(0)
+
+        inputs += ['cam/image_array_a', 'cam/image_array_b']
+        types += ['image_array', 'lossless_image_array']
+
     if cfg.TRAIN_BEHAVIORS:
         inputs += ['behavior/state', 'behavior/label', "behavior/one_hot_state_array"]
         types += ['int', 'str', 'vector']

irmark1/templates/myconfig.py

@@ -10,13 +10,20 @@
 
 # CAMERA
 CAMERA_TYPE = "D435i"
-IMAGE_W = 320
-IMAGE_H = 240
+HAVE_IMU = True
+IMAGE_W = 1280
+IMAGE_H = 720
+
 IMAGE_DEPTH = 3         # default RGB=3, make 1 for mono
 CAMERA_FRAMERATE = 30
 # CSIC camera
 PCA9685_I2C_BUSNUM = 1   #None will auto detect, which is fine on the pi. But other platforms should specify the bus num.
 
+# For training
+DNN_IMAGE_W = 160
+DNN_IMAGE_H = 120
+DNN_IMAGE_DEPTH = 3
+
 #STEERING parameters for Traxxas 4-Tec chassis
 STEERING_CHANNEL = 1            #channel on the 9685 pwm board 0-15
 STEERING_LEFT_PWM = 260         #pwm value for full left steering

irmark1/utils.py

@@ -113,6 +113,9 @@ def rgb2gray(rgb):
     '''
     take a numpy rgb image return a new single channel image converted to greyscale
     '''
+    # image not rgb
+    if len(rgb.shape)<3 or rgb.shape[-1]<3:
+        return rgb
     return np.dot(rgb[...,:3], [0.299, 0.587, 0.114])
 
 
@@ -143,8 +146,8 @@ def load_scaled_image_arr(filename, cfg):
     import irmark1 as m1
     try:
         img = Image.open(filename)
-        if img.height != cfg.IMAGE_H or img.width != cfg.IMAGE_W:
-            img = img.resize((cfg.IMAGE_W, cfg.IMAGE_H))
+        if img.height != cfg.DNN_IMAGE_H or img.width != cfg.DNN_IMAGE_W:
+            img = img.resize((cfg.DNN_IMAGE_W, cfg.DNN_IMAGE_H))
         img_arr = np.array(img)
         img_arr = normalize_and_crop(img_arr, cfg)
         croppedImgH = img_arr.shape[0]
@@ -428,7 +431,7 @@ def get_model_by_type(model_type, cfg):
         model_type = cfg.DEFAULT_MODEL_TYPE
     print("\"get_model_by_type\" model Type is: {}".format(model_type))
 
-    input_shape = (cfg.IMAGE_H, cfg.IMAGE_W, cfg.IMAGE_DEPTH)
+    input_shape = (cfg.DNN_IMAGE_H, cfg.DNN_IMAGE_W, cfg.DNN_IMAGE_DEPTH)
     roi_crop = (cfg.ROI_CROP_TOP, cfg.ROI_CROP_BOTTOM)
 
     if model_type == "tflite_linear":
@@ -529,4 +532,4 @@ def on_frame(self):
             e = time.time()
             print('fps', 100.0 / (e - self.t))
             self.t = time.time()
-            self.iter = 0
+            self.iter = 0