<filename>robot2cam_calibration/compute_transformations.py
"""A file to calculate the pose transformation between a camera and robot and
a tool offset from correspondences.
"""

# The MIT License (MIT)
#
# Copyright (c) 2016 GTRC.
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
from __future__ import division
import argparse
import json
from scipy import optimize
import datetime
import os
import math
import numpy as np
import cv2


def main():
    """
    Exposes :py:func:`compute_transformation` to the commandline. Run with arg
    `-h` for more info.
    """
    # Parse in arguments
    parser = argparse.ArgumentParser(
        description="Compute transformation between camera and robot given "
                    "existing correspondences")

    parser.add_argument("--correspondences", type=str,
                        help='The filename for the file containing the list of'
                             'correspondences, which is generated by'
                             'get_correspondences.py. '
                             'Defaults to: correspondences.json',
                        default="correspondences.json")

    parser.add_argument("--out", type=str,
                        help="File to save output to",
                        default="transformation.json")

    parser.add_argument("--cam2rob", type=float, nargs=6,
                        help="Initial guess for the camera to robot "
                             "transformation, x,y,z,rotation vector",
                        metavar=('x','y','z','a','b','c'),
                        default=np.array([0, 0, 1000, 0, 0, 0]).tolist())

    parser.add_argument("--tcp2target", type=float, nargs=6,
                        help="Initial guess for the tcp to target "
                             "(robot tool), x,y,z,rotation vector",
                        metavar=('x', 'y', 'z', 'a', 'b', 'c'),
                        default=np.array([0, 0, 0, 0, 0, 0]).tolist())

    parser.add_argument("--max_cam2rob", type=float,
                        help="Maximum deviation of the cam2robot "
                             "transformation from the guess",
                        default=2000)

    parser.add_argument("--max_tcp2target", type=float,
                        help="Maximum deviation of the cam2target "
                             "transformation from the guess",
                        default=500)

    parser.add_argument("--iter", type=int, help="number of iterations to "
                                                 "perform of the basin hopping"
                                                 "routine.", default=250)

    parser.add_argument("--minimizer", type=str, help="The minimizer to use at "
                                                      "each basin hopping stop"
                                                      "Valid options are: SLSQP"
                                                      "TNC, and L-BFGS-B",
                        default="SLSQP")

    args = parser.parse_args()

    result = compute_transformation(
        correspondences=args.correspondences,
        file_out=args.out,
        cam2rob_guess=args.cam2rob,
        tcp2target_guess=args.tcp2target,
        max_cam2rob_deviation=args.max_cam2rob,
        max_tcp2target_deviation=args.max_tcp2target,
        iterations=args.iter,
        minimizer=args.minimizer
    )

    print('Final Result:\n{}'.format(result))


