Seediscussions,stats,andauthorprofilesforthispublicationat:https://www.researchgate.net/publication/241161820

CameracalibrationbasedonExtendedKalmanFilterusingrobot'sarmmotion

Article·July2009

DOI:10.1109/AIM.2009.5229790

CITATIONS

6

READS

87

6authors,including:

Someoftheauthorsofthispublicationarealsoworkingontheserelatedprojects:

LinkingRoboticstoOrthodonticsViewproject

MEANING-CENTRICFRAMEWORKFORNATURALTEXT/SCENEUNDERSTANDINGBYROBOTSViewproject

ZeyangXia

ChineseAcademyofSciences

59PUBLICATIONS174CITATIONS

SEEPROFILE

MingXie

NanyangTechnologicalUniversity

123PUBLICATIONS950CITATIONS

SEEPROFILE

JunhongJi

HarbinInstituteofTechnology

12PUBLICATIONS90CITATIONS

SEEPROFILE

AllcontentfollowingthispagewasuploadedbyZeyangXiaon28May2014.

Theuserhasrequestedenhancementofthedownloadedfile.

Abstract— This paper presents a camera calibration

method that could be used in humanoid robot vision system.

Extended Kalman Filter is used to estimate camera

parameters. This method can take planer board feature

points of the image or the object’s 3D axis as inputs when

doing known motion, test results show this algorithm can get

good results. If the camera parameters are known, the

relationship between a reference coordinate system and

camera coordinate system can not get in high precision, in

this paper, a method based on the arm motion is discussed.

I. INTRODUCTION

ision system has been widely used in navigation and

3D computer vision of vehicles and humanoid robots.

In those fields, camera plays the role of getting information

from the out world and then provides information that

robot needed, such as the distance between the robot and

the object. The position of the object in image plane is

related to the object’s position in space, which is

determined by the camera model, the parameters of the

model is the camera parameters which can be got by

compute and experiment, this process named calibration.

Camera calibration plays a key role in most vision systems

in extracting metric information from 2D images.

In recent years, there are numerous papers which

propose new or modified techniques on this problem,

including photogrammetric calibration, self calibration and

other techniques. These methods can get good results when

considering specialized conditions. There are many

Manuscript received January 30, 2009. This paper is supported in part

by Program for Changjiang Scholars and Innovative Research Team in

University (PCSIRT)(IRT0423) and in part by China Scholarship

Council Foundation , and is sponsored by “LOCH” project.

Guodong Chen, a PHD candidate in State Key Laboratory of Robotics

and System ,Harbin Institute of Technology, China, now doing research

work in Nanyang Technological University, Singapore, phone:65-

67906591; (e-mail: guodongxyz@gmail.com).

Sun Lining is a professor in State Key Laboratory of Robotics and

System, Harbin Institute of Technology, China. (e-mail:

lnsun@hit.edu.cn)

Xie Ming, associate professor in Nanyang Technological University,

Singapore. (e-mail: mmxie@ntu.edu.sg) .

Zeyang Xia is a research fellow in Nanyang Technological University,

His current research focuses on humanoid robotics and medical robotics.

(email: zeyang.xia@ieee.org)

Junhong Ji is a associate professor in State Key Laboratory of

Robotics and System, Harbin Institute of Technology, China. (e-mail:

junhong.ji@hit.edu.cn)

Zhijiang Du is a professor in State Key Laboratory of Robotics and

System, Harbin Institute of Technology, China. (e-mail:

duzj01@hit.edu.cn)

methods on this problem, such as the earliest papers on

camera calibration based on pinhole model for the camera

[1,2] without considering the lens distortion. A standard-

sized jig is needed when doing traditional calibration,

through object points and their corresponding image pixels

to restrict camera parameters, and optimized algorithm is

used to compute camera parameters [3]. By using this

method, it can get high precision parameters, but it’s not

convenient especially in some dangerous or badly

environment. Tsai’s method based on radial alignment

constraint is widely used in camera calibration [4,5,6],

J.Weng modified Tsai’s method in camera distortion [7].

Traditional calibration method is expensive and not

convenient, in low cost case, Zhang’s method[8,9] is a

