Depth Estimation with Neural Nets

Depth Estimation from 2D Monocular RGB Images using Serial U-Nets

Depth Estimates from 2D RGB Images from KITTI Dataset

Knowledge of environmental depth is required for successful autonomous vehicle navigation and VSLAM. Current autonomous vehicles utilize range-finding solutions such as LIDAR, RADAR, and SONAR that suffer drawbacks in both cost and accuracy. Vision-based systems offer the promise of cost-effective, accurate, and passive depth estimation to compete with existing sensor technologies.

Existing research has shown that it is possible to estimate depth from 2D monocular vision cameras using convolutional neural networks. Recent advances suggest that depth estimate accuracy can be improved when networks used for supplementary tasks such as semantic segmentation are incorporated into the network architecture.

Recently, Kyle Cantrell, Dr. Carlos Morato, and I published a paper exploring a novel Serial U-Net (NU-Net) architecture. The Serial U-Net is introduced as a modular, ensembling technique for combining the learned features from N-many U-Nets into a single pixel-by-pixel output. Serial U-Nets are proposed to combine the benefits of semantic segmentation and transfer learning for improved depth estimation accuracy.

Using the Serial U-Net architecture, we were able to create some pretty cool 3D reconstructions from a single RGB image:

3D Reconstruction from Single RGB Image

All of our code is freely available on GitHub. You can use our pre-trained network on an existing video or a live webcam feed. And of course, you can also train the network on your own dataset of RGB and depth image pairs (similar to what is found in the NYU Depth V2 or KITTI datasets). 3D reconstruction MATLAB code is available here.

If you’re interested, you can find the full paper here.

MATLAB Class: Robot Arm Kinematics

MATLAB class of functions for analyzing serial kinematic chain manipulators

Here’s a MATLAB class of functions for analyzing serial kinematic chain manipulators. It includes functions for:

  • Forward manipulator kinematics (up to 6 degrees of freedom)
  • Denavit-Hartenberg (DH) matrix generation
  • Cubic polynomial trajectory generation
  • Homogeneous transformation matrix generation
  • Planar arm forward & inverse kinematics (from geometry)

To use any of these functions, save the entire class as a .m file in the same directory as your script. Then call RobotKinematics.FunctionName(args).

