rmse.py
Go to the documentation of this file.
1 # -*- coding: utf-8 -*-
2 import cv2 as cv
3 import numpy as np
4 import sys
5 import math
6 from matplotlib import pyplot as plt
7 import os
8 
9 filename = "‪D:/dataset/gt-4622.png"
10 if (len(sys.argv) > 1):
11  filename = str(sys.argv[1])
12 
13 fname, file_extension = os.path.splitext(filename)
14 
15 height = 0
16 width = 0
17 
18 i = []
19 if file_extension.lower() == ".raw":
20  f = open(filename,"r")
21  i = np.fromfile(f, dtype='uint16', sep="")
22  f.close()
23 
24  size = i.shape[0]
25  if (size / 1280 == 720):
26  height = 720
27  width = 1280
28  elif (size / 848 == 480):
29  height = 480
30  width = 848
31  elif (size / 640 == 480):
32  height = 480
33  width = 640
34  else:
35  print("Unknown dimentions")
36 if file_extension.lower() == ".png":
37  i = cv.imread(filename, -1).astype(np.uint16)
38  width = i.shape[1]
39  height = i.shape[0]
40 
41 m = np.percentile(i, 5)
42 M = np.percentile(i, 95)
43 
44 i = i.reshape([height, width])
45 
46 orig = i.copy()
47 
48 x0 = 0
49 y0 = 0
50 x1 = 0
51 y1 = 0
52 xx = 0
53 yy = 0
54 mx = 0
55 Mx = 0
56 my = 0
57 My = 0
58 
59 def click_and_crop(event, x, y, flags, param):
60  global x0
61  global y0
62  global x1
63  global y1
64  global xx
65  global yy
66  global orig
67  global mx, my, Mx, My
68 
69  xx = x
70  yy = y
71 
72  if event == cv.EVENT_LBUTTONDOWN:
73  return
74  # check to see if the left mouse button was released
75  elif event == cv.EVENT_LBUTTONUP:
76  if (x0 == 0):
77  x0 = x
78  y0 = y
79  else:
80  if (x1 == 0):
81  x1 = x
82  y1 = y
83 
84  mx = min(x0, x1)
85  Mx = max(x0, x1)
86  my = min(y0, y1)
87  My = max(y0, y1)
88  else:
89  x0 = x
90  y0 = y
91  x1 = 0
92  y1 = 0
93  mx = 0
94  Mx = 0
95  my = 0
96  My = 0
97  # record the ending (x, y) coordinates and indicate that
98  return
99 
100 cv.namedWindow("image")
101 cv.setMouseCallback("image", click_and_crop)
102 
103 i = np.divide(i, np.array([M - m], dtype=np.float)).astype(np.float)
104 i = (i - m).astype(np.float)
105 
106 i8 = (i * 255.0).astype(np.uint8)
107 
108 if i8.ndim == 3:
109  i8 = cv.cvtColor(i8, cv.COLOR_BGRA2GRAY)
110 
111 i8 = cv.equalizeHist(i8)
112 
113 colorized = cv.applyColorMap(i8, cv.COLORMAP_JET)
114 
115 colorized[i8 == int(m)] = 0
116 
117 font = cv.FONT_HERSHEY_COMPLEX_SMALL
118 colorized = cv.putText(colorized, str(m) + " .. " + str(M), (20,50), font, 1, (255, 255, 255), 2, cv.LINE_AA)
119 
120 while cv.getWindowProperty('image', 0) >= 0:
121  im = colorized.copy()
122 
123  if (x0 > 0 and x1 == 0):
124  im = cv.rectangle(im, (x0, y0), (xx, yy), (255, 255, 255), 2)
125  if (x0 > 0 and x1 > 0):
126  im = cv.rectangle(im, (x0, y0), (x1, y1), (255, 255, 255), 2)
127 
128  if (mx < Mx):
129  crop = orig[int(my):int(My), int(mx):int(Mx)].astype(np.float)
130 
131  X = []
132  Y = []
133  Z = []
134 
135  Xcrop = np.zeros_like(crop).astype(np.float)
136  Ycrop = np.zeros_like(crop).astype(np.float)
137  Zcrop = np.zeros_like(crop).astype(np.float)
138 
139  for i in range(my, My):
140  for j in range(mx, Mx):
141  z = crop[i - my, j - mx] * 0.001
142  x = (float(j) / width - 0.5) * z
143  y = (float(i) / height - 0.5) * z
144  if (z > 0):
145  X.append(x)
146  Y.append(y)
147  Z.append(z)
148  Xcrop[i - my, j - mx] = x
149  Ycrop[i - my, j - mx] = y
150  Zcrop[i - my, j - mx] = z
151 
152  xyz = np.dstack((X, Y, Z))
153  xyz = xyz.reshape(xyz.shape[0] * xyz.shape[1], xyz.shape[2])
154  C_x = np.cov(xyz.T)
155  eig_vals, eig_vecs = np.linalg.eig(C_x)
156 
157  variance = np.min(eig_vals)
158  min_eig_val_index = np.argmin(eig_vals)
159  direction_vector = eig_vecs[:, min_eig_val_index].copy()
160 
161  rmse = math.sqrt(variance)
162  #print(math.sqrt(variance) * 100)
163 
164  normal = direction_vector / np.linalg.norm(direction_vector)
165 
166  point = np.mean(xyz, axis=0)
167 
168  #print(normal)
169  #print(point)
170 
171  d = -np.dot(point, normal)
172  #print(d)
173 
174  a = normal[0]
175  b = normal[1]
176  c = normal[2]
177  e = math.sqrt(a * a + b * b + c * c)
178 
179  Dcrop = np.zeros_like(crop).astype(np.float)
180 
181  for i in range(my, My):
182  for j in range(mx, Mx):
183  x = Xcrop[i - my, j - mx]
184  y = Ycrop[i - my, j - mx]
185  z = Zcrop[i - my, j - mx]
186  #print(x)
187  dist = abs(a * x + b * y + c * z + d) / e
188  if (z > 0):
189  #print(Dcrop[i - my, j - mx])
190  #print(dist)
191  Dcrop[i - my, j - mx] = dist
192 
193  Dcrop = np.divide(Dcrop, (3 * rmse) / 255).astype(np.float)
194  Dcrop = np.clip(Dcrop, 0, 255).astype(np.uint8)
195  Dmap = np.dstack((Dcrop, Dcrop, Dcrop))
196  im[int(my):int(My), int(mx):int(Mx)] = Dmap
197 
198  rmse_mm = rmse * 1000
199  im = cv.putText(im, "%.2f mm" % rmse_mm, (mx + 2,my + 16), font, 1, (255, 255, 255), 2, cv.LINE_AA)
200 
201  # display the image and wait for a keypress
202  cv.imshow("image", im)
203  key = cv.waitKey(100) & 0xFF
204 
205 cv.destroyAllWindows()
static std::string print(const transformation &tf)
def click_and_crop(event, x, y, flags, param)
Definition: rmse.py:59
int min(int a, int b)
Definition: lz4s.c:73
void copy(void *dst, void const *src, size_t size)
Definition: types.cpp:836


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:47:40