ecse 475 final report intakhab radu tabassum

ROBOT

CALIBRATION Determination of Robot Motion Accuracy Using Artificial

Perception Devices

ECSE 475-DESIGN PROJECT II – Final Report Intakhab Alam - 260271961

Radu Sturzu - 260330228

Tabassum Mizan – 260345222

(under the supervision of Professor Frank P. Ferrie)

ECSE

475

16/4/2013

Design Project 2-Final Report

2

Abstract

The Gantry automated robot is an industrial robot permitting motions with six degrees of freedom which

can find applications in many fields. As some tasks re very accuracy dependant, it is required to verify the

accuracy of the motions performed by the robot before making use of the robot on the field.

The purpose of this project is to determine the accuracy of the motions performed by a Gantry automated

robot across the X Y and Z axes. An off the shelf video capture device will be used to track LED lights

mounted on the robot’s arm as the robot moves through space. The motion patterns are written using

RAPLII as a programming language. A simple MATLAB interface that can operate the robot will be also

validated, improved and rendered more user-friendly to the common user.

Through familiarisation with the robot, its physical functioning and the programming behind it, as well

through research on various artificial imaging techniques and devices, protocols have been developed for

measuring the accuracy for single motions across the X, Y or Z axes al low movement speeds and for

validating the MATLAB interface. During the second part of the project, these protocols will be fine-

tuned, the MATLAB interface will be improved and protocols will be developed for determining the

accuracy of the robot for compound motions at various speeds.


3

Acknowledgements

We would like to acknowledge Professor Frank. P. Ferrie and the Department of Electrical and Computer

Engineering and the Centre for Intelligent Machines (CIM) for giving us the opportunity to work on this

project. We would also like to extend our appreciation to Professor Roni Khazaka for guiding us

throughout the project.


4

Table of Contents Abstract ............................................................................................................................................... 2

Acknowledgements ............................................................................................................................. 3

List of Figures & Tables ........................................................................................................................ 5

Group Members................................................................................................................................... 6

Project Supervisor ................................................................................................................................ 6

Intellectual Property ............................................................................................................................ 6

Introduction ......................................................................................................................................... 7

Background ......................................................................................................................................... 7

Design and Implementation ............................................................................................................... 12

RAPL – II Programming ...................................................................................................................... 13

Current MATLAB System and Modifications Required......................................................................... 15

Camera Calibration Toolbox ............................................................................................................... 18

Results and Tests ............................................................................................................................... 21

Defined Path Used for Testing ............................................................................................................ 28

Impact on Society and Technology ..................................................................................................... 33

Conclusion ......................................................................................................................................... 34

References ......................................................................................................................................... 35

Appendix A ........................................................................................................................................ 36

Appendix B ........................................................................................................................................ 48

Appendix C: ....................................................................................................................................... 50


5

List of Figures & Tables

A GANTRY ROBOT ................................................................................................................................... 8

THE EPIPOLAR CONSTRAINT ..................................................................................................................... 9

CORRESPONDENCE PROBLEM SEARCH ALGORITHMS: 2D SEARCH DOMAIN (TOP) VS. 1D SEARCH DOMAIN

(BOTTOM) ...................................................................................................................................... 10

FORMULA FOR FINDING THE DEPTH FOR THE DISPARITY MAP ................................................................. 10

DISPARITY FOR A FAR AWAY OBJECT (LEFT) AND FOR A CLOSER OBJECT (RIGHT) ................................... 11

TRIANGULATION FORMULAS FOR 3D COORDINATE OF IMAGE POINTS ..................................................... 11

FLOW CHART FOR THE MATLAB DESIGN ............................................................................................. 17

HOW MATLAB CONNECTS WITH RAPL II AND THE ARM ...................................................................... 18

THE INTRINSIC PARAMETER, THE EXTRINSIC PARAMETERS ................................................................... 19

CAMERA CALIBRATION TOOLBOX ......................................................................................................... 19

CHECKERBOARD TO BE USED ................................................................................................................. 20

CAMERA CALIBRATION TOOLBOX INPUT ............................................................................................... 20

LOCALIZING IMAGES ON THE CHECKERBOARD ...................................................................................... 20

INPUTTING THE SQUARE SIZES ............................................................................................................... 21

TOP DOWN VIEW OF THE LOCATION OF THE GANTRY ROBOT .................................................................. 21

RIGHT TRIANGLE FOR MEASURING MINIMAL HORIZONTAL ANGLE OF CAMERA ....................................... 22

RELATIVE ERROR AT 0.8 M, RELATIVE ERROR AT 2 M ............................................................................. 27

THE ROBOT (BEFORE SETUP), THE ROBOT (AFTER SETUP)...................................................................... 29

DISTANCE BETWEEN THE TWO CAMERAS IS 24 CENTIMETERS ................................................................. 29

THE SQUARE MOTION OF THE ROBOT ..................................................................................................... 30

SAMPLE OF A CUBE MOTION IMPLEMENTATION ...................................................................................... 33

INTRINSIC PARAMETERS ........................................................................................................................ 48

EXTRINSIC PARAMETERS ....................................................................................................................... 49

TRIANGULATION FROM PROJECTION MATRIX ......................................................................................... 49

RAPL - II COMMANDS AND DESCRIPTIONS ............................................................................................ 14

LOCATION AND SIZE OF THE GANTRY ROBOT CAGE ................................................................................ 22

MEASUREMENTS TABLE (IN CM) ............................................................................................................ 27

ACCURACY TABLE (IN CM)..................................................................................................................... 28

READINGS AT POSITION A TABLE (IN CM) ............................................................................................... 30

READINGS AT POSITION B TABLE (IN CM) .............................................................................................. 31

READINGS AT POSITION C TABLE (IN CM) .............................................................................................. 31

READINGS AT POSITION D TABLE (IN CM) .............................................................................................. 32


6

Group Members

Name: Intakhab Alam Zeeshan

Student ID: 260271961

Course: Fall 2012 - ECSE-475-001 - Design Project II

Education: 4th year Bachelor of Electrical Engineering Student

Email: [email protected]

Name: Radu Sturzu



Education: 4th year Bachelor of Electrical Engineering Student, Minor in Software Engineering


Name: Tabassum Mizan



Education: 4th year Bachelor of Electrical Engineering

Student Email: [email protected]

Project Supervisor

Supervisor: Professor Frank. P. Ferrie

Office: McConnell Engineering Building, Room 441

Department: Electrical and Computer Engineering and the Centre for Intelligent Machines (CIM)


Tel: (514)398-6042

Intellectual Property

The intellectual scope for the software and hardware material, as well as all the employed knowledge

base, and any code employed or developed within this project has been decided by Professor Frank. P.

Ferrie. No attempt to divulge any of the previously listed outside of the team, for commercial or non-

commercial purposes, is to be made without the personal consent of the professor. However, the

documentation files on MATLAB, OpenCV or Python are freely available to the public.

mailto:[email protected]





7

Introduction

The project consists of finding the amount of error that a state-of-the-art industrial robotic arm produces

during operation. It will be programmed to perform specific movements and gestures around a given

space. If the movements do not take place precisely as desired, the amount of error that arises will be

measured and recorded. Using the data and the error generated, an “error mapping” will be created. The

robotic arm is operated by a series of motors that provide six degrees of freedom, with the inputs and

outputs being connected to a controller. Here the error is defined as the distance in X, Y, Z between the

actual location of the arm and the location computed by the software. The initial commands, as well as the

movement of the arm, are controlled through MATLAB.

Built with industrial use in mind, this project would be useful for a number of applications requiring

automated robotic arms, ranging from manipulation of hazardous substances to providing welding

tutorials to employees. As an outcome, it is expected that the error will be fully understood, allowing for

the necessary software and hardware calibrations to be performed, thus ensuring a flawless operation of

the equipment.

At this time, the project consists of two parts. The initial part translates to getting to gaining knowledge

about the environment and the commands that operate the robot, determining a preliminary protocol for

measuring the accuracy of the robot motions, which will be used by an existing Matlab interface. The

second part consists primarily of implementing the accuracy measurement protocol and testing it with a

series of robot movements, as implementing those changes inside the existing Matlab interface, as well

as rendering that interface as user friendly as possible. The required material comprises robotic arm itself,

with the necessary peripherals, measuring tools, documentation for both the Gantry robot and Matlab, as

well a lab featuring a computer containing MATLAB.

Background

We are working with a robot commonly recognized as a gantry robot is also known as a Cartesian robot.

It looks very different from what many people would imagine a robot looked like [1]