good choice, but in order to get the high precision of the

camera, more high quality pictures are needed, such as

Zhang’s method, if pictures are not enough, calibration

result will not accurate. When doing the calibration on

spot, maybe there are difficulties to get high quality

pictures, and also it’s difficult to get high precision

coordinates of the calibration object. The dimensions of

the robot are known, also we can get the posture of the

robot when it moves, in this way, we can take the hand of

the robot as the calibration object, when we calibrate the

cameras on the robot, a serial of motions is needed, in this

way we can get the image pixel coordinate and the world

coordinate of the hand, take the pixel coordinate and the

world coordinate of the hand at every posture as the inputs,

Iterated Extend Kalman filter as the calibration method, so

the parameters of the camera can be got.

Kalman Filter[10] has been widely used in many fields

such as machine vision, in recent years, there are some

papers on camera calibration based on Kalman Filter, such

as Castro[11] in two steps camera calibration. Faraz M

proposed to use Kalman filter for IMU-camera calibration

[12, 13], Zhou [14] proposed to use IEKF doing camera

calibration without considering distortion.

When buying camera, the manufacture will supply the

parameters of the camera, in general, the stereo camera

manufacture will fix the cameras and supply exact camera

parameters. In this way, if we want to know the

relationship between the camera coordinate system and a

reference coordinate system, the precision will determined

by the system precision and measurement precision, so if

can get the relationship through the object’s motion in

reference coordinate, it will get a high precision result only

related to the system’s precision.

In our lab, the parameters of the cameras are not

Camera Calibration based on Extended Kalman Filter using

Robot’s Arm Motion

Guodong Chen, Zeyang Xia, Xie Ming, Sun Lining, Junhong Ji, Zhijiang Du

Harbin Institute of Technology, Nanyang Technological University

V

2009 IEEE/ASME International Conference on Advanced Intelligent MechatronicsSuntec Convention and Exhibition CenterSingapore, July 14-17, 2009

978-1-4244-2853-3/09/$25.00 ©2009 IEEE 1839

supplied by the manufacture, and the cameras are net-

based cameras, so we do the calibration using Extended

Kalman Filter as well as Zhang’s method, and do a

comparison. Test result shows this method can get good

results.

I. INTRODUCTION OF THE ARM-HAND SYSTEM

The degrees of freedom (DOF) of the arm-hand system

are presented in Fig.1. To be more specific, each arm of

the robot has six degrees of freedom including three in

shoulder, one in elbow and two in wrist. This enables the

robot have similar kinematics as human being and do

majority of things in the ordinary life. Meanwhile each

hand is assigned to have 6 independent degrees of freedom

along with four passive ones. Since the limited space of the

hand, the passive DOFs with simpler structure enable the

hands to approach more functions [15].

Fig.1. DOF Distribution on arm-hand system

For movement range of each joint, we designed that to

be about the same as that of standard human so that the

humanoid robot performs human tasks as well as human.

Table I shows the data on movable range of upper arm,

fore arm and hand with comparison with standard human

respectively.

TABLEI

JOINT MOVEMENT RANGE

Joints robot

Pitch -180 deg. to 180 deg.

Roll - 120 deg. to 30 deg.

Shoulder

Yaw -180 deg. to 180 deg.

Elbow Pitch -135 deg. to 0 deg

Pitch - 30 deg. to 30 deg. Wrist

Yaw -180 deg. to 180 deg.

Hand Pitch 0 deg. to 90 deg.

In this paper, take the hand’s motion as the target object,

paint red color on the hand, so it is easy to get some

feature points on the hand, and it is easy to get the image

coordinate of these feature points. Fig.2 shows the whole

vision-arm-hand system. Fig.3 shows the whole humanoid

robot.

Fig.2 Vision-Arm-Hand system

Fig.3 The appearance of the humanoid robot

II. DESCRIPTION OF THE ALGORITHM

A. Non-linear Camera Model

In practice, pin-hole camera model can not describe the

relationship between object and image, especially using

PTZ camera, for drawbacks of manufacture, the camera

can not made perfectly, and there are distortions in image,

it is a non-linear relationship between object and image.

There are several distortion models used widely, such as

