math_tools.py

"""
Calculation collections
1. points_distance
    Get distance between two tuple points
2. Scaler
    Normalized the data into a new range
3. my_arctan_degrees
    Get the angle of the slope of (x, y)
4. get_circle
    Calculate a circle from 3 points
5. configure_kalman_filter
    Configure a kalman filter
6. get_min_gray_point
    Get the min points from the slope line within a distance to start_pos
7. calc_touch_angle
    Calculate the degree angle from base to target
"""

import numpy as np
import cv2

def points_distance(point1, point2):
    """Return the distance between point 1 and point2
    
    Arguments:
        point1 {[type]} -- [description]
        point2 {[type]} -- [description]
    
    Returns:
        [type] -- [description]
    """
    if type(point1) != tuple or type(point2) != tuple:
        return None
    
    if point1[0] is None or point1[1] is None or point2[0] is None or point2[1] is None:
        return None

    d = np.sqrt(np.sum(np.square(np.array(point1) - np.array(point2))))
    return d


def scaler(value, old_range, new_range):
    """Project value from [min_old, max_old] to [min_new, max_new]

    Arguments:
        value {float} -- [description]
        min_base_target {list} -- [min_old, min_new]
        max_base_target {list} -- [max_old, max_new]

    Returns:
        value -- [projected value]
    """
    if value is None or all(old_range) is False or all(new_range) is False:
        return None

    min_old, max_old = old_range
    min_new, max_new = new_range
    return (value - min_old) / (max_old - min_old) * (max_new -
                                                      min_new) + min_new

def my_arctan_degrees(x, y):
    """Return the required angle of the slope
    
    Arguments:
        x {[type]} -- [description]
        y {[type]} -- [description]
    
    Returns:
        [type] -- [description]
    """
    if x is None or y is None:
        return None

    if abs(x) < 1e-9:
        if y > 0:
            return 90
        else:
            return -90
    else:
        degree = np.degrees(np.arctan(y / x))
        if x < 0 and y < 0:
            return 180 - degree
        elif x < 0 and y > 0:
            return 180 + degree
        else:
            return degree

def get_circle(p1, p2, p3):
    """Get the center of the circle generated by 3 points
    
    Arguments:
        p1 {[type]} -- [description]
        p2 {[type]} -- [description]
        p3 {[type]} -- [description]
    
    Returns:
        [type] -- [description]
    """
    if p1 is None or p2 is None or p3 is None:
        return None

    a = 2 * (p2[0] - p1[0])
    b = 2 * (p2[1] - p1[1])
    c = p2[0] * p2[0] + p2[1] * p2[1] - p1[0] * p1[0] - p1[1] * p1[1]
    d = 2 * (p3[0] - p2[0])
    e = 2 * (p3[1] - p2[1])
    f = p3[0] * p3[0] + p3[1] * p3[1] - p2[0] * p2[0] - p2[1] * p2[1]
    if b * d - e * a == 0:
        return None

    x = (b * f - e * c) / (b * d - e * a)
    y = (d * c - a * f) / (b * d - e * a)

    center = ((int)(x), (int)(y))

    return center

class KalmanFilter:
    def __init__(self):
        self.kalman = configure_kalman_filter()

    def predict(self, pt):
        """Use pt to update the kalman filter and return the predicted value

        Arguments:
            pt {[type]} -- [description]

        Returns:
            [type] -- [description]
        """
        self.kalman.correct(
            np.array([[np.float32(pt[0])],
                      [np.float32(pt[1])]]))
        predict_pt = self.kalman.predict()
        filterd_pt = (int(predict_pt[0]), int(predict_pt[1]))
        return filterd_pt


def configure_kalman_filter():
    """Configure the kalman filter

    Returns:
        kalman_filter [cv2.KalmanFilter]
    """
    # State number: 4, including (x，y，dx，dy) (position and velocity)
    # Measurement number: 2, (x, y) (position)
    kalman = cv2.KalmanFilter(4, 2)
    kalman.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]],
                                        np.float32)
    kalman.transitionMatrix = np.array(
        [[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32)
    kalman.processNoiseCov = np.array(
        [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]],
        np.float32) * 0.03

    return kalman

def get_min_gray_point(gray_img, start_pos, distance=0, slope=None):
    """Get the min gray in the slope direction and within the distance

    Arguments:
        gray_img {[type]} -- [description]
        start_pos {[type]} -- [description]

    Keyword Arguments:
        distance {int} -- [description] (default: {0})
        vertical {bool} -- [description] (default: {True})
        slope {int} -- [description] (default: {0})

    Returns:
        point_max_gradient {tuple} -- [description]
    """
    # x is from left to right, related to width
    # y is from up to down, related to height
    x, y = start_pos
    min_gray = float('inf')
    min_x, min_y = 0, 0
    height, width = gray_img.shape[0], gray_img.shape[1]

    def __IsValid(x, y):
        """Check whether x and y is in the boundary of img
        """
        return 0 <= x < width and 0 <= y < height

    if slope is None:
        # Let column(x) to be fixed and change the row(y)
        for dy in range(int(-distance / 2), int(distance / 2)):
            if __IsValid(x, y + dy) and gray_img[y + dy, x] < min_gray:
                min_gray = gray_img[y + dy, x]
                min_x, min_y = x, y + dy
    else:
        c_x, c_y = x, y
        # up
        while __IsValid(c_x, c_y) and points_distance((c_x, c_y),
                                                      (x, y)) < distance / 2:
            if gray_img[c_y, c_x] < min_gray:
                min_gray = gray_img[c_y, c_x]
                min_x, min_y = c_x, c_y
            c_x = c_x + 1
            c_y = int(c_y + 1 * slope)

        c_x, c_y = x, y
        # down
        while __IsValid(c_x, c_y) and points_distance((c_x, c_y),
                                                      (x, y)) < distance / 2:
            if gray_img[c_y, c_x] < min_gray:
                min_gray = gray_img[c_y, c_x]
                min_x, min_y = c_x, c_y
            c_x = c_x - 1
            c_y = int(c_y - 1 * slope)

    # Check the ans
    if min_gray == float('inf'):
        return start_pos
    else:
        return (min_x, min_y)

def calc_touch_angle(base, target):
    """Calculate the degree angle from base to target
    
    Arguments:
        base {[type]} -- [description]
        target {[type]} -- [description]
    
    Returns:
        [type] -- [description]
    """
    if base is None or target is None:
        return None

    x1, y1 = base
    x2, y2 = target
    if x1 is None or y1 is None or x2 is None or y2 is None:
        return None

    if abs(x1 - x2) < 1e-9:
        if y2 > y1:
            return 90
        else:
            return -90
    else:
        slope = (y1 - y2) / (x1 - x2)
        angle = np.degrees(np.arctan(slope))
        if y2 > y1 and angle < 0:
            angle = 180 + angle

        return angle