. It contains a

minimum of three elements of motion and is mostly stationary [1]

. All the three motions are typically

linear motions in a single direction [1]

.

For a Gantry robot, each motion axis is perpendicular to the other ones; the axes are typically labeled X,

Y, and Z [1]

. X and Y are located in the horizontal plane and can be considered as the width and length of


8

a box and Z is vertical, similar to the height of the box [1]

. The interior of this box is the working envelope

of the gantry robot and can perform various kinematics and operations on an item within the envelope [1]

.

First of all, measuring the accuracy of motion of the gantry robot requires a system for measuring the

position of the robot. Secondly, it also requires instructions that can move the gantry robot, as well as a

user interface that will control the gantry robot. The Gantry installation and operation manual offers the

basic necessary knowledge in understanding how the robot works, how it interfaces with the user as well

as the outside world. It also offers information on safety measures, as well as how to use the teaching

pendant, which is a remote control, designed to interface with the robot through a serial port.

All the programming instructions to be used with the robot are fully documented in the RAPLII

Programming Manual. Most of the reading time by the team members was spent studying this section.

The Process Control Programming KIT deals with advanced functions for the Gantry controller, which is

not a requirement at the moment. Time will also be spent in future attempting to implement more

complicated commands, such as controlling the robot to perform more advanced motions, involving more

complex paths, as well as motions involving the Yaw, Pitch and Roll axes.

Figure 1: A Gantry Robot


9

A stereo camera system offers the possibility of measuring the position of a moving object in three

dimensions. It requires solving the correspondence problem, namely correctly determining which part of

the image recorded by one camera corresponds to which part of the image recorded by the other camera

[7].

Every stereo camera system requires these four steps before successfully mapping a two dimensional

camera image onto three dimensions [7]

, namely calibration, rectification, stereo correspondence and lastly

triangulation.

Calibration is the process of finding the intrinsic and the extrinsic parameters of the camera. Intrinsic

parameters include the focal length, image center, and the parameters of the lens distortion. The extrinsic

parameters consist of finding the correct relative positions between the left and the right cameras.

Rectification involves using the results from the calibration step for removing lens distortion, as well as

using those results for aligning the two cameras: this latter step allows us to use the epipolar constraint in

order to solve the correspondence problem with a significantly lower computational cost.

The epipolar constraint [7]

tells us that the correspondence for a point belonging to the red line of sight of

the image recorded by the left camera lies on the green light of sight of the image recorded by the right

camera, as seen in Figure 2.

Figure 2: The epipolar constraint


10

As seen in the example from Figure 3, the epipolar constraint allows us to search for a corresponding

point along a single horizontal line, instead of searching across the entire two dimensional plane of the

image [6]

.

Figure 3: Correspondence problem search algorithms: 2D search domain (top) vs. 1D search domain (bottom)

The following step translates in finding the disparity map between the two images. The disparity consists

of the difference in x coordinate between the left and the right images; its value is larger for points closer

to the camera, which allows us to map out the depth of the objects in our image. Given a stereo setup as

seen in Figure 5, where b is the baseline length, f is the focal length, d=XR – XT is the disparity, the depth

Z can be calculated given the formula from Figure 4[7]

.

Figure 4: Formula for finding the depth for the disparity map


11

Figure 5: Disparity for a far away object (left) and for a closer object (right)

As can be seen on the right image from Figure 5, bringing the observed real point P closer to the cameras

will increase the disparity XR-XT.

The final step allows us to triangulate the position of correspondence in 3d space, using the formulas

given in Figure 6, given the disparity map, the baseline as well as the focal length [7]

.

Figure 6: Triangulation formulas for 3D coordinate of image points

In order to provide more photographic image quality, cameras equipped with a CCD sensor have an

added infrared filter: this has the effect of blocking much of the near-infrared spectrum, which would

otherwise have the adverse effect of rendering the images less photographic by adding a glow effect. [8]


12

Design and Implementation

The first part of this project consists of developing the solutions for determining the accuracy of the

motions. In order to measure the accuracy of motions through space, manual measuring tools were first

considered. By inspection we decided that this introduced significant room for human error, as well as

lack of sufficient accuracy. With the recent advancements in performance and image quality of web-

cameras which are available at affordable prices, we have opted for using an off the shelf web cam to

measure the accuracy of motion of the Gantry chart. A further factor in our choice was the availability of

a significant body of data available on camera calibration and on mapping 2d camera data onto 3d

coordinates.

A stereo camera setup consisting of two cameras placed in parallel from each other will be used. This

allows for real-time three-dimensional readings of the position. Due to spatial limitations, there is a

maximum distance that the camera can be placed at from the scene. Due to this condition, the maximum

angle of the camera is critical.

The two cameras will need to be of the same type; this allows for an easier acquiring of images as well as

a more straightforward use of the intrinsic and extrinsic camera parameters. A camera model that allows

for accurate readings of accuracy must be chosen. This involves a higher resolution requirement, thus

allowing mapping onto pixels smaller regions of space. A sufficient refresh rate is required in order to

fully track the motion of the robot, and the movement speed must be adjusted accordingly; more research

is to be performed in this area in the second part of the project. Lastly, large enough vertical and

horizontal view angles are required in order to keep track of the whole scene and monitor or the necessary

robot motions. Although off the shelf-cameras are used, prices should still be considered, as the higher-

end camera models are significantly more costly.

A laser pointer LED mounted on the robot arm will be used in order to easily track the motion of the

robot. An appropriate type laser must be chosen (preferably an infrared laser), and the intensity must be

calibrated manually: if the spot is too bright, the size of the spot will be too large, thus introducing

imprecision, while if the spot is too dim, the light may become undetectable.

As the laser spot will cover a continuum of pixels on the image, sub pixel interpolation will be considered

in order to determine the accuracy to a minimum of 0.2 mm of accuracy. Sub pixel interpolation allows

producing an average of the two adjacent points, in order to better approximate the real position of the

spot, whenever the center of the laser spot lies between two pixels. We will delve further into this subject

as we reach the second part of this design project.


13

In order to implement the stereo visions algorithm, the Camera Calibration Toolbox for Matlab, as well as

Open Source Camera Vision applications is being considered. [9]

A black and white check board will be

used for the calibration step.

RAPL – II Programming

RAPL – II, also known as Robotic Automation Programming Language, is a CRS proprietary language. It

is designed for robot systems application due to its ease of automation. The programming format is line

structured much like the BASIC structure. It is very user friendly due to English like commands.

RAPL II has many interesting features, such as, efficient coding structure for most effective use of

memory, alternate command identifiers and lastly advanced mathematical expressions. For experienced

programmer, this RAPL II programme is similar with basic programming, such as C.

Although it has numerous advantages, RAPL – II has a few key shortcomings. It only allows execution of

one main function and does not allow any subroutines. Case statements are also not available to use thus

greatly limiting efficient control flow. This greatly reduces the programming capability in RAPL – II.

RAPL – II provides parameters that define the robot coordinate system, i.e. x, y and z axes. It provides

commands using which the robot can be easily calibrated and localized. There are two modes of

operation:

a) Manual Mode: Using a teaching pendant (controller) provided with the robot, the robot can be

moved in the x, y and z directions by pressing buttons.

b) Line editor: Our project requires automatic control and movement of the robot. The line editor

allows us to easily send automatic commands and will be used primarily in the project.

RAPL – II provides multiple commands to control the robot. Although there are numerous commands

available to us, the most important ones are described in Table 1.

RAPL – II has defined different workspace environments:

a) Joint – Coordinate Systems: The A255 robot system uses this coordinate system. This series

consists of 5 different revolute joints. The zero positions for the respective joints are located at

the midpoints.


14

b) World – Coordinate Systems: This system applies to robots with transformation capability. Such

a robot is the A465 series. In this robot, the origin (0, 0, 0) is at the centre of the base on the

ground plane.

Another important factor is the speed at which the arm will be moved. This has significant impact on

error analysis. A slow movement allows the robotic arm to keep up with the refresh rate of the camera

being used. Faster movements or rapid acceleration provides different accuracy reading as it may be too

fast for the camera to record the movement. In operations where rapid motion is required, the accuracy

becomes less critical. In the first part of this project, slow movement of the arm is assumed.

In order to accurately calibrate and localize the gantry robot, position analysis is extremely important.

Returning to origin and current position identification is crucial. If the robot successfully returns to origin,

it provides a known reference point for movement along the axes. In order to identify its current position,

RAPL – II command ‘W2’ displays the robots current position. This can be used to calculate error by