def compute_transformation(correspondences, file_out, cam2rob_guess,
                           tcp2target_guess, max_cam2rob_deviation,
                           max_tcp2target_deviation, iterations, minimizer):
    """Computes the camera to robot base and tcp to target (flange to tcp in
    some cases) transformations. Uses matched coorespondences of
    transformations from the camera to a fixed point past the final robot axis
    (for example a grid or other marker) and the robot base to tcp
    transformation.

    Args:
        correspondences (string): The filename of the correspondences file.
                                  This file should be a json file with fields:
                                  'time', 'tcp2robot', 'camera2grid'.
                                  'tcp2robot' and 'camera2grid' should be lists
                                  of lists, with each individual list being a
                                  Rodrigues vector
                                  (x,y,z,3 element rotation vector/axis-angle).
                                  Linear distance must be consistent (mm are
                                  recommended). Angular distances must be in
                                  radians.
        file_out (string): The name of the file to be output (no extension)
        cam2rob_guess (6 element list): The Rodrigues vector for the initial
                                        guess of the camera to robot
                                        transformation
        tcp2target_guess (6 element list): The Rodrigues vector for the initial
                                           guess of the tcp to target
                                           transformation
        max_cam2rob_deviation (float): The x,y,z range around the initial
                                       camera to robot guess which should be
                                       searched.
        max_tcp2target_deviation (float): The x,y,z range around the initial
                                          camera to target guess which should
                                          be searched.
        iterations (int): The number of iterations of basin hopping to perform.
        minimizer (str): The minimizer to use at each basin hopping stop
                         Valid options are: SLSQP TNC, and L-BFGS-B

    Returns: The results as a dictionary
    """
    with open(correspondences, 'r') as correspondences_file:
        correspondences_dictionary = json.load(correspondences_file)
        write_time = correspondences_dictionary['time']
        # nx6 arrays x,y,z,axis-angle:
        tcp2robot = correspondences_dictionary['tcp2robot']
        camera2grid = correspondences_dictionary['camera2grid']
        print("Loaded data from {}".format(write_time))

    #optimize
    guess = np.concatenate((cam2rob_guess, tcp2target_guess))
    bounds = Bounds([guess[0] + max_cam2rob_deviation,
                     guess[1] + max_cam2rob_deviation,
                     guess[2] + max_cam2rob_deviation,
                     np.pi,
                     np.pi,
                     np.pi,
                     guess[6] + max_tcp2target_deviation,
                     guess[7] + max_tcp2target_deviation,
                     guess[8] + max_tcp2target_deviation,
                     np.pi,
                     np.pi,
                     np.pi],
                    [guess[0] - max_cam2rob_deviation,
                     guess[1] - max_cam2rob_deviation,
                     guess[2] - max_cam2rob_deviation,
                     -np.pi,
                     -np.pi,
                     -np.pi,
                     guess[6] - max_tcp2target_deviation,
                     guess[7] - max_tcp2target_deviation,
                     guess[8] - max_tcp2target_deviation,
                     -np.pi,
                     -np.pi,
                     -np.pi])
    bounds_tuple = [(low, high) for low, high in zip(bounds.xmin, bounds.xmax)]
    # define the new step taking routine and pass it to basinhopping
    take_step = RandomDisplacementBounds(bounds.xmin, bounds.xmax)
    minimizer_kwargs = {"args": (tcp2robot, camera2grid), "method": minimizer,
                        "bounds": bounds_tuple, "options":{"maxiter": 25000}}
    print('starting basinhopping')
    result = optimize.basinhopping(
        func=error, x0=guess, minimizer_kwargs=minimizer_kwargs,
        accept_test=bounds, disp=False, callback=callback, take_step=take_step,
        niter=iterations, interval=25,
        niter_success=math.ceil(iterations/7.5))

    json_dict = {"time": str(datetime.datetime.now()),
                 "cam2robot": {"xyz-angle": result.x[:6].tolist(),
                               "Tmatrix": vector2mat(result.x[:6]).tolist()},
                 "tcp2target": {"xyz-angle": result.x[6:].tolist(),
                                "Tmatrix": vector2mat(result.x[6:]).tolist()},
                 "minimization": {"terminated for":result.message,
                                  "Number of minimization failures":result.minimization_failures,
                                  "Number of iterations":result.nit,
                                  "Number of executions of error function":result.nfev,
                                  "method": minimizer,
                                  "best result":{"success":str(result.lowest_optimization_result.success),
                                                 "message": result.lowest_optimization_result.message,
                                                 "error": result.lowest_optimization_result.fun}
                                  }
                 }

    with open(os.path.splitext(file_out)[0] + '.json', 'w') as \
            result_json_file:
        json.dump(json_dict, result_json_file, indent=4)

    return json_dict


class Bounds(object):
    def __init__(self, xmax, xmin):
        self.xmax = np.array(xmax)
        self.xmin = np.array(xmin)

    def __call__(self, **kwargs):
        x = kwargs["x_new"]
        tmax = bool(np.all(x <= self.xmax))
        tmin = bool(np.all(x >= self.xmin))
        return tmax and tmin