classdef RobotKinematics
    % Kinematics and trajectory planning functions for manipulator arms.
    % 
    % Usage: Call RobotKinematics.FunctionName(args)

    methods(Static)
        %% 6-DOF Manipulator: Forward Kinematics
        function [P]=forKin(qdes,joint_num,A)
            % Forward Kinematics for Manipulator Arm (up to 6 DOF) in terms
            % of CF0.
            %
            % qdes: Vector of desired joint positions (radians and meters)
            % joint_num: Joint of Interest (integer)
            % A: 3D array of 2D Frame Transformation A-matrices (DH)

            syms theta1 theta2 theta3 theta4 theta5 theta6;
            theta=[theta1 theta2 theta3 theta4 theta5 theta6];
            theta=theta(1:joint_num);

            % Calculate T-matrix (Composite Frame Transformation Matrix
            % (DH))
            T_local=computeT(A,joint_num);
            T_local=subs(T_local,theta(1:joint_num),qdes(1:joint_num));
            % Substitute value for pi in case symbol was used
            T_local=subs(T_local,pi,3.14159265359);
            P=vpa(T_local(1:3,4),3);
        end
        %% Denavit-Hartenberg (DH) Matrices
        function [T] = computeT(A,n)
            % Calculate Composite Frame Transformation Matrix (DH)
            %
            % A: 3D array of 2D Frame Transformation A-matrices (DH)
            n=size(A,3);
            for ii=1:n
                %Calculate Composite Transformation Matrix
                if ii==1
                    T=A(:,:,ii);
                else
                    T=T*A(:,:,ii);
                end
            end
        end
        
        function [A] = computeA(theta,d,a,alpha)
            % Calculate Frame Transformation Matrix (DH)
            %
            % DH Parameters:
            % theta: Rotation about z-axis (radians)
            % d: Translation about z-axis (meters)
            % a: Translation about x-axis (meters)
            % alpha: Rotation about x-axis (radians)
            A=computeRzh(theta)*computeTzh(d)*computeTxh(a)*computeRxh(alpha);
        end
        %% Trajectory Generation: Cubic Polynomial
        function [a]=a_coeffs(t0,tf,q0,qf,dq0,dqf)
            % Solve for a-coefficients required for cubic polynomial 
            % for defining position (p.197, Spong)
            %
            % t0: Start time (seconds)
            % tf: End time(seconds)
            % q0: Start joint position (radians)
            % qf: End joint position (radians)
            % q0: Start joint velocity (radians/second)
            % qf: End joint velocity (radians/second)
            a=[q0;                                          %a0
               dq0;                                         %a1
               (3*(qf-q0)-(2*dq0+dqf)*(tf-t0))/((tf-t0)^2); %a2
               (2*(q0-qf)+(dq0+dqf)*(tf-t0))/((tf-t0)^3)];  %a3
        end

        function [q]=joint_pos(a,t)
            % Calculate position from cubic polynomial
            %
            % a: Vector of a-coefficients required for cubic polynomial 
            % t: time (seconds)
            a_flip=flip(a); %Put in descending order (highest order term first)
            q=polyval(a_flip,t);
        end

        function [dq]=joint_vel(a,t)
            % Calculate velocity from cubic polynomial
            %
            % a: Vector of a-coefficients required for cubic polynomial 
            % t: time (seconds)
            a_flip=flip(a); %Put in descending order (highest order term first)
            a3=a_flip(1); a2=a_flip(2); a1=a_flip(3);
            dq=a1+2*a2*t+3*a3*(t.^2);
        end

        function [ddq]=joint_accel(a,t)
            % Calculate acceleration from cubic polynomial
            %
            % a: Vector of a-coefficients required for cubic polynomial 
            % t: time (seconds)
            a_flip=flip(a); %Put in descending order (highest order term first)
            a3=a_flip(1); a2=a_flip(2);
            ddq=2*a2+6*a3*t;
        end
        %% Rotation and Transformation Matrices
        function [Rxh] = computeRxh(theta)
            % Compute Basic Homogeneous Transform Matrix for
            % rotation of theta (radians) about x-axis
            Rxh=[1 0 0 0; ...
                 0 cos(theta) -sin(theta) 0; ...
                 0 sin(theta) cos(theta) 0; ...
                 0 0 0 1;];
        end

        function [Ryh] = computeRyh(theta)
            % Compute Basic Homogeneous Transform Matrix for
            % rotation of theta (radians) about y-axis
            Ryh=[cos(theta) 0 sin(theta) 0; ...
                 0 1 0 0; ...
                 -sin(theta) 0 cos(theta) 0; ...
                 0 0 0 1];
        end

        function [Rzh] = computeRzh(theta)
            % Compute Basic Homogeneous Transform Matrix for
            % rotation of theta (radians) about z-axis
            Rzh=[cos(theta) -sin(theta) 0 0; ...
                 sin(theta) cos(theta) 0 0; ...
                 0 0 1 0; ...
                 0 0 0 1];
        end

        function [Txh] = computeTxh(d)
            % Calculate Basic Homogeneous Transform Matrix for
            % translation of d (meters) along x-axis
            Txh=[1 0 0 d; ...
                 0 1 0 0; ...
                 0 0 1 0; ...
                 0 0 0 1];
        end

        function [Tyh] = computeTyh(d)
            % Calculate Basic Homogeneous Transform Matrix for
            % translation of d (meters) along y-axis
            Tyh=[1 0 0 0; ...
                 0 1 0 d; ...
                 0 0 1 0; ...
                 0 0 0 1];
        end

        function [Tzh] = computeTzh(d)
            % Calculate Basic Homogeneous Transform Matrix for
            % translation of d (meters) along z-axis
            Tzh=[1 0 0 0; ...
                 0 1 0 0; ...
                 0 0 1 d; ...
                 0 0 0 1];
        end
        %% Planar Arm: Forward & Inverse Kinematics
        function [x,y]=planar_forKin(q,l)
            % Calculate forward kinematics for Planar Arm 
            % robot geometrically.
            %
            % q: Vector of Joint Positions (radians)
            % l: Vector of Link Lengths (meters)
            q1=q(1);q2=q(2);l1=l(1);l2=l(2);
            x=l1*cos(q1)+l2*cos(q1+q2);
            y=l1*sin(q1)+l2*sin(q1+q2);
        end

        function [q1,q2]=planar_invKin(x,y,l,elbow_up)
            % Calculate inverse kinematics for Planar Arm robot
            % geometrically.
            %
            % x: End Effector x-Position (meters)
            % y: End Effector y-Position (meters)
            % l: Vector of Link Lengths (meters)
            % elbow_up: 0 or 1
            l1=l(1);l2=l(2);
            D=(x^2+y^2-l1^2-l2^2)/(2*l1*l2);
            if elbow_up==1
                q2=atan2(-sqrt(1-D^2),D);
            else %elbow is down
                q2=atan2(sqrt(1-D^2),D);
            end
            q1=atan2(y,x)-atan2((l2*sin(q2)),(l1+l2*cos(q2)));
        end
    end