analyzing the position obtained from the camera and the LED.

Command Type Description

Motion Commands The motion command is used in the movement of the arm controlled by

RAPL – II programming. There are maximum eight axes of motion, which

the controller can command. In the case of the gantry robot, the movement

is limited to the 3 axes – x, y, and z. The speed of the motion is controlled

with SPEED command, maximum (0 – 100%)

Gripper Commands The gripper is basically controlled by RAPL – II for open and close

commands. This is designed as a control pneumatic, servo and

electromagnetic gripper. The air pressure of the gripper is controlled for the

gripping force.

Robot Location assignment By using this command, the robot can be moved directly to a set of

coordinates on a grid

Pose Commands These commands are used to move joints in a robot thus allowing the robot

to be put in multiple joint configurations

Homing Commands In order to control the robot system, RAPL – II must know the exact

position of the joints of the machine. It accomplishes this by moving each

joint to an accurately repeatable location. Once the robot has moved, a

number is taken from a special memory register and loaded into the current

position value. RAPL – II will now know where the joint is and can

position the joint by counting encoder pulses up and down from there as he

joint moves

Table 1: RAPL - II Commands and descriptions


15

Current MATLAB System and Modifications Required

MATLAB is relatively easy to learn and is optimized to be relatively quick when performing matrix

operations and in some cases may behave like a calculator or as a programming language. It is an

interpreted language so errors are easier to fix. Hence, all the measurements from the external sensors will

be feed in into the MATLAB script to produce results. It does have a few limitations hence it will be

combined with RAPL II program and command lines to make the process more efficient and user-

friendly.

The MATLAB session currently present in the lab is capable to calibrate the gantry robot. It can take an

(Nx6) matrix as an input and can send the command through RAPL II programming, which activates the

kinematics of the robot as per the commands. It also allows us to change the speed parameters as well as

based on the position of the robot, can provide us a 3-dimensional picture of its location.

Operating the machine introduced us to the different limitations and abnormal behaviors the robot arm is

subjected to. The abnormal behavior comes from the fact that testing was not done to the fullest due to

availability of resources. Initially, all work was done using the terminal interface of the robot. Later on it

was moved to the MATLAB interface since it provided us more information in real time and also aids on

obtaining the position of the robot in the virtual XYZ axis. This led us to a bug that was present in the

program that was already loaded and was used to connect the robot arm with the computer itself. The

MATLAB program currently designed is a high level program and an extensive training is required

before a user can work hands-on with the automated arm. A simplified and more user-friendly MATLAB

session needs to be designed using the current one as the backbone to make sure when this industrial

robot is out in the field its basic functions can easily be mastered within a few hours so that work can

resume without any disruptions.

In midst of all the actions it can currently perform, the current system does not allow us to verify if the

robot has indeed made to its final position as per its commands or not. It does not provide us any

assurance of the accuracy and how off it was in terms of errors when reaching its designated position.

And since, everything is based on soft code it is not robust at all.

Figure 8 shows how MATLAB and RAPL II relate to each other.

The flow chart in Figure 7 is the first foot print of what we will be aiming to design for the MATLAB

simulation. This starting point will be used as the reference point for all calculations hereafter.


16

In order to improve the performance of the current MATLAB program, the following design was

proposed as shown in Figure 4. It adds a camera calibration method on the current program. Initially

when the robot will be started, the arm will be homed and calibrated to a starting point. It localizes the

robot to an origin (0, 0, 0) from any position it previously resided on within the working envelope. Once

calibrated it assigns metric units to the robot, so that it runs on the same units as the program intends it to.

Once all this has been performed the system than connects itself to the MATLAB program, disabling the

manual controller.

Our program once executed will activate two processes at the same time. It will initiate the link with the

robot through the RAPL II program as well as initiate the camera being placed. The camera will be

feeding in data continuously, as the robot simultaneously moves through the working envelope. These

data will then be used to calculate the actual position of the robot in the envelope and also to find the

errors and accuracy associated with each movement.

The previous MATLAB program will be used as the backbone of the current design; hence it will retain

all other previous capabilities, such as displaying the results on a 3-dimensional grid.


17

Calibrate Data Obtained

Input Data Through MATLAB Interface

Initial Start Up of Program

CalibrationAssign Units

Conversion of RAPL II Programs to MATLAB and

storing them

Convert Output Values to Visual Data

Display Results in MATLAB 3D

interface

START OF PROGRAM

Read Real Time Data from

Camera Feed

Initiate Camera

Kill Program

Execute Input Commands in RAPL II Program

Interface

Read Output Values using

RAPL II Commands

Figure 7: Flow Chart for the MATLAB Design


18

Figure 8: How MATLAB connects with RAPL II and the arm

Camera Calibration Toolbox

The reason behind choosing a camera calibration method is because it is more advantageous that any

mechanical measurement systems for kinematic calibration [2]

. A mechanical measurement involves wire-

based system which in some cases are very difficult to setup and handle [2]

. In addition to that, a

mechanical system needs to be physically attached or somehow connected to the moving robot itself [2]

.

Laser trackers can be highly accurate, but has a drawback of being too expensive [2]

. Hence a more

effective and economical solution was therefore to use a computer vision including appropriate image

processing software [2]

.

The camera calibration method uses the intrinsic and extrinsic parameters obtained from the camera to

calculate the accuracy of the robot kinematics. The intrinsic parameters include:

Geometrical Distortion

Radial Distortion

The geometrical distortion involves the position of the image points on the image plane and the radial

distortion concerns the inward or outward displacement of a given image point from its ideal location [3]

.

The extrinsic parameters include:

Pan

Tilt

Swing


19

The extrinsic parameters will not be used widely for the first part of the implementation. It will be

mostly used for complex movements further in the project.

Figure 9: The Intrinsic Parameters Figure 10: The Extrinsic Parameters

Based on the parameters obtained from the camera we can then input the data on a viable MATLAB

program which is to be implemented to obtain the pattern detection and the relative orientation between

the robot and the camera. The calibration pattern can then be further used to calculate the images

localized to get the distance moved by the robot and also its actual position.

The camera calibration toolbox is an open source program that provides us the calibration pattern

localized on an image when using a checkerboard method. It also provides us the relative orientation

between the camera frame and calibration as well as an algorithm which returns back pixel-based

coordinate system [4]

. It is a user friendly interface that takes into input the number of images to be

processed and the window size to detect the corner [4]

. It also requires the number of squares present in the

X-direction and also the sizes of the square [4]

.

Figure 11: Camera Calibration Toolbox


20

Figure 12: Checkerboard to be used

Figure 13: Camera Calibration Toolbox Input

Figure 14: Localizing Images on the Checkerboard


21

Figure 15: Inputting the Square Sizes

Results and Tests

A series of measurements were manually performed in order to determine the various constraints imposed

on the cameras used for measuring the accuracy. The location of the Gantry robot cage inside the

laboratory room from a top-down view can be seen in Figure 16, where dx and the dy represent the

distance from the room walls across the X and Y axes, while x and y represent the length and width

parameter of the area covered by the Gantry cage. Table 2 shows the measured values for the parameters,

where z represents the height of the cage.

Figure 16: Top down view of the location of the Gantry robot

Variable Value

X 158 cm

Y 154 cm


22

Z 125 cm

dx 84 cm

dy 73 cm

Table 2: Location and size of the Gantry robot cage

The cameras are expected to be mounted on the mid left of the room, as shown by the yellow star drawn

in Figure 16: this allows for the furthest possible distance from the Gantry robot, and thus the largest

viewing angle.

Considering that the robot arm has a diameter of close to 25 cm, we are accounting for a motion space of

a minimum of around one meter. This results in a remaining motion space of 75 cm (100cm – 25 cm),

before the robot arm is not fully inside the field of view of the camera. Given this factor and considering

that the robot is at the closest distance from the wall (84 cm), by applying simple trigonometry to the

triangle in Figure 17 and multiplying with two results in a minimum horizontal angle of 44 degrees.

Figure 17: Right triangle for measuring minimal horizontal angle of camera

After accounting for the beams that may obstruct the view of the cameras across the diagonals), the

camera should be able to see up to a height of ~75 cm. (the maximum height of the skeleton is 125 cm, as

recorded in Table 2). A calculation for the vertical angle similar to the one for the horizontal angle yields

a minimum angle of 2*40 = 80 degrees, if the camera were to be mounted at the floor level. As a vertical

angle this high is not found in most web cams, a different vertical mounting position must be chosen.