def error(guess, tcp2robot, camera2grid, ratio=0.25):
    """
    Calculates the difference between a guess at robot 2 cam transformations
    compared to gathered data. Uses manhattan error for the distance (as
    opposed to true line distance). Finds the angular difference between two
    points. Takes the weighted sum of the manhattan distance and angular
    distance based on the ratio.

    Args:
        guess (1x12 array): Input guess array. Values will range between the
                            bounds passed in the optimize function. 6 dof
                            camera 2 robot (x,y,z,axis-angle), 6 dof tcp 2
                            target (x,y,z,axis-angle)
        tcp2robot (nx6 array): Array of gathered data for the pose of the robot
                               tool center point wrt. the robot coordinate base
        camera2grid (nx6 array): Array of gathered data for the transformation
                                 from the camera to the target
        ratio (float): The ratio of weight given to the manhattan error vs the
                       angular error. A higer value will give more weight to
                       the manhattan error and less to the the angular error.
                       Must be in the range [0,1]

    Returns: A float, the total error between the guess and the collected
             data
    """
    errors = np.zeros(len(tcp2robot))
    total_error = 0
    if ratio < 0:
        raise ValueError("ratio must be greater than or equal to zero")
    if ratio > 1:
        raise ValueError("ratio must be less than or equal to one")
    for i in range(len(tcp2robot)):
        guess_cam2rob = vector2mat(guess[:6])
        guess_tcp2target = vector2mat(guess[6:])
        guess_cam2tcp = np.matmul(guess_cam2rob,
                                  vector2mat(np.concatenate(
                                      (np.array(tcp2robot[i][:3]),
                                       np.array(tcp2robot[i][3:])))))
        guess_cam2target = np.matmul(guess_cam2tcp, guess_tcp2target)

        euclidean_distance = np.sqrt(np.sum(np.square(
            np.array(guess_cam2target[:3, 3]) - np.array(camera2grid[i][:3])
        )))
        angular_error = math.acos(
            (np.trace(np.matmul(vector2mat(np.array(camera2grid[i]))[:3, :3].T,
                                guess_cam2target[:3, :3]))-1)/2)

        errors[i] = euclidean_distance*ratio + angular_error*(1-ratio)

    return np.mean(errors[
                       np.where(mad_based_outlier(np.array(errors)) == False)])


def mad_based_outlier(points, thresh=3.5):
    """http://stackoverflow.com/questions/22354094/pythonic-way-of-detecting-
    outliers-in-one-dimensional-observation-data/22357811#22357811"""
    if len(points.shape) == 1:
        points = points[:,None]
    median = np.median(points, axis=0)
    diff = np.sum((points - median)**2, axis=-1)
    diff = np.sqrt(diff)
    med_abs_deviation = np.median(diff)

    modified_z_score = 0.6745 * diff / med_abs_deviation

    return modified_z_score > thresh


def vector2mat(vector):
    """
    Converts a vector in form x,y,z,axis-angle to a homogenous transformation
    matrix

    Args:
        vector (6 element list): a vector representation form of a
                                 transformation matrix. x,y,z,axis-angle

    Returns: A 4x4 np.ndarry of the homogenous transformation matrix
    """
    transformation_matrix = np.zeros((4, 4))
    transformation_matrix[3, 3] = 1
    try:
        transformation_matrix[0:3, 3] = vector[:3, 0]
    except:
        transformation_matrix[0:3, 3] = vector[:3]
    rotation_matrix, _ = cv2.Rodrigues(np.array(vector[3:]))
    transformation_matrix[:3, :3] = rotation_matrix
    return transformation_matrix


def mat2vector(mat):
    """
    Converts a transformatiion matrix into a 6 dof vector. x,y,z,axis-angle
    Args:
        mat (4x4 ndarray): the transformation matrix

    Returns: A 6 element list, x,y,z,axis-angle
    """
    vector = [0]*6
    vector[:3] = np.asarray(mat[:3, 3])
    axis_angle, _ = cv2.Rodrigues(np.array(mat[:3, :3]))
    vector[3:] = axis_angle
    return vector