Weng’s model [7], Fig.4 shows the model of radial

distortion.

1840

( , )u u uP x y

( , )d d dP x y

x

cx

cy

cO

1O

( , , )w w wP x y z

wx

wy

wz

wO

cz

Fig.4 the model of radial distortion

u d xx x δ= + (1)

u d yy y δ= + (2) 2 2 2 2 2 2

1 1 2 1( ) ( (3 ) 2 ) ( )x d d d d d d d d dk x x y p x y p x y s x yδ = + + + + + +

(3) 2 2 2 2 2 2

2 2 1 2( ) ( (3 ) 2 ) ( )y d d d d d d d d dk y x y p x y p x y s x yδ = + + + + + +

(4)

In which

( , )u ux y — Ideal object axis in image plane

( , )d dx y —Real object axis in image plane

xδ —distortion in x direction

yδ —distortion in y direction

1 2,k k —Radial distortion parameter

1 2,p p —Centrifugal distortion parameter

1s ,

2s —Lens distortion parameter

In general, considering so much distortion will make the

result unstable, so also just considering the radial

distortion [16] in practice as function

2 2 2 2 2

1 2( ) ( )x d d d d d dk x x y k x x yδ = + + + (5)

2 2 2 2 2

1 2( ) ( )y d d d d d dk y x y k y x yδ = + + + (6)

In this paper, the author use Brown’s distortion model

and just considering radial distortion [17]. 2 4 6 2 2

1 2 5 3 4(1 ) (2 ( 2 ))d u u u ux x k r k r k r k x y k r x= + + + + + +

(7) 2 4 6 2 2

1 2 5 3 4(1 ) ( ( 2 ) 2 )d u u u uy y k r k r k r k r y k x y= + + + + + +

(8)

B. Camera Calibration Based on Extended Kalman

Filter

Take intrinsic and extrinsic parameters as the state

vector, the state vector is:

0 1 2 3 0 0 0 0 0 1 2 5( , , , , , , , , , , , , , )T

k u vx q q q q x y z f f u v k k k= (9)

State model can be described as

1k k kx A x −=

In which:

1kx −—state vector at instant 1k −

kx —state vector at instant k

kA —14 14× state transition matrix

When doing the calibration, image point ( , )u v and its

corresponding world axis ( , , )w w wx y z can be observed,

take them as the observation vector. Also take the

constrained condition 2 2 2 2

0 1 2 3 1q q q q+ + + = as observation,

so the observation vector can be described as

[ ] [ ]1 2 3 1 2 3( ), ( ), ( ) ( ) ( ) ( )

( )

T T

k

k k k

z h k h k h k n k n k n k

H x N

= +

= +

(10)

In which

( )k kH x —got by camera project model

kN —zero-mean white noise, is the functions of

time

1 2 3

1

7 8 9

2 2 2

1 2 5 0

5 6 7

2

7 8 9

2 2

1 2 5

( ) ( ) ( ) ( )( ) ( )

( ) ( ) ( ) ( )

(1 ( ) ( ) ( ) ) ( ) ( )

( ) ( ) ( ) ( )( ) ( )

( ) ( ) ( ) ( )

(1 ( ) ( ) (

w w w x

u

w w w x

w w w x

v

w w w x

r k x r k y r k z t kh k f k

r k x r k y r k z t k

k k r k k r k k r u k u k

r k x r k y r k z t kh k f k

r k x r k y r k z t k

k k r k k r k

+ + +=

+ + +

• + + + + =

+ + +=

+ + +

• + + + 2

0

2 2 2 2

3 0 1 2 3

) ) ( ) ( )

( ) 1

k r u k v k

h k q q q q

+ =

= + + + =

(11)

The state vector and the observation vector, this is a

non-linear system, the observation vector is non-linear,

most of the articles use Kalman filter do the calibration

based on the linear camera model. In general, extend

Kalman filter as an optimized linear filter to solve the non-

linear equation.

In order to solve the non-linear problem, we can transfer

the state equation and observation equation to linear the

equation by using first order terms from Taylor series

expansion of non-linear functions. Now the state

vectornx ∈ℜ , linearize around current estimate through

partial derivates of state update equations and