Mounting the cameras at exactly half the viewing distance will result in halving the required minimum

viewing angle. Thus, the new angle will be 40 degrees, which is much more easily attainable in current

models of web cams.


23

In order to reduce the dependency on ambient lightning and reduce the need for complex filtering or

additional light/contrast calibration, an infrared LED mounted on the robot will be used instead of regular

LED, in order to track the motion of the Gantry robot arm.

Most web cams available today possess CCD filters, and CCD filters are capable of seeing near-infrared

frequencies. The infrared filter of the web cam will be removed, as the CCD camera sensor performs

better at detecting infrared wavelengths with the filter taken out. This was discovered through personal

testing, where the spot of the infrared emissions from the LED of a remote control appeared larger when

the infrared filter was removed.

No specific model of camera was chosen at this point, more research will be performed during the second

part of the course in order to determine the minimum resolution required, a resolution that would allow us

to attain an accuracy of the position measurement of at least 0.2 mm. As mentioned in the paragraphs

above, all the other camera constraints have been fully determined.

A small RAPL – II script is explained below that can be used to calibrate the robot:

RUN Device

RUN CAL

RUN INIT_SYS

JOG 90 90 60

SPEED 30

STOP

@ZERO

A brief explanation is of the commands is given below:

JOG: move TCP by a Cartesian (X, Y, Z) increment

SPEED: Set speed of robot motion in percentage. A speed of 30% is chosen to move the controller as it

was assumed that speed > 50% is high speed thus leading to be out of focus of the camera.

CAL: Calibrate the robot arm

@Zero: Sets the current robot position to zero for all axes.

A few test scenarios have been implemented for the testing. Initially the testing will be quite straight

forward using only movements through each axis one at a time as shown in the script above. Later on,


24

more complex movements will be implemented after the robustness for our script has been identified.

Example of such a complex movement, could be a ``spline`` motion.

Even though the camera calibration toolbox algorithm does not give us exactly the errors and the accuracy

but it does give us the raw data and the TCP points of the localizations. Further implementation is

required to make sure that results are obtained in scaling metric units and also make sure users obtain the

best results in the simplest form.

This type of motion keeps the Tool Centre Point (TCP) moving in a straight line. As the geometry of the

tool or gripper which the robot carries can vary, RAPL – II provides programmers to define the tool tip

via TOOL coordinate programming.

As the second part of the project began, the old robot was unfortunately out of order, so the team is

waiting for new robot to be installed. During this period of time, we have received two C270 Logitech

cameras from our supervisor. We have experimented with acquiring images using the two cameras for

calibration and the Image Acquisition Toolbox from Matlab that make this possible. The cameras

function properly up to the maximum specified resolution of 1280x720. There was one problem where

attempting to use both USB cameras simultaneously due to both cameras attempting to access the same

USB controller. Upon changing the permissions of the ports accessed by the web-cams, the issue was

resolved with the help of Professor Ferrie. We have tried to use the GUI commands of the Matlab Image

Acquisition Toolbox for our image captures, but since it did not allow us to have two camera feeds active

at the same time, a script was instead; this script would acquire stereo images at pre-set time intervals

using manual triggering mode; this allows each of the cameras to perform the initialization process only

once at the beginning instead of before each image capture; as such, each subsequent image capture takes

less than 100 ms, versus around 1 second in automatic triggering mode. Using this, the image capturing

process is smoother and there is a significantly smaller delay between the capture of the left and right

images.

As we moved further in our design, we began investigating various camera calibration tools. We have

also installed the Camera Calibration Toolbox on Matlab, another option was OpenCV, which is C/C++

based. Initially we had found that the GUI of the Camera Calibration Toolbox had most of the features we

required, so at that point we did not need to use any code or any commands. We have experimented with

example images from camera calibration toolbox and we found the calibration process lengthy.


25

The new gantry robot was assembled at the start of the week. But it also had issues with the cables

connecting the arm to the main core. As a result, the whole robot malfunctioned. New cables were

ordered by Professor Ferrie in an attempt to fix one of the two gantry robots currently residing in the lab.

As no part of the gantry robot was at that point functional, Professor Ferrie has advised the team to look

into the other aspects of the project in more detail, such as the stereo calibration of the two cameras being

used. He has suggested and introduced us to one of his PhD students, Andrew Phan who has extensive

experience in the respective field. The team will be working and collaborating side-by-side with some his

work, in order to assemble an ideal stereo calibration system that will allow general gantry robots to

calibrate itself within its XYZ space at any light conditions.

So far the team has been using the Camera Calibration Toolbox as the pivotal software. But as per

Andrew Phan’s advice, the team will be using Open CV as well as Python to implement a more efficient

and better calibration system.

As time was short at this point in the semester, the team is implementing a new calculation method which

is different from what was planned initially. Specifically, a chessboard was used to track the position of

the Gantry robot hand, instead of using a laser pointer; since code already exists to detect the image points

of the chessboard corners, this simplifies the task of the team.

First, the cameras were mounted in parallel, at a certain distance (called baseline) from each other. A

series of images of the chessboard were acquired at various angles for a more accurate calibration. Our

script acquired the corners on these chessboard images as the image points, and generated a set of

corresponding 3d object points. Using these, the intrinsic parameters of each camera, such as the camera

matrix and the distortion coefficients were found. Afterwards, the stereo calibration was performed,

which calculated the translation and rotation vector that mapped the right camera image onto the left

camera image for the given baseline. Using rectification, the projection matrix was generated, which

allows for easy triangulation. Once this process was performed once for the given camera setup, the robot

hand.

A chessboard of the same size was then mounted on the robot; after each translational motion of the

robot, new projection points were acquired. With these points as well as the projection matrices initially

calculated, the world coordinates of the chessboard corners located on the robot hand are triangulated.

There was much trial and error, as there are two existing libraries for Python, some functions required

being available in only one of these two. Unfortunately, each uses different matrix representations; The

initial Python cv module employs matrices as type Mat, while the newer cv2 module uses matrices


26

belonging to a separate widely used module for numerical computations called NumPy (shorthand for

Numerical Python). As such, the team was required to carefully convert between the two matrix formats,

to carefully use either single or double precision floating point matrices as each function required, and to

reshape the size of some of the used matrices to fit the format of the required input/output of each

function. Furthermore, the documentation was spread out over multiple sources and thus inconsistent at

times, which slowed down our progress. Help was provided by Andrew Phan, as well as camera vision

enthusiasts found on various message boards. For a look at the formulas used by OpenCV, see Appendix

A. For examples from our code, see Appendix B.

Late in the project we decided to move our code onto the Ubuntu Linux platform. Unfortunately, after a

very lengthy installation process, as well as multiple issues of incompatibility issues with the camera

capture code from Python, we decided to revert back to our Windows based version of our code. Due to

this fact, we were not able to incorporate the results from our triangulation script inside the Gantry Robot

Matlab Interface found on the laboratory computer.

As for the results of our triangulations, we have found that that using chessboard with insufficiently sized

squares produced inconsistent results at best, as the relative error of the chessboard corners was too large

relative to the square size. This also occurs when the target is too far from the cameras. While the size of

the relative error circles of the found corners is around 10% of the square size in Figure 18 at a distance of

~0.8m, the relative error approaches 30% of the square size in Figure 19, at a distance of 2 m.

Camera overexposure, glare and improper lightning conditions also sometimes created blurry images and

lead to false negatives in the detection of chessboard corners.

Through measurement we have found that at a distance of 1m away, the camera can visualize close to an

80 cm wide image. Assuming the maximum resolution of 1280 horizontally, this translates to 0.625 mm

per pixel (if no sub pixel interpolation is to be applied). As such, we expect position measurement

accuracy readings of at least 0.625 mm. It should be note that upon finding the corners of the chessboard

patterns, an iterative sub-pixel interpolation algorithm was used to find a more accurate estimate of the

corner.


27

Figure 18: Relative error at 0.8 m Figure 19: Relative error at 2 m

During the triangulation process, the same point was selected, measured and recorded from the

chessboard, as the chessboard was moved across three different distances in order to determine the

accuracy. These findings can be found in the table from Table 3 below.

Measurements in x, y, z at 0.6

m away

Measurements in x, y, z at 1.5

m away

Measurements in x, y, z at 2 m

away

-9.4300 3.5952 62.3368 -9.4488 3.4477 153.3502 -9.7681 3.5231 153.5684

-9.4035 3.5893 62.2874 -9.5537 3.4902 153.3564 -9.3920 3.6435 153.2285