def callback(x, f, accept):
    """Prints out the local minimum result found in each iteration of the
    basinhopping routine."""
    print('minimized to: {}\nWith an error of: {}. This is {}ACCEPTED\n'.format(x, f, '' if accept else 'NOT '))


class RandomDisplacementBounds(object):
    """random displacement with bounds. For use with the baisnhopping routine.
    Based on: http://stackoverflow.com/questions/21670080"""
    def __init__(self, xmin, xmax, stepsize=0.5):
        """Initializes a displacement generator

        Args:
            xmin (list of floats): The minimum values for all of the paramaters
            xmin (list of floats): The maximum values for all of the paramaters
            stepsize: The initial stepsize for the algorithim. This will be
                      overwritten by the basinhopping routine.
        """
        self.xmin = xmin
        self.xmax = xmax
        self.stepsize = stepsize

    def __call__(self, x):
        """Take a random step, from the prior, proportional to the stepsize wrt
        the bounds. Ensure the new position is within the bounds

        Args:
            x (np.array of floats): The prior position

        Returns:
            The new starting position for optimization
        """
        print('generating points with step size {}, yielding a range of: {} to {}'.format(self.stepsize, x + np.multiply(
                [-self.stepsize]*x.size,
                (self.xmax-self.xmin)), x + np.multiply(
                [self.stepsize]*x.size,
                (self.xmax-self.xmin))))
        while True:
            xnew = x + np.multiply(
                np.random.uniform(-self.stepsize, self.stepsize, np.shape(x)),
                (self.xmax-self.xmin))
            if np.all(xnew < self.xmax) and np.all(xnew > self.xmin):
                break
        print('finished generating new guess: {}'.format(xnew))
        return xnew

if __name__ == "__main__":
    main()

<filename>robot2cam_calibration/compute_transformations.py
"""A file to calculate the pose transformation between a camera and robot and
a tool offset from correspondences.
"""

# The MIT License (MIT)
#
# Copyright (c) 2016 GTRC.
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
from __future__ import division
import argparse
import json
from scipy import optimize
import datetime
import os
import math
import numpy as np
import cv2


def main():
    """
    Exposes :py:func:`compute_transformation` to the commandline. Run with arg
    `-h` for more info.
    """
    # Parse in arguments
    parser = argparse.ArgumentParser(
        description="Compute transformation between camera and robot given "
                    "existing correspondences")

    parser.add_argument("--correspondences", type=str,
                        help='The filename for the file containing the list of'
                             'correspondences, which is generated by'
                             'get_correspondences.py. '
                             'Defaults to: correspondences.json',
                        default="correspondences.json")

    parser.add_argument("--out", type=str,
                        help="File to save output to",
                        default="transformation.json")

    parser.add_argument("--cam2rob", type=float, nargs=6,
                        help="Initial guess for the camera to robot "
                             "transformation, x,y,z,rotation vector",
                        metavar=('x','y','z','a','b','c'),
                        default=np.array([0, 0, 1000, 0, 0, 0]).tolist())

    parser.add_argument("--tcp2target", type=float, nargs=6,
                        help="Initial guess for the tcp to target "
                             "(robot tool), x,y,z,rotation vector",
                        metavar=('x', 'y', 'z', 'a', 'b', 'c'),
                        default=np.array([0, 0, 0, 0, 0, 0]).tolist())

    parser.add_argument("--max_cam2rob", type=float,
                        help="Maximum deviation of the cam2robot "
                             "transformation from the guess",
                        default=2000)

    parser.add_argument("--max_tcp2target", type=float,
                        help="Maximum deviation of the cam2target "
                             "transformation from the guess",
                        default=500)

    parser.add_argument("--iter", type=int, help="number of iterations to "
                                                 "perform of the basin hopping"
                                                 "routine.", default=250)

    parser.add_argument("--minimizer", type=str, help="The minimizer to use at "
                                                      "each basin hopping stop"
                                                      "Valid options are: SLSQP"
                                                      "TNC, and L-BFGS-B",
                        default="SLSQP")

    args = parser.parse_args()

    result = compute_transformation(
        correspondences=args.correspondences,
        file_out=args.out,
        cam2rob_guess=args.cam2rob,
        tcp2target_guess=args.tcp2target,
        max_cam2rob_deviation=args.max_cam2rob,
        max_tcp2target_deviation=args.max_tcp2target,
        iterations=args.iter,
        minimizer=args.minimizer
    )

    print('Final Result:\n{}'.format(result))


