Summary of the invention
For above shortcomings in prior art, the invention provides and a kind ofly can carry out to left view sheet and right view sheet the stereoscopic vision automatic geometric calibration system of geometric calibration, outer irregular different adjustment in order to left view sheet and the right view sheet of the stereo visual system collection to mobile robot, the stereo visual system that solves mobile robot easily causes the poor problem of locating effect because outer ginseng changes, in order to help mobile robot accurately to locate.
For solving the problems of the technologies described above, realize goal of the invention, the technical solution used in the present invention is as follows:
A stereoscopic vision automatic geometric calibration system, comprises picture load module, unique point processing module, picture adjustment module, described picture load module is used for inputting left view sheet and right view sheet, described unique point processing module is for extracting many stack features pixel pair from left view sheet and right view sheet, the left view feature pixel of every stack features pixel centering and right view feature pixel represent Same Physical point, solve the position coordinates of left view feature pixel in left camera coordinate system and right view feature pixel at the position coordinates of right camera coordinate system, calculate right view sheet with respect to rotation matrix and the translation matrix of left view sheet, described picture adjustment module utilizes rotation matrix and translation matrix to adjust at the position coordinates of right camera coordinate system each pixel in right view sheet, complete the geometric calibration to left view sheet and right view sheet.
As the further optimization of such scheme, described unique point processing module utilizes SIFT conversion to extract left view feature pixel and right view feature pixel.
As the further optimization of such scheme, described unique point processing module is set up coordinate transformation relation formula, solves feature pixel centering left view feature pixel at the position coordinates (X of left camera coordinate system
1, Y
1, Z
1) and right view feature pixel at the position coordinates (X of right camera coordinate system
2, Y
2, Z
2), described coordinate transformation relation formula is:
In formula, i=1,2, (u
1, v
1) and (u
2, v
2) be respectively coordinate and the right view feature pixel coordinate in right view sheet coordinate system of feature pixel centering left view feature pixel in left view sheet coordinate system; F is the focal length of camera; B is the baseline distance of left and right two cameras, d=u
1-u
2.
As the further optimization of such scheme, described unique point processing module is set up geometric transformation relational expression, utilizes least square method, calculates right view sheet with respect to rotation matrix R and the translation matrix T of left view sheet; Described geometric transformation relational expression is:
As the further optimization of such scheme, described picture adjustment module utilizes rotation matrix and translation matrix to adjust at the position coordinates of right camera coordinate system each pixel in right view sheet:
Wherein, (x
2, y
2, z
2) for pixel in right view sheet before adjusting is at the position coordinates of right camera coordinate system, (x
2', y
2', z
2') for this pixel in right view sheet after adjusting is at the position coordinates of right camera coordinate system.
Than prior art, tool of the present invention has the following advantages:
1. stereoscopic vision automatic geometric calibration system provided by the invention, can adjust in right view sheet pixel at the position coordinates of right camera coordinate system, make the position coordinates of physical points imaging point in left camera that this pixel is corresponding and in right camera the position coordinates of imaging point reach unanimity, solved in existing mobile robot's stereo visual system, due to the degree of accuracy decline that in use left and right two camera relative positions change and cause, the problem that locating effect is poor.
2. the stereoscopic vision automatic geometric calibration system that this method provides, have calculated amount little, can adjust in real time online the deviation of left and right camera, real-time is good, guarantees the optimum matching effect of stereo visual system.
3. the stereoscopic vision automatic geometric calibration system that this method provides, does not need additionally to arrange scaling board, can be to any environment, and left view sheet and right view sheet that random time obtains carry out real-time calibration, have environment self-adaption.
Embodiment
Stereoscopic vision automatic geometric calibration system provided by the invention, for mobile apparatus human stereo vision, mobile robot is provided with two cameras in left and right, for simulating people's eyes.When localization for Mobile Robot jobbie, two of left and right camera is taken this object simultaneously.The camera on the left side obtains left view sheet, and the camera on the right obtains right view sheet.
According to binocular stereo vision principle: as shown in Figure 3, baseline is the distance of the projection centre line of left and right two cameras apart from b; Camera focal length is f, and the initial point of camera coordinate system is at the photocentre place of camera lens.If left and right two cameras, at synchronization, are watched the Same Physical point P of space object on same level line, the imaging point of physical points P in left view sheet is P
left, the imaging point in right view sheet is P
right, with left camera coordinate, be standard, the position coordinates of physical points P in left camera coordinate system is expressed as P (x
c, y
c, z
c), P
leftcoordinates table in left view sheet coordinate system is shown (u
left, v
left), P
rightcoordinates table in right view sheet coordinate system is shown (u
right, v
right).In the ideal case, p
leftand p
rightin same plane, their coordinate should meet following relation:
P
leftand p
righty coordinate identical, i.e. v
left=v
right=v.But in actual conditions, due to the unpredictable factor in use procedure, left and right two camera relative positions cannot meet the condition of ideal situation hypothesis, and may often change.Must this variation be carried out real-time adaptive and be compensated, the accuracy of guarantee system.
Stereoscopic vision automatic geometric calibration system provided by the invention, as shown in Figure 1, comprises picture load module, unique point processing module, picture adjustment module, described picture load module is used for inputting left view sheet and right view sheet, described unique point processing module is for extracting many stack features pixel pair from left view sheet and right view sheet, the left view feature pixel of every stack features pixel centering and right view feature pixel represent Same Physical point, be that left view feature pixel refers to this physical points imaging point in left view sheet, right view feature pixel refers to this physical points imaging point in right view sheet, as shown in Figure 2, point a, point b is respectively left view feature pixel, point a ', point b ' is respectively right view feature pixel, its mid point a and some a ' are a stack features pixel pair, point b and some b ' are a stack features pixel pair.Solve the position coordinates of left view feature pixel in left camera coordinate system and right view feature pixel at the position coordinates of right camera coordinate system, calculate right view sheet with respect to rotation matrix and the translation matrix of left view sheet, described picture adjustment module utilizes rotation matrix and translation matrix to adjust at the position coordinates of right camera coordinate system each pixel in right view sheet, completes the geometric calibration to left view sheet and right view sheet.
In order to reduce error, unique point processing module can be extracted 100 feature pixels pair, and to these 100 feature pixels to processing.The present invention can adjust in right view sheet pixel at the position coordinates of right camera coordinate system, make the position coordinates of physical points imaging point in left camera that this pixel is corresponding and in right camera the position coordinates of imaging point reach unanimity, solved in existing mobile robot's stereo visual system, due to the degree of accuracy decline that in use left and right two camera relative positions change and cause, the problem that locating effect is poor.In addition, the present invention does not need, according to actual conditions, scaling board is additionally set, unique point processing module acquiescence left view sheet is that scaling board is (when specifically apply, also can take right view sheet as scaling board), when mobile robot is at any environment, when the left view sheet that random time obtains and right view sheet, the present invention can adjust the position coordinates of right view sheet rapidly, the optimum matching effect that has guaranteed stereo visual system, has environment self-adaption.
As prioritization scheme of the present invention, described unique point processing module is set up coordinate transformation relation formula, solves feature pixel centering left view feature pixel at the position coordinates (X of left camera coordinate system
1, Y
1, Z
1) and right view feature pixel at the position coordinates (X of right camera coordinate system
2, Y
2, Z
2), described coordinate transformation relation formula is:
In formula, i=1,2, (u
1, v
1) and (u
2, v
2) be respectively coordinate and the right view feature pixel coordinate in right view sheet coordinate system of feature pixel centering left view feature pixel in left view sheet coordinate system; F is the focal length of camera; B is the baseline distance of left and right two cameras, d=u
1-u
2.
Described unique point processing module is set up geometric transformation relational expression, utilizes least square method, calculates right view sheet with respect to rotation matrix R and the translation matrix T of left view sheet; Described geometric transformation relational expression is:
Described picture adjustment module utilizes rotation matrix and translation matrix to adjust at the position coordinates of right camera coordinate system each pixel in right view sheet:
Wherein, (x
2, y
2, z
2) for pixel in right view sheet before adjusting is at the position coordinates of right camera coordinate system, (x
2', y
2', z
2') for this pixel in right view sheet after adjusting is at the position coordinates of right camera coordinate system.
SIFT converts the (abbreviation of Scale-invariant feature transform, be yardstick invariant features conversion) be used for detecting and describe the locality feature in image, it finds extreme point in space scale, and extracts its position, yardstick, rotational invariants.The left view feature pixel and the right view feature pixel that utilize SIFT conversion to obtain are SIFT unique point, SIFT feature pixel has good robustness and stability for the deviation of left and right two cameras, this category feature pixel centering left view feature pixel is stable at position coordinates and the geometric maps Relationship Comparison of right view feature pixel between the position coordinates of right camera coordinate system of left camera coordinate system, more accurate to solving the rotation matrix and the translation matrix that obtain according to this category feature pixel, can adjust better in right view sheet pixel at the position coordinates of right camera coordinate system, make the position coordinates of physical points imaging point in left camera that this pixel is corresponding and in right camera the position coordinates of imaging point reach unanimity.The present invention only needs according to a small amount of feature pixel pair, just can calculate rotation matrix and translation matrix comparatively accurately, and calculated amount is little, can adjust in real time online the deviation of left and right camera, and real-time is good, guarantees the optimum matching effect of stereo visual system.And the present invention is when adjusting right view sheet, with v
left=v
right=v is constant index, and the left view sheet after adjustment and right view sheet still meet binocular stereo vision principle under perfect condition (formula 1), have improved mobile robot's stereo visual system degree of accuracy.
When rotation matrix and a multiplication of vectors, vectorial direction can be changed, vectorial size can not be changed.Rotation matrix is defined as:
In formula, r
1=cos ψ cos φ, r
2=sin θ sin ψ cos φ-cos θ sin φ, r
3=cos θ sin ψ cos φ-sin θ cos φ, r
4=cos ψ sin φ, r
5=sin θ sin ψ sin φ+cos θ cos φ, r
6=cos θ sin ψ sin φ-sin θ cos φ, r
7=sin ψ, r
8=sin θ cos ψ, r
9=cos θ cos ψ, θ, ψ, φ refer to for left camera coordinate system in left view feature pixel Vectors matching, in right camera coordinate system right view feature pixel vector respectively around X-axis, Y-axis, the angle of Z axis rotation.Left view feature pixel vector refers to the vector that points to left view feature pixel from left camera coordinate origin, and right view feature pixel vector refers to the vector that points to right view feature pixel from right camera coordinate origin.Translation matrix is defined as:
In formula, T
x=X
1-X
2, T
y=Y
1-Y
2, T
z=Z
1-Z
2.
Can find out, in rotation matrix and translation matrix, always have 6 unknown number: θ, ψ, φ, T
x, T
y, T
z, utilize least square method to obtain this 6 parameters, can solve easily rotation matrix and translation matrix, error is minimum, in adjusting right view sheet during the position coordinates of pixel at right camera coordinate system, rotation matrix R has changed the direction of pixel vector in right view sheet, translation matrix T makes the pixel vector of adjusting after direction in right view sheet can be along Y-axis translation, wherein, the pixel vector of right view sheet refers to the vector that points to this pixel right view sheet from right camera coordinate origin, like this, just can adjust in right view sheet pixel at the position coordinates of right camera coordinate system, make the position coordinates of physical points imaging point in left camera that this pixel is corresponding and in right camera the position coordinates of imaging point reach unanimity, solved in existing mobile robot's stereo visual system, due to the degree of accuracy decline that in use left and right two camera relative positions change and cause, the problem that locating effect is poor.
In sum, stereoscopic vision automatic geometric calibration system provided by the invention, from left view sheet and right view sheet, extract many stack features pixel pair, solve the position coordinates of left view feature pixel in left camera coordinate system and right view feature pixel at the position coordinates of right camera coordinate system, calculate right view sheet with respect to rotation matrix and the translation matrix of left view sheet, finally, utilize rotation matrix and translation matrix to adjust the position coordinates of each pixel in right view sheet.Can adjust in right view sheet pixel at the position coordinates of right camera coordinate system, make the position coordinates of physical points imaging point in left camera that this pixel is corresponding and in right camera the position coordinates of imaging point reach unanimity, solved in existing mobile robot's stereo visual system, due to the degree of accuracy decline problem that in use left and right two camera relative positions change and cause.Do not need additionally to arrange scaling board, can be to any environment, left view sheet and right view sheet that random time obtains are processed in real time, have environment self-adaption.
Finally explanation is, above embodiment is only unrestricted in order to technical scheme of the present invention to be described, although the present invention is had been described in detail with reference to preferred embodiment, those of ordinary skill in the art is to be understood that, can modify or be equal to replacement technical scheme of the present invention, and not departing from aim and the scope of technical solution of the present invention, it all should be encompassed in the middle of claim scope of the present invention.