-9.4007 3.5987 62.321 -9.5692 3.4361 153.3194 -9.4717 3.7168 153.5511

-9.3775 3.5839 62.3497 -9.5194 3.417 153.3411 -9.7570 3.376 153.4564

-9.4337 3.511 62.3532 -9.5500 3.4838 153.337 -9.6787 3.5847 153.2407

-9.3940 3.5586 62.3297 -9.5399 3.5343 153.2913 -9.6814 3.6811 153.5193

-9.3851 3.6006 62.2917 -9.4949 3.4581 153.3453 -9.6253 3.4791 153.3704

-9.4670 3.5884 62.2927 -9.4639 3.5304 153.2876 -9.8301 3.5977 153.2114

-9.4300 3.5952 62.3368 -9.4488 3.4477 153.3502 -9.7681 3.5231 153.5684

Table 3: Measurements table (in cm)


28

From these values, the error (in millimetres) from the average was calculated for the x, y and z axis at all

three distances. The results can be seen in the table from Table 4 upon inspection, it is to be noted that the

accuracy is around 0.25 mm at 0.6 m, increases to around 0.33 at 1.5 m away. Unfortunately, the error

increases to over 1 mm at 2 m away.

Error At 0.6 m away At 1.5 m away At 2 m

away

X axis 0.0241 0.0362 0.1157

Y axis 0.027559 0.0349 0.0968

Z axis 0.0192 0.0318 0.1305

Table 4: Accuracy table (in cm)

Defined Path Used for Testing

All the testing prior to this point was done with the robot being moved on one axis at a time. At this point,

when the calculation and triangulation was sufficiently robust, a defined path was implemented for the

robot, so to test our method on real life motions.

Initially, a square path was implemented, with the help of RAPL-II program. The points were predefined

so that the robot can recall them from the memory when the program is run.

The setup of the checkerboard on the gantry robot was also an important aspect to be considered. If the

checkerboard was placed too high, the yaw and roll were to be ignored and if it was placed too high, it

might go out of the field of view of the cameras, for the bottom coordinates. Figure 20 and Figure 21

below shows the setup that was undertaken for the tests:


29

Figure 20: The Robot (before setup) Figure 21: The Robot (after Setup)

Once the set up was confirmed and the pair of camera was set up as mentioned before. The robot arm was

programmed to initiate a path as shown below within its coordinate axis, changing one variable at a time.

Figure 22: Distance between the two cameras is 24 centimeters


30

Figure 23: The square motion of the robot

Below you can find the triangulation readings at position A, B, C and D, with each motion of 10 cm along

the X or the Y axis. As you can see, the accuracy values match the values during the testing phase.

POSITION A

X Error X Y Error Y Z Error Z -9.4066

0.0194 13.6179 0.038213 62.2897 0.0258

-9.4285

0.0025 13.6205 0.040812 62.3442 0.0287

-9.4518

0.0259 13.583 0.003312 62.2993 0.0162

-9.4136

0.0123 13.5781 0.001588 62.2884 0.0271

-9.4551

0.0291 13.5212 0.058488 62.3357 0.0202

-9.4262

0.0002 13.6083 0.028612 62.3316 0.0161

-9.3733

0.0526 13.5485 0.031187 62.321 0.0055

-9.4525

0.0266 13.56 0.019687 62.3138 0.0017

Table 5 - Readings at position A

The error in X,Y, Z at position A was found to be 0.0211 cm, 0.0277 cm and 0.0177 cm.


31

POSITION B

X Error X Y Error Y Z Error Z -

9.4729 0.0370 3.6043 0.031125 62.3089 0.0145

-9.4270

0.0089 3.6116 0.038425 62.3149 0.0085

-9.4666

0.0307 3.6182 0.045025 62.3229 0.0005

-9.4322

0.0037 3.5721 0.001075 62.3506 0.0272

-9.4195

0.0164 3.5727 0.000475 62.2889 0.0345

-9.4000

0.0359 3.5467 0.026475 62.3396 0.0162

-9.4352

0.0007 3.5435 0.029675 62.3192 0.0042

-9.4335

0.0024 3.5163 0.056875 62.3425 0.0191

The error in X,Y, Z at position B was found to be 0.0169 cm, 0.0286 cm and 0.0156 cm.

POSITION C

X Error X Y Error Y Z Error Z 0.5730 0.0024 3.6096 0.045062 62.3498 0.0208 0.5790 0.0084 3.581 0.016462 62.3352 0.0062 0.5920 0.0214 3.5619 0.002638 62.3603 0.0313 0.5263 0.0443 3.517 0.047538 62.2846 0.0444 0.6080 0.0374 3.5796 0.015063 62.3044 0.0246 0.6063 0.0357 3.5525 0.012037 62.3314 0.0024 0.5382 0.0324 3.5725 0.007962 62.3579 0.0289 0.5417 0.0289 3.5422 0.022338 62.3088 0.0202

Avg Avg Error

Avg Avg Error Avg Avg Error

0.5706 0.0264 3.564538 0.021138 62.3291 0.0223

The error in X,Y, Z at position C was found to be 0.0264 cm, 0.0211 cm and 0.0223 cm.


32

POSITION D

X Error X Y Error Y Z Error Z 0.5312 0.0436 13.6115 0.047112 62.3111 0.0220 0.5795 0.0047 13.5929 0.028512 62.2889 0.0442 0.6113 0.0365 13.5202 0.044188 62.3523 0.0192 0.6197 0.0449 13.5957 0.031312 62.3452 0.0121 0.5916 0.0168 13.555 0.009388 62.3436 0.0105 0.5215 0.0533 13.5317 0.032688 62.3275 0.0056 0.5227 0.0521 13.5565 0.007888 62.363 0.0299 0.6207 0.0459 13.5516 0.012788 62.3332 0.0001

Avg Avg Error

Avg Avg Error Avg Avg Error

0.5748 0.0372 13.56439 0.026734 62.3331 0.0180

The error in X,Y, Z at position D was found to be 0.0372 cm, 0.0267 cm and 0.0180 cm.

As Figure 23 shows, the robot starts on position A and then moves to position B. In position B, the robot

pauses for a while, so as to allowing time for the cameras to take down its position. Once done, the driver

can manually precede the robot, to the next positions respectively. At each position, the robot requires a

brief pause, so that it allows enough time for the cameras to take a snap of the checkerboard attached to

the machine. The speed of the robot was set to be 20 percent of its full speed and the robot changed one

variable at a time (e.g. x, y, z).

Once the square position was successfully implemented, the robot was programmed for a more complex

motion. The cube was considered to be a more complicated motion than the square as, it includes motion

in all three axis throughout the motion unlike the square.


33

Figure 24: Sample of a cube motion implementation

Impact on Society and Technology

The Gantry robot is a large scale industrial robot. As such, its construction requires decent amounts of

metal and wiring. It requires a special controller that requires a few minutes to power up and adequate

cooling in order to prevent overheat, as the robot is sensitive to temperature variations. This entails non-

trivial costs in terms of energy. Little additional energy is required from the cameras or the attached LED.

The use of the robot is safe. However, special precautions must be taken during and after use. New users

will require training in order to minimize safety risks. As the robot has enough torque to harm a person at

high movement speeds, all personnel is to keep a minimum distance away from the robot during

operation. Furthermore, to avoid any accidents or to prevent additional physical or electrical damage to

the robot, at least one user must have the stop button in reach at all times. This is one of the main safety

features of the robot. Such a case can occur during incorrect or absent calibration, when the robot does not

move as expected. Furthermore, the power bar those powers up the robot as well as the controller must be

kept off after use of the robot. Other than the lubrication employed in the robot articulations, the metal the

metal and the rubber used, there is no other waste. If sufficient renewable energy is available, the machine

can function properly using renewable energy as well.

Validating and improving the MATLAB interface, as well as understanding the implementation of the

RAPLII functions used to perform the necessary motions will allow to optimize the desired operations,

add any possible fail safes, as well as render the system more accessible and user friendly, which could

save countless hours spent on unnecessary tasks.

No additional safety risks are introduced by attacking the LED light to the robotic arm, nor by mounting

the cameras at the designated locations. Successful validation of the accuracy of the motions will allow

the robot to be used on the field, which could potentially open up new job opportunities. Some


34

applications include 3d data acquisition, assembly of various industrial parts using complex motions,

spray painting, manipulation of dangerous substances, among other, depending of the required accuracy

of the task.

Conclusion

Through research and practical experience on the task at hand, the team has learned about using the