def compute_transformation(correspondences, file_out, cam2rob_guess,
                           tcp2target_guess, max_cam2rob_deviation,
                           max_tcp2target_deviation, iterations, minimizer):
    """Computes the camera to robot base and tcp to target (flange to tcp in
    some cases) transformations. Uses matched coorespondences of
    transformations from the camera to a fixed point past the final robot axis
    (for example a grid or other marker) and the robot base to tcp
    transformation.

    Args:
        correspondences (string): The filename of the correspondences file.
                                  This file should be a json file with fields:
                                  'time', 'tcp2robot', 'camera2grid'.
                                  'tcp2robot' and 'camera2grid' should be lists
                                  of lists, with each individual list being a
                                  Rodrigues vector
                                  (x,y,z,3 element rotation vector/axis-angle).
                                  Linear distance must be consistent (mm are
                                  recommended). Angular distances must be in
                                  radians.
        file_out (string): The name of the file to be output (no extension)
        cam2rob_guess (6 element list): The Rodrigues vector for the initial
                                        guess of the camera to robot
                                        transformation
        tcp2target_guess (6 element list): The Rodrigues vector for the initial
                                           guess of the tcp to target
                                           transformation
        max_cam2rob_deviation (float): The x,y,z range around the initial
                                       camera to robot guess which should be
                                       searched.
        max_tcp2target_deviation (float): The x,y,z range around the initial
                                          camera to target guess which should
                                          be searched.
        iterations (int): The number of iterations of basin hopping to perform.
        minimizer (str): The minimizer to use at each basin hopping stop
                         Valid options are: SLSQP TNC, and L-BFGS-B

    Returns: The results as a dictionary
    """
    with open(correspondences, 'r') as correspondences_file:
        correspondences_dictionary = json.load(correspondences_file)
        write_time = correspondences_dictionary['time']
        # nx6 arrays x,y,z,axis-angle:
        tcp2robot = correspondences_dictionary['tcp2robot']
        camera2grid = correspondences_dictionary['camera2grid']
        print("Loaded data from {}".format(write_time))

    #optimize
    guess = np.concatenate((cam2rob_guess, tcp2target_guess))
    bounds = Bounds([guess[0] + max_cam2rob_deviation,
                     guess[1] + max_cam2rob_deviation,
                     guess[2] + max_cam2rob_deviation,
                     np.pi,
                     np.pi,
                     np.pi,
                     guess[6] + max_tcp2target_deviation,
                     guess[7] + max_tcp2target_deviation,
                     guess[8] + max_tcp2target_deviation,
                     np.pi,
                     np.pi,
                     np.pi],
                    [guess[0] - max_cam2rob_deviation,
                     guess[1] - max_cam2rob_deviation,
                     guess[2] - max_cam2rob_deviation,
                     -np.pi,
                     -np.pi,
                     -np.pi,
                     guess[6] - max_tcp2target_deviation,
                     guess[7] - max_tcp2target_deviation,
                     guess[8] - max_tcp2target_deviation,
                     -np.pi,
                     -np.pi,
                     -np.pi])
    bounds_tuple = [(low, high) for low, high in zip(bounds.xmin, bounds.xmax)]
    # define the new step taking routine and pass it to basinhopping
    take_step = RandomDisplacementBounds(bounds.xmin, bounds.xmax)
    minimizer_kwargs = {"args": (tcp2robot, camera2grid), "method": minimizer,
                        "bounds": bounds_tuple, "options":{"maxiter": 25000}}
    print('starting basinhopping')
    result = optimize.basinhopping(
        func=error, x0=guess, minimizer_kwargs=minimizer_kwargs,
        accept_test=bounds, disp=False, callback=callback, take_step=take_step,
        niter=iterations, interval=25,
        niter_success=math.ceil(iterations/7.5))

    json_dict = {"time": str(datetime.datetime.now()),
                 "cam2robot": {"xyz-angle": result.x[:6].tolist(),
                               "Tmatrix": vector2mat(result.x[:6]).tolist()},
                 "tcp2target": {"xyz-angle": result.x[6:].tolist(),
                                "Tmatrix": vector2mat(result.x[6:]).tolist()},
                 "minimization": {"terminated for":result.message,
                                  "Number of minimization failures":result.minimization_failures,
                                  "Number of iterations":result.nit,
                                  "Number of executions of error function":result.nfev,
                                  "method": minimizer,
                                  "best result":{"success":str(result.lowest_optimization_result.success),
                                                 "message": result.lowest_optimization_result.message,
                                                 "error": result.lowest_optimization_result.fun}
                                  }
                 }

    with open(os.path.splitext(file_out)[0] + '.json', 'w') as \
            result_json_file:
        json.dump(json_dict, result_json_file, indent=4)

    return json_dict