measurement update equations, we can get the

approximate linear model as

1 1 1( , , )k k k kx f x u w− − −= (12)

( , )k k kz H x v= (13)

The non-linear function f in difference equation

transforms the state at 1k − instance into the state at k

instance. Non-linear function H shows the relationship

between state vector kx and observation vector kz .

In fact, we can not get the value of process and

measurement noise kw and

kN at each instance with zero

mean Normal distribution, so we can get the state vector

and observation vector:

1 1

ˆ( , ,0)k k kx f x u− −=� (14)

( ,0)k kz H x= �� (15)

In which,

H—relates the state to the measurement

1841

When doing the calibration, the estimating parameters in

state vector are constant, so we can change the non-linear

function into linear one:

1 1 1

ˆ( )k k k k kx x A x x Ww− − −≈ + − +� (16)

( )k k k kz z H x x Vv≈ + − +�� (17)

In which, A, W, H, V are Jacobins and 1ku − is known

input

kx —state vector

kz —measurement vector

kx� —estimate state vector

kz� —estimate measurement vector

ˆkx —post estimate state vector

[ ]

[ ]

[ ],

ˆ( ,0)i

ki j

j

hV x

v

∂=

∂ (18)

[ ]

[ ]

[ ]

1

2

,

3

( )

( )

( )ˆ( , 0)

( )

( )

( )

j

i

ki j

jj

j

h k

x k

H h kM x

x x k

h k

x k

∂ ∂ ∂ ∂ = ==

∂ ∂

∂ ∂

(19)

Like the basic linear Kalman filter function, we can get

the time update equation and the state update equation, the

time update equations to predict and the measurement

update to correct.

Time update equations

1

ˆ ˆk k

x x−

−= (20)

1 1 1

T T

k k k k k k kP A P A W Q W−

− − −= + (21)

Measurement update equations

1( )T T T

k k k k k k k k kK P H H P H V R V− − −= + (22)

ˆkx = ˆ

kx− ˆ( ( ,0))k k kK z H x

−+ − (23)

( )k k k kP I K M P−= − (24)

So take the object axis and image axis as the inputs, the

flow chart of the algorithm is as Fig.5: start

Input data

Initial parameter and set

the iterative count n

k=1

k>n?

, ,1 1

ˆ ˆ( 0)k k k

x f x u−

− −=

1 1 1

T T

k k k k k k kP A P A W Q W−

− − −= +

1( )T T T

k k k k k k k k kK P H H P H V R V− − −= +

ˆ ˆ ˆ( ( ,0))k k k k k

x x K z h x− −= + −

( )k k k kP I K H P−

= −

k=k+1

No

Output parameter

Yes

End Fig.5 the algorithm flow

C. Stereo Calibration

After getting the parameters of each camera, it is easy

to get the translation matrix and rotation matrix between

the two cameras. A point P in world coordinate

system ( , , )w w wx y z , in left and right camera coordinate

system are ( , , )cL cL cLx y z , ( , , )cR cR cRx y z ,

So

xL w

cL L w L

cL w

x x

y R y T

z z

= +

xR w

cR R w R

cR w

x x

y R y T

z z

= +

From the above two equations, the following

relationship can got,

1 1

xR cL cL

cR R L cL R R L L cL

cR cl cl

x x x

y R R y T R R T R y T

z z z

− −

= + − = +

The translation matrix and rotation matrix are -1

-1-

R L

R R L L

R R R

T T R R T

= ⋅

=

D. Determine Relationship between Camera

Coordinate and Reference Coordinate

Assume the rotation matrix is R and translation vector

T between camera coordinate and reference coordinate.

The rotation matrix R can be described by unit 4-D

vector0 1 2 3( , , , )Tq q q q q= ,

In which,

2 2 2 2

0 1 2 3 1q q q q+ + + = ,

So

1 2 3

4 5 6

7 8 9

2 2 2 2

0 1 2 3 1 2 0 3 1 3 0 2

2 2 2 2

1 2 0 3 0 1 2 3 2 3 0 1

2 2 2 2

1 3 0 2 2 3 0 1 0 1 2 3

2( ) 2( )

2( ) 2( )

2( ) 2( )

r r r