Gantry robot and its specific programming language, about artificial perception techniques and devices. A

partial protocol capable of measuring the accuracy of simple spatial movements of the robot was

developed; this protocol includes the commands outputted in order to move the robot during

measurements, and the measuring tools and algorithms.

During the second part of the project, this protocol was be fine-tuned, tested and improved as required,

and more research was performed as new issues arose. Elaborate accuracy measurement software tools

were developed to perform image capture and find the triangulated world positions. These tools allowed

us to determine the accuracy of the position in world coordinates. The team was able to implement a

calibration code that was able to provide consistent results and also detect the gantry robot`s path and

coordinates within its boundaries after it moves. There is inconsistent behaviour near the edges of the

camera field of view. (results were found consistent for motions no longer than 10 cm). The team was not

able to fix it even though a plan was devised with the help of Professor Ferrie to fix this particular

problem, which can be found in Appendix 4.

If time would permit, we would mount a laser pointer instead of a chessboard pattern on the robot arm,

potentially allowing for more accurate position readings. Using the chessboard, another option would be

to also perform position readings across more complex, circular paths, and we would include code that

would calculate the pose of the robot arm as well as the position. Lastly, we would integrate our code

with the existing Matlab interface and have the code calculate the difference between the triangulated

position and the position taken as input before the motion is performed, as well as further enhance the

Matlab interface, as was initially planned.


35

References

1. http://www.wisegeek.com/what-are-gantry-robots.htm- Accessed on November 27, 2012

2. Dressler, Isolde. "Modeling and Control of Stiff Robots for Flexible Manufacturing.".

Department of Automatic Control Lund University, n.d. Web. 3 Dec 2012.

<http://www.control.lth.se/documents/2012/dressler2012phd.pdf>.

3. Smook, 0 .E.F.. "Camera Position Calibration Using the Image of Known Object Edges." . M.Sc.

Thesis, Measurement and Control Section ER, Department of Electrical Engineering, Eindhoven

University of Technology, 22 1994. Web. 3 Dec 2012.

<http://hanquier.m.free.fr/Worcester/references/Others papers/calibration/Camera Position

Calibration.pdf>.

4. http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/parameters.html- Accessed on

November 27, 2012

5. http://www.umiacs.umd.edu/~ramani/cmsc828d/lecture9.pdf - Accessed on November 25, 2012

6. http://blog.martinperis.com/2011/08/stereo-vision-why-camera-calibration-is.html- Accessed on

November 25, 2012

7. http://www.vision.deis.unibo.it/smatt/Seminars/StereoVision.pdf - Accessed on November 25

8. http://www.wrotniak.net/photo/infrared/#DIFF- Accessed on December 4, 2012

9. http://www.vision.caltech.edu/bouguetj/calib_doc/ - Accessed on November 28, 2012

10. http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example5.html - Accessed on November

28, 2012

11. Emanuele Trucco and Alessandro Verri (1998). Introductory Techniques for 3-D Computer Vision.

Prentice Hall. ISBN 0-13-261108-2.

12. H. Christopher Longuet-Higgins (September 1981). "A computer algorithm for reconstructing a

scene from two projections". Nature 293 (5828): 133–135. doi:10.1038/293133a0.


36

Appendix A cv_calibrate_single

import numpy as np import cv2 import cv2.cv as cv import os from common import splitfn import yaml import sys, getopt from glob import glob resolution = (0,0) pattern_size = (0,0) square_size = 1.0 #calibrate.py myimage C:\opencv\samples\python2\data image #http://stackoverflow.com/questions/10022568/opencv-2-3-camera-calibration #calling python from matlab!!! http://stackoverflow.com/questions/1707780/call-python-function-from-matlab def get_formatting(): global resolution, pattern_size, square_size f = open('C:/opencv/samples/python2/formatting.yaml') data = f.readlines() f.close() data = data[0].split(',') resolution = (int(data[0]), int(data[1])) pattern_size = (int(data[2]), int(data[3])) square_size = float(data[4]) print '\nResolution used is %dx%d'%(resolution[0],resolution[1]) print '\nPattern size used is %dx%d\n'%(pattern_size[0],pattern_size[1]) print '\nSquare size used is %d\n'%(square_size) def calibrate(): print #Selecting left or right camera to perform calibration (depending on command line arguments) if len(sys.argv) > 1: if sys.argv[1] == 'left' or sys.argv[1] == 'right': camera_type = sys.argv[1] print 'Finding intrinsic parameters for %s camera:\n' %(sys.argv[1]) else:


37

print 'FAILURE! "%s" IS NOT A VALID CAMERA TYPE.\nPlease enter right or left as command line argument!!!' %(sys.argv[1]) return else: camera_type = 'left' print 'Finding intrinsic parameters for %s camera (by default):\n' %(camera_type) if pattern_size == (0,0): print 'FAILURE: NEED VALID PATTERN SIZE!' return img_mask = './data/image_%s*.jpg' %(camera_type) img_names = glob(img_mask) #loads all images with this file name mask #need dummy camera matrix and distorsion coefficient matrix (=the two sets of intrsinsic parameters) matrix_size = (3,3) camera_matrix = np.zeros( (np.prod(matrix_size), 3), np.float32 ) dist_coefs = (0, 0, 0, 0) #finding information for object points - Number of inner corners: picture has 4,4; picture 2 has 9x9; andre's pattern has 8x6 pattern_points = np.zeros( (np.prod(pattern_size), 3), np.float32 ) #returns array of zeroes pattern_points[:,:2] = np.indices(pattern_size).T.reshape(-1, 2) #returns array with indeces of a grid + reshape (gives array a new shape without changing data) pattern_points *= square_size #scalar multiplication (since all the calculations before were assuming square size of 1 pixel) - T/R depend on res obj_points = [] img_points = [] height, width = 0, 0 if not img_names: print 'FAILURE. THERE WERE NO IMAGES FOUND FOR CALIBRATION!' return for fn in img_names: print 'processing %s...' % fn, img = cv2.imread(fn, 0) height, width = img.shape[:2] #resolution found, corners = cv2.findChessboardCorners(img, pattern_size) if found: term = ( cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1 ) cv2.cornerSubPix(img, corners, (5, 5), (-1, -1), term) vis = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) cv2.drawChessboardCorners(vis, pattern_size, corners, found) path, name, ext = splitfn(fn)


38

cv2.imwrite('./debug/%s_chess.jpg'%(name), vis) if not found: print 'chessboard not found' continue img_points.append(corners.reshape(-1, 2)) obj_points.append(pattern_points) print 'ok' #calculate camera matrix and dist coeff rms, camera_matrix, dist_coefs, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, (width, height), camera_matrix, dist_coefs) print "\nRMS:", rms #final reprojection error print "camera matrix:\n", camera_matrix #you need only 00, 02, 11, 12 print "distortion coefficients:\n ", dist_coefs.ravel() #ravel flattens matrix cv2.destroyAllWindows() #calculate and display some important parameters fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues(camera_matrix, resolution, 1, 1) print "\nField of view over X:", fovx print "Field of view over Y:", fovy print "Focal length", focalLength print "Principal point:", principalPoint print "Aspect ratio:", aspectRatio, "\n" #writing intrinsic parameters to yaml file (basically text file) #first 4 values are fx, cx, fy, cy; next 5 values are the distorsion coefficients cv.Save('C:/opencv/samples/python2/intrinsic_camera_%s.yaml'%(camera_type), cv.fromarray(camera_matrix)) cv.Save('C:/opencv/samples/python2/intrinsic_distorsion_%s.yaml'%(camera_type), cv.fromarray(dist_coefs)) return def main(): get_formatting() calibrate() return if __name__ == "__main__": main()


39

cv_calibrate_stereo

import numpy as np import cv2 import cv2.cv as cv import os from common import splitfn import yaml import sys, getopt from glob import glob import csv resolution = (0,0) pattern_size = (0,0) square_size = 1.0 def get_formatting(): global resolution, pattern_size, square_size f = open('C:/opencv/samples/python2/formatting.yaml') data = f.readlines() f.close() data = data[0].split(',') resolution = (int(data[0]), int(data[1])) pattern_size = (int(data[2]), int(data[3])) square_size = float(data[4]) print '\nResolution used is %dx%d'%(resolution[0],resolution[1]) print '\nPattern size used is %dx%d\n'%(pattern_size[0],pattern_size[1]) print '\nSquare size used is %f\n'%(square_size) def get_intrinsic(camera_type): #loading intrinsic parameters from file #first 4 values are fx, cx, fy, cy - next 5 values are the distorsion coefficients if camera_type == 'left' or camera_type == 'right': print 'Acquiring intrinsic parameters for %s camera from file.' %(camera_type) else: print 'FAILURE: Camera type cannot be %s' %(camera_type) return try: camera_matrix = np.asarray(cv.Load('C:/opencv/samples/python2/intrinsic_camera_%s.yaml'%(camera_type), cv.CreateMemStorage()))


40

dist_coefs = np.asarray(cv.Load('C:/opencv/samples/python2/intrinsic_distorsion_%s.yaml'%(camera_type), cv.CreateMemStorage())) return camera_matrix, dist_coefs except IOError: print "EXCEPTION: File was not found or could not be read." return def find_chessboard(camera_type, img_points, obj_points): if camera_type == 'left' or camera_type == 'right': print 'Acquiring image points and object points for the %s camera.' %(camera_type) else: print 'FAILURE: Camera type cannot be %s' %(camera_type) return #print pattern_size if pattern_size == (0,0): print 'FAILURE: NEED VALID PATTERN SIZE!' return img_mask = './data/image_%s*.jpg' %(camera_type) img_names = glob(img_mask) #loads all images with this file name mask #2.7 #1.0 #need dummy camera matrix and distorsion coefficient matrix (=the two sets of intrsinsic parameters) matrix_size = (3,3) camera_matrix = np.zeros( (np.prod(matrix_size), 3), np.float32 ) dist_coefs = (0, 0, 0, 0) #finding information for object points - Number of inner corners: picture has 4,4; picture 2 has 9x9; andre's pattern has 8x6 pattern_points = np.zeros( (np.prod(pattern_size), 3), np.float32 ) #returns array of zeroes pattern_points[:,:2] = np.indices(pattern_size).T.reshape(-1, 2) #returns array with indeces of a grid + reshape (gives array a new shape without changing data) pattern_points *= square_size #scalar multiplication (since all the calculations before were assuming square size of 1 pixel) - T/R depend on res height, width = 0, 0 if not img_names: print 'FAILURE. THERE WERE NO IMAGES FOUND FOR CALIBRATION!' return for fn in img_names: #print 'processing %s...' % fn, img = cv2.imread(fn, 0) height, width = img.shape[:2] #resolution


41

found, corners = cv2.findChessboardCorners(img, pattern_size) if found: term = ( cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1 ) cv2.cornerSubPix(img, corners, (5, 5), (-1, -1), term) if not found: print 'chessboard not found' continue img_points.append(corners.reshape(-1, 2)) obj_points.append(pattern_points) #print 'ok' return def calibrate_stereo(): #if this doesn't work, use CreateMat(rows, cols, type) -> mat to recreate matrix !!! type is F for float #http://opencv.willowgarage.com/documentation/python/calib3d_camera_calibration_and_3d_reconstruction.html print img_points_left = [] img_points_right = [] obj_points_left = [] obj_points_right = [] #same as left #reading intrinsic parameters from file camera_matrix_left, dist_coefs_left = get_intrinsic('left') camera_matrix_right, dist_coefs_right = get_intrinsic('right') print camera_matrix_left, '\n\n', camera_matrix_right, '\n\n', dist_coefs_left, '\n\n', dist_coefs_right, '\n\n' #obtaining object points and image points find_chessboard('left', img_points_left, obj_points_left) find_chessboard('right', img_points_right, obj_points_right) #stereo calibration (finding translation and rotation vector of right camera onto the left one rms, cm1, dc1, cm2, dc2, rotation, translation, E, fundM = cv2.stereoCalibrate(obj_points_left, img_points_left, img_points_right, resolution, \ camera_matrix_left, dist_coefs_left, camera_matrix_right, dist_coefs_right, flags = 1) print print 'Translation matrix of right camera onto left camera is:\n', translation print 'Rotation matrix of right camera onto left camera is:\n', rotation


42

#saving translation and rotation vector to file cv.Save('C:/opencv/samples/python2/extrinsic_translation.yaml', cv.fromarray(translation)) cv.Save('C:/opencv/samples/python2/extrinsic_rotation.yaml', cv.fromarray(rotation)) R1 = cv.CreateMat(3, 3, cv.CV_64FC1) #must be double!!!! R2 = cv.CreateMat(3, 3, cv.CV_64FC1) P1 = cv.CreateMat(3, 4, cv.CV_64FC1) P2 = cv.CreateMat(3, 4, cv.CV_64FC1) Q = cv.CreateMat(4, 4, cv.CV_64FC1) print '\nPerforming the stereo rectification' cv.StereoRectify(cv.fromarray(camera_matrix_left), cv.fromarray(camera_matrix_right), cv.fromarray(dist_coefs_left), cv.fromarray(dist_coefs_right), \ resolution, cv.fromarray(rotation), cv.fromarray(translation), R1, R2, P1, P2, Q, alpha=-1, newImageSize=(0, 0)) P1 = np.asarray(P1) P2 = np.asarray(P2) print print '\nProjection matrix for left image:\n', P1 print 'Projection matrix for right image:\n', P2 #USE THIS IF YOU DECIDE TO CREATE SEPARATE TRIANGULATION THAT YOU LOOP THROUGH - saving projection matrices to file cv.Save('C:/opencv/samples/python2/proj_matrix_left.yaml', cv.fromarray(P1)) cv.Save('C:/opencv/samples/python2/proj_matrix_right.yaml', cv.fromarray(P2)) cv.Save('C:/opencv/samples/python2/fund_matrix.yaml', cv.fromarray(fundM)) def main(): get_formatting() #gets resolution and pattern size from file print resolution print pattern_size calibrate_stereo() return if __name__ == "__main__": main()

cv_calibrate_stereo import numpy as np


43

import cv2 import cv2.cv as cv import os from common import splitfn import yaml import sys, getopt from glob import glob import csv import winsound #http://tech.groups.yahoo.com/group/OpenCV/message/81443 #https://code.ros.org/trac/opencv/ticket/1319 #http://stackoverflow.com/questions/1707780/call-python-function-from-matlab resolution = (0,0) pattern_size = (0,0) square_size = 1.0 def get_formatting(): global resolution, pattern_size, square_size f = open('C:/opencv/samples/python2/formatting.yaml') data = f.readlines() f.close() data = data[0].split(',') resolution = (int(data[0]), int(data[1])) pattern_size = (int(data[2]), int(data[3])) square_size = float(data[4]) print data[4] print float(data[4]) print square_size print '\nResolution used is %dx%d'%(resolution[0],resolution[1]) print '\nPattern size used is %dx%d\n'%(pattern_size[0],pattern_size[1]) print '\nSquare size used is %f\n'%(square_size) def get_intrinsic(camera_type): #loading intrinsic parameters from file #first 4 values are fx, cx, fy, cy - next 5 values are the distorsion coefficients if camera_type == 'left' or camera_type == 'right': print 'Acquiring intrinsic parameters for %s camera from file.' %(camera_type) else: print 'FAILURE: Camera type cannot be %s' %(camera_type) return


44

try: camera_matrix = np.asarray(cv.Load('C:/opencv/samples/python2/intrinsic_camera_%s.yaml'%(camera_type), cv.CreateMemStorage())) dist_coefs = np.asarray(cv.Load('C:/opencv/samples/python2/intrinsic_distorsion_%s.yaml'%(camera_type), cv.CreateMemStorage())) return camera_matrix, dist_coefs except IOError: print "EXCEPTION: File was not found or could not be read." return def take_projection_point_picture(camera_type): index = 0 if camera_type == 'left': camera = cv.CaptureFromCAM(0) #left cam ---FIND CORRECT VALUES elif camera_type == 'right': camera = cv.CaptureFromCAM(1) #right cam else: print 'FAILURE: Invalid camera type!' return cv.SetCaptureProperty(camera, cv.CV_CAP_PROP_FRAME_WIDTH, resolution[0]) cv.SetCaptureProperty(camera, cv.CV_CAP_PROP_FRAME_HEIGHT, resolution[1]) print '\nAcquiring images at %sx%s resolution!\n' %(resolution[0], resolution[1]) print 'WHEN YOU HEAR THE BEEP, MUST STAND STILL\n' image = cv.QueryFrame(camera) cv.SaveImage(".\data\proj_point_image_%s.jpg"%(camera_type), image) print '\nSaved %s projection point image into the .\data folder!'%(camera_type) def find_proj_points(camera_type, img_points): if camera_type == 'left' or camera_type == 'right': print '\nAcquiring projection points for the %s camera.' %(camera_type) else: print 'FAILURE: Camera type cannot be %s' %(camera_type) return #reading saved projection point image from file img_mask = './data/proj_point_image_%s*.jpg' %(camera_type) img_names = glob(img_mask) #loads all images with this file name mask


45

#need dummy camera matrix and distorsion coefficient matrix (=the two sets of intrsinsic parameters) matrix_size = (3,3) camera_matrix = np.zeros( (np.prod(matrix_size), 3), np.float32 ) dist_coefs = (0, 0, 0, 0) #finding information for object points - Number of inner corners: picture has 4,4; picture 2 has 9x9; andre's pattern has 8x6 height, width = 0, 0 if not img_names: print 'FAILURE. THERE WERE NO IMAGES FOUND FOR CALIBRATION!' sys.exit() #finding projection points (=chessboard points) from image fn = img_names[0] #print 'processing %s...' % fn, img = cv2.imread(fn, 0) height, width = img.shape[:2] #resolution found, corners = cv2.findChessboardCorners(img, pattern_size) img_index = 0 #drawing chessboard points to image and saving image if found: term = ( cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1 ) cv2.cornerSubPix(img, corners, (5, 5), (-1, -1), term) img_mask = './debug/proj_point_%s_chess*.jpg'%(camera_type) img_names = glob(img_mask) #loads all images with this file name mask img_index = len(img_names) vis = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) cv2.drawChessboardCorners(vis, pattern_size, corners, found) #path, name, ext = splitfn(fn) cv2.imwrite('./debug/proj_point_%s_chess_%d.jpg'%(camera_type, img_index), vis) if not found: print 'Chessboard not found. One of the camera might have its view blocked.' sys.exit() img_points.append(corners.reshape(-1, 2)) return img_points def triangulate_all_points(proj_matrix_1, proj_matrix_2, fundM): proj_points_left = [] proj_points_right = [] print '\a', '\a', '\a' # 3 beeping noises STAND STILL


46

#taking pictures for projection points take_projection_point_picture('left') take_projection_point_picture('right') #acquiring projection points find_proj_points('left', proj_points_left) find_proj_points('right', proj_points_right) proj_points_left = np.asarray(proj_points_left) #take only 1 or many projection points??? proj_points_right = np.asarray(proj_points_right) #refines coordinates of corresponding points print '\nFirst point of left camera projection points BEFORE correct_matches:\n', proj_points_left[0][0], '\n' proj_points_left, proj_points_right= cv2.correctMatches(fundM, proj_points_left, proj_points_right) print 'First point of left camera projection points AFTER correct_matches:\n', proj_points_left[0][0], '\n' #triangulates 3d position points4D = cv2.triangulatePoints(proj_matrix_1, proj_matrix_2, proj_points_left, proj_points_right) #do proj points need to be homogenous ??? ''' #using convertToNonHom() points4D = points4D.reshape(np.prod(pattern_size), 4) points4D = cv2.convertPointsFromHomogeneous(points4D) points4D = np.asarray(points4D) first_point = ( float(points4D[0][0][0]), float(points4D[0][0][1]), float(points4D[0][0][2]) ) ''' #turning matrix into non-homogenous (euclidean) points4D /= points4D[3] first_point = (points4D[0][0], points4D[1][0], points4D[2][0]) print '\nNew point is:',first_point, '\n' #saving point to csv with open("output.csv", "ab") as f: fileWriter = csv.writer(f, delimiter=',',quotechar='|', quoting=csv.QUOTE_MINIMAL) fileWriter.writerow(first_point) return first_point def main(): '''Function that triangulates 3d world coordinate of the first point of a chessboard from intrinsic and extrinsic parameters, saves the point to a csv file, and performs the difference between the old point and the new point '''


47

#read current resolution, pattern size and square size get_formatting() #gets resolution and pattern size from file #load projection and fundamental matrices from file (needed for triangulation) P1 = np.asarray(cv.Load('C:/opencv/samples/python2/proj_matrix_left.yaml', cv.CreateMemStorage())) P2 = np.asarray(cv.Load('C:/opencv/samples/python2/proj_matrix_right.yaml', cv.CreateMemStorage())) fundM = np.asarray(cv.Load('C:/opencv/samples/python2/fund_matrix.yaml', cv.CreateMemStorage())) #clear the .csv file if there is 'cl' command line argument argc = len(sys.argv) if argc == 2: if sys.argv[1] == 'cl': f = open('output.csv', "w") f.truncate() f.close() print '\nCLEARED output.csv FILE\n' new_point, old_point = (0, 0, 0), (0, 0, 0) difference = (0, 0, 0) length = 0 #triangulate new_point = triangulate_all_points(P1, P2, fundM) file = os.getcwd() file = os.path.join(file, "output.csv") #getting # of rows of .csv file with open(file, 'rb') as csvfile: spamreader = csv.reader(csvfile, delimiter=',', quotechar='|') index = 0 for row in spamreader: length = length + 1 #Read first point from csv file and perform difference between new point and previous point with open(file, 'rb') as csvfile: spamreader = csv.reader(csvfile, delimiter=',', quotechar='|') index = 0


48

for row in spamreader: if index == length - 2: first_point = () for item in row: #print item first_point = first_point + (float(item),) print '\nprevious point is:',first_point, '\n' difference = tuple(x - y for x,y in zip(new_point, first_point)) print '\n2. Difference between NEW point and PREVIOUS point is: ', difference index+=1 winsound.Beep(800,600) #beep to confirm end of triangulation and return of value return difference if __name__ == "__main__": main()

Appendix B

Formulas used in OpenCV image calibration:

The following Formulas are taken from Trucco and Verri`s Introductory Techniques for 3d Computer