class Bounds(object):
    def __init__(self, xmax, xmin):
        self.xmax = np.array(xmax)
        self.xmin = np.array(xmin)

    def __call__(self, **kwargs):
        x = kwargs["x_new"]
        tmax = bool(np.all(x <= self.xmax))
        tmin = bool(np.all(x >= self.xmin))
        return tmax and tmin


def error(guess, tcp2robot, camera2grid, ratio=0.25):
    """
    Calculates the difference between a guess at robot 2 cam transformations
    compared to gathered data. Uses manhattan error for the distance (as
    opposed to true line distance). Finds the angular difference between two
    points. Takes the weighted sum of the manhattan distance and angular
    distance based on the ratio.

    Args:
        guess (1x12 array): Input guess array. Values will range between the
                            bounds passed in the optimize function. 6 dof
                            camera 2 robot (x,y,z,axis-angle), 6 dof tcp 2
                            target (x,y,z,axis-angle)
        tcp2robot (nx6 array): Array of gathered data for the pose of the robot
                               tool center point wrt. the robot coordinate base
        camera2grid (nx6 array): Array of gathered data for the transformation
                                 from the camera to the target
        ratio (float): The ratio of weight given to the manhattan error vs the
                       angular error. A higer value will give more weight to
                       the manhattan error and less to the the angular error.
                       Must be in the range [0,1]

    Returns: A float, the total error between the guess and the collected
             data
    """
    errors = np.zeros(len(tcp2robot))
    total_error = 0
    if ratio < 0:
        raise ValueError("ratio must be greater than or equal to zero")
    if ratio > 1:
        raise ValueError("ratio must be less than or equal to one")
    for i in range(len(tcp2robot)):
        guess_cam2rob = vector2mat(guess[:6])
        guess_tcp2target = vector2mat(guess[6:])
        guess_cam2tcp = np.matmul(guess_cam2rob,
                                  vector2mat(np.concatenate(
                                      (np.array(tcp2robot[i][:3]),
                                       np.array(tcp2robot[i][3:])))))
        guess_cam2target = np.matmul(guess_cam2tcp, guess_tcp2target)

        euclidean_distance = np.sqrt(np.sum(np.square(
            np.array(guess_cam2target[:3, 3]) - np.array(camera2grid[i][:3])
        )))
        angular_error = math.acos(
            (np.trace(np.matmul(vector2mat(np.array(camera2grid[i]))[:3, :3].T,
                                guess_cam2target[:3, :3]))-1)/2)

        errors[i] = euclidean_distance*ratio + angular_error*(1-ratio)

    return np.mean(errors[
                       np.where(mad_based_outlier(np.array(errors)) == False)])