R r r r

r r r

q q q q q q q q q q q q

q q q q q q q q q q q q

q q q q q q q q q q q q

=

+ − − − +

= + − + − − − + − − +

(25)

0 0 0( , , )

TT x y z= (26)

The parameters of the cameras are known, and the matrix

of the camera coordinate system is M , so if a point in

image is p , its corresponding coordinate in camera

coordinate system is P Mp= , as shown in Fig.6

The coordinate of point P in ref ref ref refO x y z is

unknown, assume its coordinate is rP ( , , )ref ref ref

T

ef x y zP P P= ,

So

rP efP R T= + (27)

Now, R , T and rP ef are unknown, if the motion of point

P is known, each time its incremental vector

( , , )T

i i ix y z∆ ∆ ∆ is known, so

1842

r(P ( , , ) )

T

i ef i i iP R x y z T= + ∆ ∆ ∆ + 1, 2,3....i n= (28)

There are 10 unknown parameters in Eq.28, so if the

object moves four times, all parameters can be got easily.

refxrefy

refzrefO

ux

uy

uO

cx

cy

cz

cO Pp

0 0( , )u v

x

y

1O

Fig.6 Camera coordinate system and reference coordinate system

III. EXPERIMENT AND RESULTS

A. Experiment Carried Out

In order to get the intrinsic and extrinsic parameters of

the stereo vision system and the relationship between the

camera coordinate system and the reference coordinate

system, the planner data and the 3D data are needed for

calibration and comparison.

Planner data can be got like Zhang’s method [9].

3D data is got by getting the hand’s feature point in both

image plane coordinate system and the 3D reference

coordinate system. As the hand’s initial posture is known,

let the arm and hand moves to a position where the robot

can see, record the posture both in 3D reference coordinate

and in image coordinate, in order to simple the

computation and get the landmark on the hand easily, we

paint several landmark points on the hand(as shown in

Fig.2).

If the arm and the hand can move in a large range, the

precision is much higher than in a smaller range. In this

paper, we make the arm and hand moves to 8 different

positions, record the postures, take the image plane

coordinate and the 3D space coordinate of the marked

point as the input.

B. Test Results

We take the planer data as the test data, in this way, can

do comparison with Zhang’s method, the data is also can

be used in Tsai’s method.

Calibration result of Zhang’s method:

1030.5599uf =

1032.6006vf =

1 0.1170k =

6

2 1.251 10k −= − ×

1030.5599 0.0043 199.1639

0 1032.6006 150.6752

0 0 1.0000

A

=

For the board’s first position, the rotation matrix and

transfer matrix is:

0.2993 0.9102 0.2861

0.6408 0.0304 0.7671

0.7069 0.4130 0.5742

R

= − − −

246.7855

102.7614

1855.2921

T

−

= −

Calibration result of Tsai’s method:

1030.2958f = 1 0.03k =

0.2980 0.9091 0.2911

0.6473 0.0317 0.7615

0.7016 0.4154 0.5790

R

= − − −

-246.9340

-102.3380

1856.0865

T

=

Calibration result of EKF method

0 0.4368q = ,1 0.6797q = ,

2 0.5741q = ,3 0.1546q = −

0 246.6727x = − ,0 102.2763y = − ,

0 1856.2910z =

u =1031.0077f , = 1032.1602vf

= 199.5635xC , = 149.8453yC

0.2992 0.9154 0.2914

0.6553 0.0345 0.7713

0.711 0.4 0.5769

R

= − − −

246.6727

102.2763

1856.2910

T

−

= −

C. Result Analyze

Standard deviation of mean can be computed by Eq.29

2

1

2

1

1( )

1

1( )

1

n

u i du

i

n

u i dv

i

u mn

v mn

σ

σ

=

=

= ∆ −

−

= ∆ − −

∑

∑ (29)

The result of the 3 methods is shows as follows: TABLEII

STANDARD DEVIATION OF MEAN

uσ （pixel） vσ （pixel）

Zhang’s 0.5399 0.7266

Tsai’s 0.7363 0.4522

EKF 0.2903 0.2025

All method can get good results, EKF method can get

better results in precision than the other methods.

IV. CONCLUSION