end

Also available on GitHub.

Transformation Matrices for Robotic Arms

Python functions for serial manipulators.

# -*- coding: utf-8 -*-
"""
Functions for calculating Basic Transformation Matrices in 3D space.
"""
from math import cos, radians, sin
from numpy import matrix

def rotate(axis, theta, angular_units='radians'):
    '''Compute Basic Homogeneous Transform Matrix for
    rotation of "theta" about specified axis.'''
    #Verify string arguments are lowercase
    axis=axis.lower()
    angular_units=angular_units.lower()
    #Convert to radians if necessary
    if angular_units=='degrees':
        theta=radians(theta)
    elif angular_units=='radians':
        pass
    else:
        raise Exception('Unknown angular units.  Please use radians or degrees.')
    #Select appropriate basic homogenous matrix
    if axis=='x':
        rotation_matrix=matrix([[1, 0, 0, 0],
                               [0, cos(theta), -sin(theta), 0],
                               [0, sin(theta), cos(theta), 0],
                               [0, 0, 0, 1]])
    elif axis=='y':
        rotation_matrix=matrix([[cos(theta), 0, sin(theta), 0],
                               [0, 1, 0, 0],
                               [-sin(theta), 0, cos(theta), 0],
                               [0, 0, 0, 1]])  
    elif axis=='z':
        rotation_matrix=matrix([[cos(theta), -sin(theta), 0, 0],
                               [sin(theta), cos(theta), 0, 0],
                               [0, 0, 1, 0],
                               [0, 0, 0, 1]])
    else:
        raise Exception('Unknown axis of rotation.  Please use x, y, or z.')
    return rotation_matrix

def translate(axis, d):
    '''Calculate Basic Homogeneous Transform Matrix for
    translation of "d" along specified axis.'''   
    #Verify axis is lowercase
    axis=axis.lower()
    #Select appropriate basic homogenous matrix
    if axis=='x':
        translation_matrix=matrix([[1, 0, 0, d],
                                  [0, 1, 0, 0],
                                  [0, 0, 1, 0],
                                  [0, 0, 0, 1]])
    elif axis=='y':
        translation_matrix=matrix([[1, 0, 0, 0],
                                  [0, 1, 0, d],
                                  [0, 0, 1, 0],
                                  [0, 0, 0, 1]])
    elif axis=='z':
        translation_matrix=matrix([[1, 0, 0, 0],
                                  [0, 1, 0, 0],
                                  [0, 0, 1, d],
                                  [0, 0, 0, 1]])
    else:
        raise Exception('Unknown axis of translation.  Please use x, y, or z.')
    return translation_matrix

if __name__=='__main__':
    #Calculate arbitrary homogeneous transformation matrix for CF0 to CF3
    H0_1=rotate('x', 10, 'degrees')*translate('y', 50)
    H1_2=rotate('y', 30, 'degrees')*translate('z', 10)
    H2_3=rotate('z', -20, 'degrees')*translate('z', 10)
    H0_3=H0_1*H1_2*H2_3
    print(H0_3)

Also available on GitHub.