def mad_based_outlier(points, thresh=3.5):
    """http://stackoverflow.com/questions/22354094/pythonic-way-of-detecting-
    outliers-in-one-dimensional-observation-data/22357811#22357811"""
    if len(points.shape) == 1:
        points = points[:,None]
    median = np.median(points, axis=0)
    diff = np.sum((points - median)**2, axis=-1)
    diff = np.sqrt(diff)
    med_abs_deviation = np.median(diff)

    modified_z_score = 0.6745 * diff / med_abs_deviation

    return modified_z_score > thresh


def vector2mat(vector):
    """
    Converts a vector in form x,y,z,axis-angle to a homogenous transformation
    matrix

    Args:
        vector (6 element list): a vector representation form of a
                                 transformation matrix. x,y,z,axis-angle

    Returns: A 4x4 np.ndarry of the homogenous transformation matrix
    """
    transformation_matrix = np.zeros((4, 4))
    transformation_matrix[3, 3] = 1
    try:
        transformation_matrix[0:3, 3] = vector[:3, 0]
    except:
        transformation_matrix[0:3, 3] = vector[:3]
    rotation_matrix, _ = cv2.Rodrigues(np.array(vector[3:]))
    transformation_matrix[:3, :3] = rotation_matrix
    return transformation_matrix


def mat2vector(mat):
    """
    Converts a transformatiion matrix into a 6 dof vector. x,y,z,axis-angle
    Args:
        mat (4x4 ndarray): the transformation matrix

    Returns: A 6 element list, x,y,z,axis-angle
    """
    vector = [0]*6
    vector[:3] = np.asarray(mat[:3, 3])
    axis_angle, _ = cv2.Rodrigues(np.array(mat[:3, :3]))
    vector[3:] = axis_angle
    return vector


def callback(x, f, accept):
    """Prints out the local minimum result found in each iteration of the
    basinhopping routine."""
    print('minimized to: {}\nWith an error of: {}. This is {}ACCEPTED\n'.format(x, f, '' if accept else 'NOT '))


class RandomDisplacementBounds(object):
    """random displacement with bounds. For use with the baisnhopping routine.
    Based on: http://stackoverflow.com/questions/21670080"""
    def __init__(self, xmin, xmax, stepsize=0.5):
        """Initializes a displacement generator

        Args:
            xmin (list of floats): The minimum values for all of the paramaters
            xmin (list of floats): The maximum values for all of the paramaters
            stepsize: The initial stepsize for the algorithim. This will be
                      overwritten by the basinhopping routine.
        """
        self.xmin = xmin
        self.xmax = xmax
        self.stepsize = stepsize

    def __call__(self, x):
        """Take a random step, from the prior, proportional to the stepsize wrt
        the bounds. Ensure the new position is within the bounds

        Args:
            x (np.array of floats): The prior position

        Returns:
            The new starting position for optimization
        """
        print('generating points with step size {}, yielding a range of: {} to {}'.format(self.stepsize, x + np.multiply(
                [-self.stepsize]*x.size,
                (self.xmax-self.xmin)), x + np.multiply(
                [self.stepsize]*x.size,
                (self.xmax-self.xmin))))
        while True:
            xnew = x + np.multiply(
                np.random.uniform(-self.stepsize, self.stepsize, np.shape(x)),
                (self.xmax-self.xmin))
            if np.all(xnew < self.xmax) and np.all(xnew > self.xmin):
                break
        print('finished generating new guess: {}'.format(xnew))
        return xnew

if __name__ == "__main__":
    main()

