From cd69f4ea0faa05eb57d5c5fe56ded6249166235f Mon Sep 17 00:00:00 2001
From: toinsson <antoine.loriette@gmail.com>
Date: Tue, 30 Aug 2016 18:49:42 +0100
Subject: [PATCH] fix init camera parameters

---
 src/examples.py | 38 +++++++++++++++++++++++++++++++++++---
 src/realsense.c | 18 ++++++------------
 2 files changed, 41 insertions(+), 15 deletions(-)

diff --git a/src/examples.py b/src/examples.py
index c8e6f89..69ae516 100644
--- a/src/examples.py
+++ b/src/examples.py
@@ -4,7 +4,39 @@
 pyrs.start()
 time.sleep(2)
 
-cm = pyrs.get_colour()
-plt.imshow(cm)
-plt.show()
+# cm = pyrs.get_colour()
+# plt.imshow(cm)
+# plt.show()
 
+import cv2
+import numpy as np
+import time
+
+
+
+cnt = 0
+last = time.time()
+smoothing = 0.9;
+fps_smooth = 30
+
+while True:
+
+    cnt += 1
+    if (cnt % 10) == 0:
+        now = time.time()
+        dt = now - last
+        fps = 10/dt
+        fps_smooth = (fps_smooth * smoothing) + (fps * (1.0-smoothing))
+        last = now
+
+    c = pyrs.get_colour()
+    d = pyrs.get_depth() >> 3
+    d = cv2.applyColorMap(d.astype(np.uint8), cv2.COLORMAP_RAINBOW)
+
+    cd = np.concatenate((c,d), axis=1)
+
+    cd = cv2.putText(cd, str(fps_smooth)[:4], (0,50), cv2.FONT_HERSHEY_SIMPLEX, 2, (0,0,0))
+
+    cv2.imshow('', cd)
+    if cv2.waitKey(1) & 0xFF == ord('q'):
+        break
\ No newline at end of file
diff --git a/src/realsense.c b/src/realsense.c
index 1aa2984..8a7d7da 100644
--- a/src/realsense.c
+++ b/src/realsense.c
@@ -37,14 +37,11 @@ void check_error()
 rs_context * ctx = NULL;
 rs_device * dev = NULL;
 
+// global camera parameters
 rs_intrinsics depth_intrin, color_intrin;
 rs_extrinsics depth_to_color;
-
-
 float scale;
 
-// static Py
-
 static PyObject *create_context(PyObject *self, PyObject *args, PyObject *keywds)
 {
     // parse arguments
@@ -608,9 +605,11 @@ static PyMethodDef RealSenseMethods[] = {
     {NULL, NULL, 0, NULL}        /* Sentinel */
 };
 
-void init_default_rs_extrinsics_value(void);
-void init_default_rs_extrinsics_value(void)
+void init_default_camera_parameters(void);
+void init_default_camera_parameters(void)
 {
+    scale = 0.00012498664727900177;
+
     // save static values for calling projection functions when camera not present
     depth_to_color.rotation[0] =  0.999998;
     depth_to_color.rotation[1] = -0.001382;
@@ -625,11 +624,7 @@ void init_default_rs_extrinsics_value(void)
     depth_to_color.translation[0] =  0.025700;
     depth_to_color.translation[1] = -0.000733;
     depth_to_color.translation[2] =  0.003885;
-}
 
-void init_default_rs_intrinsics_value(void);
-void init_default_rs_intrinsics_value(void)
-{
     // save static values for calling projection functions when camera not present
     color_intrin.width      = 640;
     color_intrin.height     = 480;
@@ -664,8 +659,7 @@ PyMODINIT_FUNC initpyrealsense(void)
     import_array();
 
     // init default values
-    init_default_rs_intrinsics_value();
-    init_default_rs_extrinsics_value();
+    init_default_camera_parameters();
 }