In this paper, a calibration method based extended

Kalman filter is proposed, based on this method the

extrinsic and intrinsic camera parameters can be got.

Experiment results show this method is effective. A

method based on vision to determine the relationship

between camera coordinate system and reference

coordinate system is discussed in this paper.

REFERENCES

[1] O.D.Faugeras, M.Herbert, “Representation, recognition and

locating of 3-D objects, International Conference on Pattern

Recognition”, 1986, pp15-20.

[2] S.Ganapathy, “Decomposition of transformation matrices for robot

vision”, Proceeding of IEEE International Conference on Robot

and Automation, 1984, pp130-139.

[3] O. D. Faugeras, G.. Toscani, “Camera Calibration for 3D

Computer Vision”, Proc.Int. Workshop on Industrial Application of

Machine Vision and Machine Intelligence. NewYork: IEEE, 1986,

pp15-20.

1843

[4] R. Y. Tsai, “An Efficient and Accurate Camera Calibration

Technique for 3D Machine Vision”, Proc. of IEEE Conference of

Computer Vision and Pattern Recognition. 1986, pp364~374.

[5] R. Y. Tsai, “A Versatile Camera Calibration Technique for High

Accuracy 3D Machine Vision Metrology Using Off-the-shelf TV

Cameras and Lenses”, IEEE Journal of Robotics and

Automation,1987, 4,pp323~344.

[6] L. M. Song, M. P. Wang, L. Lu, H. J. Huan, “High Precision

Camera Calibration in Vision Measurement”, Optics and Laser

Technology. 2007(39), pp1413~1420.

[7] J. Weng, P. Cohen, M. Herniou, “Camera Calibration with

Distortion Models and Accuracy evaluation”, Trans. on Pattern

Analysis and Machine Intelligence, 1992, 14(10), pp965~980.

[8] Z. Y. Zhang, “A Flexible New Technique for Camera Calibration”,

IEEE Transactions on Pattern Analysis and Machine Intelligence.

2000, 22(11),pp1330~1334.

[9] Z. Y. Zhang, “Flexible Camera Calibration by Viewing a Plane

from Unkown Orientations”, IEEE International Conference on

Computer Vision. Greece, 1999, pp666~673.

[10] R. E. Kalman, “A New Approach to Linear Filtering and Prediction

Problems”, Transaction of the ASME-Journal of Basic Engineering,

1960, pp35~45.

[11] G.J. CASTRO, J.M. GALLEGO and L. P.E. CABELLO, “An

effective camera calibration method,” IEEE AMC’98-Coimbra. 5th

International Workshop, 1998, pp171-174.

[12] Faraz M. Mirzaei and Stergios I. Roumeliotis, “A Kalman Filter-

based Algorithm for IMU-Camera Calibration”, Proceedings of the

2007 IEEE/RSJ International Conference on Intelligent Robots and

Systems San Diego, CA, USA, Oct 29 - Nov 2, 2007, pp2427-

2434.

[13] Faraz M. Mirzaei, “A Kalman Filter-Based Algorithm for IMU-

Camera Calibration: Observability Analysis and Performance

Evaluation”, IEEE transactions on robotics, 2008, 24(5), pp1143-

1156.

[14] ZHOU Fuqiang, ZHAI Jin, ZHANG Guangjun, “A camera

calibration method based on Iterated Extended Kalman Filter using

planar target”, Sixth Intl. Symp. On Instrumentation and Control

technology:sensors, automatic measurement, control, and computer

simulation, 2006

[15] L. Wang, M. Xie, Z.W. Zhong, H.J. Yang and J. Li, “Design of

Dexterous Arm-Hand for Human-Assisted Manipulation”, First

International Conference on Intelligent Robotics and Applications,

Wuhan, China.

[16] G.. Q. Wei. S. D. Ma, “Implicit and Explicit Camera Calibration:

theory and experiment”,IEEE Trans. Pattern Recognition and

Machine Intelligence. 1994, 16(5),pp469~480

[17] D. C. Brown, “Lens Distortion for Close-range Photogrammetry”,

Photometric Engineering. 1971, 37(8), pp855~866

1844

View publication statsView publication stats