A file to calculate the pose transformation between a camera and robot and a tool offset from correspondences. # The MIT License (MIT) # # Copyright (c) 2016 GTRC. # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal # in the Software without restriction, including without limitation the rights # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell # copies of the Software, and to permit persons to whom the Software is # furnished to do so, subject to the following conditions: # # The above copyright notice and this permission notice shall be included in # all copies or substantial portions of the Software. # # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. Exposes :py:func:`compute_transformation` to the commandline. Run with arg     `-h` for more info. # Parse in arguments Computes the camera to robot base and tcp to target (flange to tcp in     some cases) transformations. Uses matched coorespondences of     transformations from the camera to a fixed point past the final robot axis     (for example a grid or other marker) and the robot base to tcp     transformation.      Args:         correspondences (string): The filename of the correspondences file.                                   This file should be a json file with fields:                                   'time', 'tcp2robot', 'camera2grid'.                                   'tcp2robot' and 'camera2grid' should be lists                                   of lists, with each individual list being a                                   Rodrigues vector                                   (x,y,z,3 element rotation vector/axis-angle).                                   Linear distance must be consistent (mm are                                   recommended). Angular distances must be in                                   radians.         file_out (string): The name of the file to be output (no extension)         cam2rob_guess (6 element list): The Rodrigues vector for the initial                                         guess of the camera to robot                                         transformation         tcp2target_guess (6 element list): The Rodrigues vector for the initial                                            guess of the tcp to target                                            transformation         max_cam2rob_deviation (float): The x,y,z range around the initial                                        camera to robot guess which should be                                        searched.         max_tcp2target_deviation (float): The x,y,z range around the initial                                           camera to target guess which should                                           be searched.         iterations (int): The number of iterations of basin hopping to perform.         minimizer (str): The minimizer to use at each basin hopping stop                          Valid options are: SLSQP TNC, and L-BFGS-B      Returns: The results as a dictionary # nx6 arrays x,y,z,axis-angle: #optimize # define the new step taking routine and pass it to basinhopping Calculates the difference between a guess at robot 2 cam transformations     compared to gathered data. Uses manhattan error for the distance (as     opposed to true line distance). Finds the angular difference between two     points. Takes the weighted sum of the manhattan distance and angular     distance based on the ratio.      Args:         guess (1x12 array): Input guess array. Values will range between the                             bounds passed in the optimize function. 6 dof                             camera 2 robot (x,y,z,axis-angle), 6 dof tcp 2                             target (x,y,z,axis-angle)         tcp2robot (nx6 array): Array of gathered data for the pose of the robot                                tool center point wrt. the robot coordinate base         camera2grid (nx6 array): Array of gathered data for the transformation                                  from the camera to the target         ratio (float): The ratio of weight given to the manhattan error vs the                        angular error. A higer value will give more weight to                        the manhattan error and less to the the angular error.                        Must be in the range [0,1]      Returns: A float, the total error between the guess and the collected              data http://stackoverflow.com/questions/22354094/pythonic-way-of-detecting-     outliers-in-one-dimensional-observation-data/22357811#22357811 Converts a vector in form x,y,z,axis-angle to a homogenous transformation     matrix      Args:         vector (6 element list): a vector representation form of a                                  transformation matrix. x,y,z,axis-angle      Returns: A 4x4 np.ndarry of the homogenous transformation matrix Converts a transformatiion matrix into a 6 dof vector. x,y,z,axis-angle     Args:         mat (4x4 ndarray): the transformation matrix      Returns: A 6 element list, x,y,z,axis-angle Prints out the local minimum result found in each iteration of the     basinhopping routine. random displacement with bounds. For use with the baisnhopping routine.     Based on: http://stackoverflow.com/questions/21670080 Initializes a displacement generator          Args:             xmin (list of floats): The minimum values for all of the paramaters             xmin (list of floats): The maximum values for all of the paramaters             stepsize: The initial stepsize for the algorithim. This will be                       overwritten by the basinhopping routine. Take a random step, from the prior, proportional to the stepsize wrt         the bounds. Ensure the new position is within the bounds          Args:             x (np.array of floats): The prior position          Returns:             The new starting position for optimization