CN103487035B - Based on monocular localization method and the system of installed video - Google Patents

Abstract

Based on monocular localization method and the system of installed video, comprise off-line procedure and at line process: off-line procedure, comprises training objective master drawing, sets up object classifiers storehouse; At line process, comprise the interesting target in based target sorter storehouse detection input picture, obtain respective objects rectangular block region, for the target of type of ground objects, carry out ground monocular according to respective objects rectangular block region and locate the coordinate obtained in three-dimensional scenic, finally convert the longitude and latitude of target to; The target of described type of ground objects, a certain lateral edges ground connection of feeling the pulse with the finger-tip target; For the target of non-type of ground objects, carry out ground binocular according to respective objects rectangular block region and locate the coordinate obtained in three-dimensional scenic, finally convert the longitude and latitude of target to; The target of described non-type of ground objects, feeling the pulse with the finger-tip mark does not have either side edge ground connection.

Description

Based on monocular localization method and the system of installed video

Technical field

The present invention relates to traverse measurement field, particularly relate to a kind of monocular localization method based on installed video and system.

Background technology

Vehicle-mounted mobile measuring system can be taken live-action image and be positioned static city parts and measure, be applied to city management parts and collect management, but ubiquity efficiency is low at present, automaticity is not high, acquisition cost is high, upgrades difficulty, cannot obtain the problems such as the movement properties of object.Classic method utilizes manpower manual review to select attention object, the point on two images is selected to carry out binocular location, each 2 or more images are all needed to position atural object, this not only significantly reduces target and collects efficiency, also improve acquisition cost, and be difficult to the movement velocity direction of Quick Measurement object.Therefore in the urgent need to a kind of vehicle-mounted automatic collection, the dynamic live-action image method for measurement of location targets of interest.

Summary of the invention

The object of the invention is to overcome above-mentioned technical disadvantages, a kind of monocular location technology scheme based on installed video is provided.

Technical scheme of the present invention is a kind of monocular localization method based on installed video, comprises off-line procedure and at line process,

Off-line procedure, comprises training objective master drawing, sets up object classifiers storehouse;

At line process, comprise the interesting target in based target sorter storehouse detection input picture, obtain respective objects rectangular block region; For the target of type of ground objects, carry out ground monocular according to respective objects rectangular block region and locate the coordinate obtained in three-dimensional scenic, finally convert the longitude and latitude of target to; The target of described type of ground objects, a certain lateral edges ground connection of feeling the pulse with the finger-tip target.

And ground monocular location implementation is as follows,

Set up road surface intersection point coordinate system, do a vertical line perpendicular to ground from roof GPS, set up intersection point O coordinate system X-Y-Z using ground intersection point O as initial point, this coordinate system X-Z axle is close to ground, X-axis is relative to car towards the right side, and Z axis points to the dead ahead of car, and Y-axis is perpendicular to sensing underground, ground; h _gpsfor roof GPS is relative to the height on ground;

The lower edge in described target rectangle block region is in ground, and the top left co-ordinate in target rectangle block region is u, v, and the frame in target rectangle block region is wide=w, and frame is high=h;

For the pose transformation matrix R of ground intersection point _picture-Owith the displacement transition matrix T of camera relative to ground intersection point _picture-O, decompose the matrix H obtaining 3 × 3 _o,

H _o3x3=sK [r _{picture-Che 1}r _{picture-Che 3}-R _picture-Chet _picture-O]=s [r _{picture-Che 1}r _{picture-Che 3}-R _picture-Che(T _picture-Che+ T _car-O)]

Wherein, s is Arbitrary Coefficient, and K is camera internal reference matrix, R _picture-Chethe pose transformation matrix of camera relative to car, r _{picture-Che 1}and r _{picture-Che 3}be respectively matrix R _picture-Chethe 1st row and the 3rd arrange, T _picture-Chethe displacement transition matrix of camera relative to car, T _car-Othe displacement transition matrix of car relative to ground intersection point;

If the object space three-dimensional coordinate in intersection point O coordinate system X-Y-Z is (X, Y, Z), by ground intersection point as coordinate u+w/2, v+h/2 calculates X, and Z is as follows,

[X Z 1] ^T=zH _O ^-1[u+w/2 v+h 1] ^T

Wherein, z is unknowm coefficient, is solved by above-mentioned equation row 3 equations;