Vision. They will briefly illustrate the process through which OpenCV performs calibration and

triangulation.

Calibration:

Single camera calibration is performed (finding values of focal length, distortion coefficients, rotation,

translation, all found in Figure 2) , followed by stereo camera calibration(finding values of translation and

rotation vectors of both cameras respective to each other, as seen in Figure 20 and Figure 21) .

Figure 25: Intrinsic parameters


49

The extrinsic parameters consist of the rotation and translation matrix between the left and the right

camera images: they are used to allow the two images to exist into the same image plane. The equation

below in Figure 22, taken from an article by Christopher Longuet – Higgins called ‘A computer algorithm

for reconstructing a scene from two projections’ illustrates this.

Figure 26: Extrinsic parameters

The images are correctly mapped onto the same image plane, and the matrix transformations that allow a

correct mapping for each camera image are called projection matrices. Afterwards, using the projection

points/image points ui, vi, wi and the projection matrix M, through matrix operations triangulation can be

easily performed to find the world coordinates Xw, Y

w and Z

w. This can be seen in figure 23, also taken

from the book by Trucco and Verri.

Figure 27: Triangulation from projection matrix


50

Appendix C:

1. Gantry robot:

2. Checkerboard set up on the robot:


51

3. Camera Setup being used:

4. Robot Arm on which the Checkerboard calibration will also work:

