hight-level-robotics-congnitive-agent/librealsense/wrappers/tensorflow/tools/rmse.py

# -*- coding: utf-8 -*-
import cv2 as cv
import numpy as np
import sys
import math
from matplotlib import pyplot as plt
import os

filename = "‪D:/dataset/gt-4622.png"
if (len(sys.argv) > 1):
    filename = str(sys.argv[1])

fname, file_extension = os.path.splitext(filename)

height = 0
width = 0

i = []
if file_extension.lower() == ".raw":
    f = open(filename,"r")
    i = np.fromfile(f, dtype='uint16', sep="")
    f.close()

    size = i.shape[0]
    if (size / 1280 == 720):
        height = 720
        width = 1280
    elif (size / 848 == 480):
        height = 480
        width = 848
    elif (size / 640 == 480):
        height = 480
        width = 640
    else:
        print("Unknown dimentions")
if file_extension.lower() == ".png":
    i = cv.imread(filename, -1).astype(np.uint16)
    width = i.shape[1]
    height = i.shape[0]

m = np.percentile(i, 5)
M = np.percentile(i, 95)

i = i.reshape([height, width])

orig = i.copy()

x0 = 0
y0 = 0
x1 = 0
y1 = 0
xx = 0
yy = 0
mx = 0
Mx = 0
my = 0
My = 0

def click_and_crop(event, x, y, flags, param):
    global x0
    global y0
    global x1
    global y1
    global xx
    global yy
    global orig
    global mx, my, Mx, My

    xx = x
    yy = y
        
    if event == cv.EVENT_LBUTTONDOWN:
        return
	# check to see if the left mouse button was released
    elif event == cv.EVENT_LBUTTONUP:
        if (x0 == 0):
            x0 = x
            y0 = y
        else:
            if (x1 == 0):
                x1 = x
                y1 = y

                mx = min(x0, x1)
                Mx = max(x0, x1)
                my = min(y0, y1)
                My = max(y0, y1)
            else:
                x0 = x
                y0 = y
                x1 = 0
                y1 = 0
                mx = 0
                Mx = 0
                my = 0
                My = 0
	# record the ending (x, y) coordinates and indicate that
        return

cv.namedWindow("image")
cv.setMouseCallback("image", click_and_crop)

i = np.divide(i, np.array([M - m], dtype=np.float)).astype(np.float)
i = (i - m).astype(np.float)

i8 = (i * 255.0).astype(np.uint8)

if i8.ndim == 3:
    i8 = cv.cvtColor(i8, cv.COLOR_BGRA2GRAY)

i8 = cv.equalizeHist(i8)

colorized = cv.applyColorMap(i8, cv.COLORMAP_JET)

colorized[i8 == int(m)] = 0

font = cv.FONT_HERSHEY_COMPLEX_SMALL
colorized = cv.putText(colorized, str(m) + " .. " + str(M), (20,50), font, 1, (255, 255, 255), 2, cv.LINE_AA)

while cv.getWindowProperty('image', 0) >= 0:
    im = colorized.copy()

    if (x0 > 0 and x1 == 0):
        im = cv.rectangle(im, (x0, y0), (xx, yy), (255, 255, 255), 2)
    if (x0 > 0 and x1 > 0):
        im = cv.rectangle(im, (x0, y0), (x1, y1), (255, 255, 255), 2)

    if (mx < Mx):
        crop = orig[int(my):int(My), int(mx):int(Mx)].astype(np.float)

        X = []
        Y = []
        Z = []

        Xcrop = np.zeros_like(crop).astype(np.float)
        Ycrop = np.zeros_like(crop).astype(np.float)
        Zcrop = np.zeros_like(crop).astype(np.float)

        for i in range(my, My):
            for j in range(mx, Mx):
                z = crop[i - my, j - mx] * 0.001
                x = (float(j) / width - 0.5) * z
                y = (float(i) / height - 0.5) * z
                if (z > 0):
                    X.append(x)
                    Y.append(y)
                    Z.append(z)
                Xcrop[i - my, j - mx] = x
                Ycrop[i - my, j - mx] = y
                Zcrop[i - my, j - mx] = z

        xyz = np.dstack((X, Y, Z))
        xyz = xyz.reshape(xyz.shape[0] * xyz.shape[1], xyz.shape[2])
        C_x = np.cov(xyz.T)
        eig_vals, eig_vecs = np.linalg.eig(C_x)

        variance = np.min(eig_vals)
        min_eig_val_index = np.argmin(eig_vals)
        direction_vector = eig_vecs[:, min_eig_val_index].copy()

        rmse = math.sqrt(variance)
        #print(math.sqrt(variance) * 100)

        normal = direction_vector / np.linalg.norm(direction_vector)

        point = np.mean(xyz, axis=0)

        #print(normal)
        #print(point)

        d = -np.dot(point, normal)
        #print(d)

        a = normal[0]
        b = normal[1]
        c = normal[2]
        e = math.sqrt(a * a + b * b + c * c)

        Dcrop = np.zeros_like(crop).astype(np.float)

        for i in range(my, My):
            for j in range(mx, Mx):
                x = Xcrop[i - my, j - mx]
                y = Ycrop[i - my, j - mx]
                z = Zcrop[i - my, j - mx]
                #print(x)
                dist = abs(a * x + b * y + c * z + d) / e
                if (z > 0):
                    #print(Dcrop[i - my, j - mx])
                    #print(dist)
                    Dcrop[i - my, j - mx] = dist

        Dcrop = np.divide(Dcrop, (3 * rmse) / 255).astype(np.float)
        Dcrop = np.clip(Dcrop, 0, 255).astype(np.uint8)
        Dmap = np.dstack((Dcrop, Dcrop, Dcrop))
        im[int(my):int(My), int(mx):int(Mx)] = Dmap

        rmse_mm = rmse * 1000
        im = cv.putText(im, "%.2f mm" % rmse_mm, (mx + 2,my + 16), font, 1, (255, 255, 255), 2, cv.LINE_AA)
    
    # display the image and wait for a keypress
    cv.imshow("image", im)
    key = cv.waitKey(100) & 0xFF

cv.destroyAllWindows()