Gained X, Z are then [X-h relative to car gps coordinate _gpsz], [X-h _gpsz] be converted to again and measure city coordinate system, finally conversion obtains ground latitude and longitude coordinates to terrestrial coordinate system.

And, for the target of non-type of ground objects, carry out ground binocular according to respective objects rectangular block region and locate the coordinate obtained in three-dimensional scenic, finally convert the longitude and latitude of target to; The target of described non-type of ground objects, feeling the pulse with the finger-tip mark does not have either side edge ground connection;

The top left co-ordinate supposing the target rectangle block region detected on image 1 is u ₁, v ₁, frame is wide=w, and frame is high=h, obtain the coordinate x of center on image 1 in target rectangle block region ₁=u ₁+ w/2, y ₁=v ₁+ h/2, another image 2 carries out match search, obtains the coordinate x of the respective objects rectangular block regional center on image 2 ₂=u ₂+ w/2, y ₂=v ₂+ h/2,

Separate the object space three-dimensional coordinate X that following equation obtains object center, Y, Z

$\left[\begin{array}{ccc}{M}_{111}-x_1{M}_{131}& M_{112}-x_1{M}_{132}& M_{113}-x_1{M}_{133}\\ M_{121}-y_1{M}_{131}& M_{122}-y_1{M}_{132}& M_{123}-y_1{M}_{133}\\ M_{211}-x_2{M}_{231}& M_{212}-x_2{M}_{232}& M_{213}-x_2{M}_{233}\\ M_{221}-y_2{M}_{231}& M_{222}-y_2{M}_{232}& M_{223}-y_{\text{2}}{M}_{233}\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\left[\begin{array}{c}{x}_1{m}_{13}-m_{11}\\ y_1{m}_{13}-m_{12}\\ x_2{m}_{23}-m_{21}\\ y_2{m}_{23}-m_{22}\end{array}\right]$

Wherein [M _im _i] construct the projection matrix of i-th camera

M _i=K _iR _im _i=-K _iR _iT _ii=1,2

M _ithe matrix that 3 row 3 arrange, m _ithe matrix that 3 row 1 arrange, M _ircrepresent i-th camera M _ithe capable c train value of r of matrix, m _irrepresent i-th camera m _icapable 1 train value of r of matrix; K _iit is the inner parameter matrix of i-th camera;

Gained [X Y Z] is converted to measures city coordinate system, and finally conversion obtains ground latitude and longitude coordinates to terrestrial coordinate system.

The present invention is also corresponding provides a kind of monocular positioning system based on installed video, comprises off-line part and online part,

Off-line part, comprises the training module for training objective master drawing, for setting up the classifier modules in object classifiers storehouse;

Online part, comprises with lower module,

Detection module, detects the interesting target in input picture for based target sorter storehouse, obtains respective objects rectangular block region; Monocular locating module, for the target for type of ground objects, carries out ground monocular according to respective objects rectangular block region and locates the coordinate obtained in three-dimensional scenic, finally convert the longitude and latitude of target to; The target of described type of ground objects, a certain lateral edges ground connection of feeling the pulse with the finger-tip target.

And online part comprises binocular locating module, for the target for non-type of ground objects, carries out ground binocular and locates the coordinate obtained in three-dimensional scenic, finally convert the longitude and latitude of target to according to respective objects rectangular block region; The target of described non-type of ground objects, feeling the pulse with the finger-tip mark does not have either side edge ground connection.

Monocular location technology scheme works based on installed video provided by the invention is:

1. can locate in real time the movement of the street and static attention object with measurement car, directly can carry out list as objective location for ground object target, ground object target does not need binocular to locate; Realize simple, corresponding system device can be arranged on to be measured on car.

2. versatility is good, and the work off-line setting up object library completes, can support that the batch processing of various interesting target is collected;

3. easily extensible is to the binocular location, ground of non-ground object target, supports ground object target and non-ground object target two class location simultaneously, efficient and convenient.

Accompanying drawing explanation

Fig. 1 is the schematic flow sheet of the embodiment of the present invention.

Fig. 2 be the embodiment of the present invention road ground image on the schematic diagram of center of going to the bottom corresponding ground intersection point of ground object target frame.

Embodiment

The monocular location that the present invention introduces ground object target to be located with the binocular of non-atural object and is combined, and the mode establishing target sorter storehouse adopting sorter to train, to support automatic collection and the three-dimensional fix of target, drastically increase efficiency.According to the position characteristics of target, target is divided into atural object and the large class of non-atural object two.

Ground object target refers to a certain lateral edges ground connection of target, comprises pedestrian, vehicle, well lid, wire pole tower, the building construction of power transformer, Stall ground connection, the shop front, and the stall with goods spread out on the ground for sale retailer gone up in the street waits ground connection target.

Non-ground object target feeling the pulse with the finger-tip mark, without any a lateral edges ground connection, comprises traffic mark board, the high-rise architecture against regulations, traffic lights, various billboard, bank's signboard, government department's signboard, face.

Technical solution of the present invention is described in detail below in conjunction with drawings and Examples.

The present invention can adopt software engineering to realize automatically running, and defiber and online two parts process are carried out, as Fig. 1:

Off-line procedure, comprise training objective master drawing, set up object classifiers storehouse: the off-line part of usually carrying out in advance can utilize the Haar features training in OpenCV to obtain Cascade sorter, namely the Haar feature of sample image is extracted, cascade sorter training algorithm is adopted to train great amount of samples, obtain sorter, specific implementation can with reference to prior art, and it will not go into details in the present invention.Different classifiers combination constituent class device storehouse can be adopted for identifying various different interesting target.In embodiment, first collect the image of a large amount of targets of interest, train, each group target image sample obtains a sorter, and the training of many group different targets obtains sorter set C ₁... C _nrepresentative has the sorter of n target altogether, forms object classifiers storehouse, wherein sorter C _ican be used for judging in image, whether a rectangle frame target belongs to the i-th class target, and the value of i is 1 ... n.During concrete enforcement, training can adopt software modularity mode, provides in the mode of training module, and sample image input training module, obtains sorter.

At line process, comprise the interesting target in based target sorter storehouse detection input picture, obtain respective objects rectangular block region.The monocular that the present invention proposes for type of ground objects target is located, when specifically implementing, for improving range of application of the present invention, the existing binocular location technology of accessible site.

For the target of type of ground objects, carry out ground monocular according to respective objects rectangular block region and locate the coordinate obtained in three-dimensional scenic, finally convert the longitude and latitude of target to;

For the target of non-type of ground objects, carry out ground binocular according to respective objects rectangular block region and locate the coordinate obtained in three-dimensional scenic, finally convert the longitude and latitude of target to.

The realization of the online part of embodiment is described as follows:

(1) first carry out target detection, scan full figure search based on sorter i and obtain target frame set rect _i1, rect _i2, rect _i3Rect _ijrepresent that the i-th classification is marked on the jth target that image finds.Target frame is the rectangle frame that the minimum area-encasing rectangle of target is formed.

(2) whether target localization, comprises the attribute according to sorter i, be namely ground object target, these rectangle frames entered respectively (2.1) (2.2) and process.

(2.1) the installed video ground object list picture location realization of embodiment is described as follows:

Positioning result is exactly obtain ground latitude and longitude coordinates according to image ground object target coordinate (general employing top left co-ordinate u, v is as image origin).To ground object, object detection results gained target frame lower limb just in time on the ground, if the feet station of pedestrian is on ground, rectangle frame just comprises the pin of pedestrian.As Fig. 2, be pavement of road in the middle of highway sideline, the target frame that target rectangle block region is formed is positioned on road, the top left co-ordinate in target rectangle block region is u, v, the frame in target rectangle block region is wide=and w, frame is high=h, in Fig. 2, the go to the bottom cross at center of rectangle frame represents intersection point, and namely the center point coordinate of going to the bottom in target rectangle block region is u+w/2, v+h.

Set up road surface intersection point coordinate system, do a vertical line perpendicular to ground from roof GPS.Intersection point O coordinate system X-Y-Z is set up using ground intersection point O as initial point.This coordinate system X-Z axle is close to ground, and X-axis is relative to car towards the right side, and Z axis points to the dead ahead of car, and Y-axis is perpendicular to sensing underground, ground.

Suppose that the GPS device on car is fixed as h relative to the height on ground _gps(the i.e. distance h of O to GPS on vertical line _gps).

Ideally, the posture position relation on onboard system and ground keeps necessarily, and camera is relative to the T of car _picture-Che, R _picture-Chealso fix, so

T _picture-O=T _picture-Che+ T _car-O

R _picture-O=R _picture-Cher _car-O=R _picture-Che

Wherein T _car-O=[0 0-h _gps] R _car-O=I

T _picture-Chethe displacement transition matrix of camera relative to car, T _picture-Othe displacement transition matrix of camera relative to ground intersection point, T _{car -O}the displacement transition matrix of car relative to ground intersection point;

R _picture-Chethe pose transformation matrix of camera relative to car, R _picture-Othe pose transformation matrix of camera relative to ground intersection point, R _{car -O}the pose transformation matrix of car relative to ground intersection point.

So by R _picture-Oand T _picture-Othe H obtaining 3 × 3 can be decomposed _omatrix,

H _o3x3=sK [r _{picture-Che 1}r _{picture-Che 3}-R _picture-Chet _picture-O]=s [r _{picture-Che 1}r _{picture-Che 3}-R _picture-Che(T _picture-Che+ T _car-O)]

S is Arbitrary Coefficient, and K is camera internal reference matrix,

$K=\left[\begin{array}{ccc}{f}_u& 0& c_u\\ 0& f_v& c_v\\ 0& 0& 1\end{array}\right]$

C _u, c _vthe principal point of camera, f _u, f _vthe focal length pixel being camera is long;

R _{picture-Che 1}and r _{picture-Che 3}be respectively matrix R _picture-Chethe 1st row and the 3rd arrange

T _picture-Chebe the fixed amount on earth of calibration, therefore no matter which plane road surface garage sails on, H _oall fixing.H _o3x3to reflect in intersection point coordinate system ground any point to the mapping relations on image.H conversely _o ^-1to reflect on the road surface on image any point to the topocentric mapping relations in intersection point coordinate system.

If the object space three-dimensional coordinate in Local Road Surface coordinate system X-Y-Z is (X, Y, Z), by ground intersection point as coordinate u+w/2, v+h calculates X, and Z is as follows,

[X Z 1] ^T=zH _O ^-1[u+w/2 v+h 1] ^T

Z is a unknowm coefficient, solves by equation row 3 equations above.(about above formula equation, two 3x1 vectors are equal, every a line equation row equation, and three equations just in time solve three unknown number X Z z)

Notice that X, Z are the road surface intersection point coordinate systems relative to local, being converted to relative to the coordinate of car gps coordinate is then [X-h _gpsz].[X-h _gpsz] be converted to again and measure city coordinate system, finally conversion can obtain ground latitude and longitude coordinates to terrestrial coordinate system.

(2.2) installed video non-ground object fast locating algorithm for the sake of ease of implementation, is introduced as follows:

Positioning result be exactly according to the non-ground object target rectangle frame of image as coordinate (adopt top left co-ordinate u, v) and the wide w of frame, frame height h obtains ground latitude and longitude coordinates.

If the 1st camera have taken image 1, the 2nd camera have taken image 2, and the top left co-ordinate of hypothetical target frame on image i is u _i, v _i, frame is wide=w _i, frame is high=h _i, the value of i is 1,2.Two camera subject image size approximately equals of taking side by side, make w ₁=w ₂=w, h ₁=h ₂=w.

Suppose on image 1, detect that the top left co-ordinate of target frame is u ₁, v ₁, frame is wide=w, and frame is high=h, now and be positioned at ground without requiring rectangle frame lower limb.The coordinate of center on image 1 of target frame is x ₁=u ₁+ w/2, y ₁=v ₁+ h/2.

Another image 2 to fluctuate pixel distance D along core line, D is arranged (such as D=0.01 figure image height) in proportion according to image size, the rectangular block search of wide w height h pixel is carried out in this belt-like zone, namely the rectangular block center searched for and the pixel distance≤D of core line, the rectangular block that prior art calculating can be adopted to search for mates correlation coefficient value with the target frame on image 1, if maximum coupling correlation coefficient value has exceeded a certain predetermined threshold value, then using rectangle frame position corresponding for maximum coupling correlation coefficient value as the match bit on image 2, otherwise illustrate and do not search match bit.So obtain the target frame centre coordinate x on image 2 ₂=u ₂+ w/2, y ₂=v ₂+ h/2.

The following equation of binocular stereo imaging solution obtains the object space three-dimensional coordinate X at object center, Y, Z

$\left[\begin{array}{ccc}{M}_{111}-x_1{M}_{131}& M_{112}-x_1{M}_{132}& M_{113}-x_1{M}_{133}\\ M_{121}-y_1{M}_{131}& M_{122}-y_1{M}_{132}& M_{123}-y_1{M}_{133}\\ M_{211}-x_2{M}_{231}& M_{212}-x_2{M}_{232}& M_{213}-x_2{M}_{233}\\ M_{221}-y_2{M}_{231}& M_{222}-y_2{M}_{232}& M_{223}-y_{\text{2}}{M}_{233}\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\left[\begin{array}{c}{x}_1{m}_{13}-m_{11}\\ y_1{m}_{13}-m_{12}\\ x_2{m}_{23}-m_{21}\\ y_2{m}_{23}-m_{22}\end{array}\right]$

Wherein [M _im _i] construct the projection matrix of i-th camera

M _i=K _iR _im _i=-K _iR _iT _ii=1,2

M _ithe matrix that 3 row 3 arrange, m _ithe matrix that 3 row 1 arrange, M _ircrepresent i-th camera M _ithe capable c train value of r of matrix, m _irrepresent i-th camera m _icapable 1 train value of r of matrix.

K _ithe inner parameter matrix of i-th camera,

$K_i=\left[\begin{array}{ccc}{f}_{\mathrm{ui}}& 0& c_{\mathrm{ui}}\\ 0& f_{\mathrm{vi}}& c_{\mathrm{ui}}\\ 0& 0& 1\end{array}\right]$

C _ui, c _vithe principal point of camera i, f _ui, f _vithe focal length pixel being camera i is long;

R _iit is the attitude matrix of i-th camera;

T _ithe location matrix of i-th camera, i.e. camera photocentre position.

Notice that X, Y, Z are the coordinates relative to car gps coordinate.[X Y Z] is converted to and measures city coordinate system, and finally conversion can obtain ground latitude and longitude coordinates to terrestrial coordinate system.

Software solidification mode can be adopted to provide the corresponding monocular positioning system based on installed video during concrete enforcement, mainly be divided into off-line and online two large divisions, off-line part training objective master drawing, sets up object classifiers storehouse; Online part detects interesting target in real time according to sorter storehouse.Off-line part comprises training module and the classifier modules of sample image, is mainly set up the sorter storehouse of interesting target by machine learning according to training sample image.Online part comprises detection module, monocular locating module, also can arrange binocular locating module:

Detection module is with object classifiers storehouse for instructing, and scanning full figure, automatically detects the interesting target in input picture, obtain target rectangle block region.

Monocular locating module is for ground object target, and the target rectangle block region obtained according to detection module is carried out ground monocular and located the coordinate obtained in three-dimensional scenic, finally converts the longitude and latitude of target to.

Binocular locating module is for non-ground object target, and the target rectangle block region obtained according to detection module is carried out ground binocular and located the coordinate obtained in three-dimensional scenic, finally converts the longitude and latitude of target to.

Each module specific implementation is corresponding to method, and it will not go into details in the present invention.

Specific embodiment described herein is only to the explanation for example of the present invention's spirit.Those skilled in the art can make various amendment or supplement or adopt similar mode to substitute to described specific embodiment, but can't depart from spirit of the present invention or surmount the scope that appended claims defines.

Claims (2)

1. based on a monocular localization method for installed video, it is characterized in that: comprise off-line procedure and at line process,

Off-line procedure, comprises training objective master drawing, sets up object classifiers storehouse;

At line process, comprise the interesting target in based target sorter storehouse detection input picture, obtain respective objects rectangular block region; For the target of type of ground objects, carry out ground monocular according to respective objects rectangular block region and locate the coordinate obtained in three-dimensional scenic, finally convert the longitude and latitude of target to; The target of described type of ground objects, a certain lateral edges ground connection of feeling the pulse with the finger-tip target;

Monocular location, described ground implementation is as follows,

Set up road surface intersection point coordinate system, do a vertical line perpendicular to ground from roof GPS, set up intersection point O coordinate system X-Y-Z using ground intersection point O as initial point, this coordinate system X-Z axle is close to ground, X-axis is relative to car towards the right side, and Z axis points to the dead ahead of car, and Y-axis is perpendicular to sensing underground, ground; h _gpsfor roof GPS is relative to the height on ground;

The lower edge in described target rectangle block region is in ground, and the top left co-ordinate in target rectangle block region is u, v, and the frame in target rectangle block region is wide=w, and frame is high=h;

For the pose transformation matrix R of ground intersection point _picture-Owith the displacement transition matrix T of camera relative to ground intersection point _picture-O, decompose the matrix H obtaining 3 × 3 _o,

H _o3x3=sK [r _{picture-Che 1}r _{picture-Che 3}-R _picture-Chet _picture-O]=s [r _{picture-Che 1}r _{picture-Che 3}-R _picture-Che(T _picture-Che+ T _car-O)]

Wherein, s is Arbitrary Coefficient, and K is camera internal reference matrix, R _picture-Chethe pose transformation matrix of camera relative to car, r _{picture-Che 1}and r _{picture-Che 3}be respectively matrix R _picture-Chethe 1st row and the 3rd arrange, T _picture-Chethe displacement transition matrix of camera relative to car, T _car-Othe displacement transition matrix of car relative to ground intersection point;

If the object space three-dimensional coordinate in intersection point O coordinate system X-Y-Z is (X, Y, Z), by ground intersection point as coordinate u+w/2, v+h calculates X, and Z is as follows,

[X Z 1] ^T＝zH _O ^-1[u+w/2 v+h 1] ^T

Wherein, z is unknowm coefficient, is solved by above-mentioned equation row 3 equations;

Gained X, Z are then [X-h relative to car gps coordinate _gpsz], [X-h _gpsz] be converted to again and measure city coordinate system, finally conversion obtains ground latitude and longitude coordinates to terrestrial coordinate system.

2. based on a monocular positioning system for installed video, it is characterized in that: comprise off-line part and online part,

Off-line part, comprises the training module for training objective master drawing, for setting up the classifier modules in object classifiers storehouse;

Online part, comprises with lower module,

Detection module, detects the interesting target in input picture for based target sorter storehouse, obtains respective objects rectangular block region; Monocular locating module, for the target for type of ground objects, carries out ground monocular according to respective objects rectangular block region and locates the coordinate obtained in three-dimensional scenic, finally convert the longitude and latitude of target to; The target of described type of ground objects, a certain lateral edges ground connection of feeling the pulse with the finger-tip target;

Monocular location, described ground implementation is as follows,

Set up road surface intersection point coordinate system, do a vertical line perpendicular to ground from roof GPS, set up intersection point O coordinate system X-Y-Z using ground intersection point O as initial point, this coordinate system X-Z axle is close to ground, X-axis is relative to car towards the right side, and Z axis points to the dead ahead of car, and Y-axis is perpendicular to sensing underground, ground; h _gpsfor roof GPS is relative to the height on ground;

The lower edge in described target rectangle block region is in ground, and the top left co-ordinate in target rectangle block region is u, v, and the frame in target rectangle block region is wide=w, and frame is high=h;

For the pose transformation matrix R of ground intersection point _picture-Owith the displacement transition matrix T of camera relative to ground intersection point _picture-O, decompose the matrix H obtaining 3 × 3 _o,

H _o3x3=sK [r _{picture-Che 1}r _{picture-Che 3}-R _picture-Chet _picture-O]=s [r _{picture-Che 1}r _{picture-Che 3}-R _picture-Che(T _picture-Che+ T _car-O)]

Wherein, s is Arbitrary Coefficient, and K is camera internal reference matrix, R _picture-Chethe pose transformation matrix of camera relative to car, r _{picture-Che 1}and r _{picture-Che 3}be respectively matrix R _picture-Chethe 1st row and the 3rd arrange, T _picture-Chethe displacement transition matrix of camera relative to car, T _car-Othe displacement transition matrix of car relative to ground intersection point;

If the object space three-dimensional coordinate in intersection point O coordinate system X-Y-Z is (X, Y, Z), by ground intersection point as coordinate u+w/2, v+h calculates X, and Z is as follows,

[X Z 1] ^T＝zH _O ^-1[u+w/2 v+h 1] ^T

Wherein, z is unknowm coefficient, is solved by above-mentioned equation row 3 equations;

Gained X, Z are then [X-h relative to car gps coordinate _gpsz], [X-h _gpsz] be converted to again and measure city coordinate system, finally conversion obtains ground latitude and longitude coordinates to terrestrial coordinate system.