5. Phidget System Used during the testing phases:


52

6. Manual Controller (pendent) and the ARM power of the Gantry Robot:

Appendix D:

Sanity checks

Assumptions:

Two calibrated parallel calibrated cameras mounted in parallel. The baseline is equal to T (distance

between the cameras).

Image points of left camera (Xl,Yl) and those of the right camera (Xr,Yr) have been found. Note: these

included in the second return values of findChessboardCorners (first return values = 0 or 1 depending if

points were found, while second return value is a matrix of all the found image points).

Sx = pixel size on x.

Sanity check 1: Yl should be very close to Yr (within a pixel or two).

Sanity check 2: For each pair of corresponding points, compute disparity di = Xr-Xl (where Xr and X1are

the real distances of the camera frame . This set of values would then provide us the Z components which

can be calculated by Zi = f(T/di), where T is the baseline and f is the focal length the team obtained from

the calibration method.

Note: Inside Opencv, since the f found by cv_calibrate_single is in fact fx = f/sx and not f (see here), and

the Xr and Xl used by our functions are in fact Xr*sx and Xl*sx, this essentially results in:

:

This shows that this function is still viable as a sanity check for the particular

functions used by opencv calibration!

http://opencv.willowgarage.com/documentation/python/calib3d_camera_calibration_and_3d_reconstruction.html

ecse 475 final report intakhab radu tabassum

Documents

4th year bachelor

1d search domain

2d search domain

distorsion coefficient matrix

term criteria eps cv2

extrinsic parameters consist

loading intrinsic parameters

cv import os