CN103413352A - Scene three-dimensional reconstruction method based on RGBD multi-sensor fusion - Google Patents

Abstract

The invention discloses a scene three-dimensional reconstruction method based on RGBD multi-sensor fusion. The method is used for solving the technical problem that an existing scene three-dimensional reconstruction method is low in reconstruction speed. According to the technical scheme, firstly, a SURF algorithm is used for replacing a SIFT algorithm to carry out feature matching; then, the division standard of a sub-map is improved according to the method of an observation center, an image pyramid after multilayer image abstraction is made to reflect the topological structure of an environment better, and the improvement of the speed of constructing a large-scale scene three-dimensional map is facilitated better. Meanwhile, in a map generating part, a classical multiframe fusion method based on a space body is used for reducing redundant points, and the reconstruction speed is improved. As is tested, the reconstruction speed of the method is three times of that of the background technology.

Description

Scene three-dimensional rebuilding method based on the RGBD Multi-sensor Fusion

Technical field

The present invention relates to a kind of scene three-dimensional rebuilding method, particularly a kind of three-dimensional rebuilding method of scene based on the RGBD Multi-sensor Fusion.

Background technology

In the autonomous mobile robot field, with setting up, be all a study hotspot to the exploration of circumstances not known all the time.Existing method for reconstructing three-dimensional scene mainly contains: based on the infrared laser sensor with based on the large class of vision sensor two.

Document " RGB-D mapping:Using depth cameras for dense 3D modeling of indoor environments, 12th ISER. 2010,20:p22-25 " discloses a kind of figure of TORO based on RGB-D sensor optimized algorithm.The method adopts the SIFT algorithm to carry out characteristic matching, obtain the initial estimation with respect to the present frame pose of the first frame, and then use the ICP algorithm to carry out point cloud matching to optimize initial estimation, when loop closure being detected, all frames of input are added in pose figure and carry out global optimization, thereby obtain high-precision map.But aspect time cost, characteristic matching is to adopt the general SIFT algorithm of computing velocity, can not meet well real-time three-dimensional and rebuild the requirement to real-time speed, therefore, should adopt the algorithm that speed is higher when characteristic matching, to improve real-time.Aspect map optimization, because the track of mobile device only builds map centered by a position, cause the generation of area of absence, therefore can not set up complete map; Simultaneously, when output 3D point cloud chart, only, by the simple addition of the cloud data of each frame, cause the increase of redundant points, not only reduced arithmetic speed, and brought very big interference for follow-up processing again.

Summary of the invention

In order to overcome existing scene three-dimensional rebuilding method, rebuild the low deficiency of speed, the invention provides a kind of three-dimensional rebuilding method of scene based on the RGBD Multi-sensor Fusion.At first the method adopts the SURF algorithm to replace the SIFT algorithm to carry out characteristic matching, secondly, according to the observation the method improvement at center the criteria for classifying of sub-map, make image pyramid after multilayer figure is abstract better reflect the topological structure of environment, more help to improve the speed that builds the large scene three-dimensional map.Simultaneously, in the map generating portion, proposed to reduce redundant points by the classical fusion method of the multiframe based on space body, not only can improve reconstruction speed, and provide data more accurately for subsequent operation.

The technical solution adopted for the present invention to solve the technical problems is: a kind of three-dimensional rebuilding method of scene based on the RGBD Multi-sensor Fusion is characterized in comprising the following steps:

Step 1, employing SURF algorithm carry out feature extracting and matching.The extreme value of the Hessian determinant of a matrix that the extraction and application calculating of feature is approximate is determined the position of unique point.Input piece image I, to scheming upper each pixel x=(x, y), utilize formula (1) to calculate the Hessian matrix H (X, σ) that it at x place yardstick is.

$H(x,\mathrm{\σ})=\left[\begin{array}{cc}{L}_{\mathrm{xx}}(x,\mathrm{\σ})& L_{\mathrm{xy}}(x,\mathrm{\σ})\\ L_{\mathrm{xy}}(x,\mathrm{\σ})& L_{\mathrm{yy}}(x,\mathrm{\σ})\end{array}\right]---\left(1\right)$

Wherein, L _Xx(x, σ) is the Gaussian second-order partial differential coefficient

Convolution at the x place to image I.Then, calculate the determinant of each H:

det(H _approx)=D _xxD _yy-(0.9D _xy) ² (2)

Wherein, D _XxBe the convolution of box masterplate and image, use D _XxThe approximate L that replaces _Xx.Then, the symbol of judgement determinant (2).If symbol is not Local Extremum for negative; If symbol is for just, this point classifies as extreme point.

Then, the metric space of design of graphics picture, SURF are asked for the response image of Hessian matrix determinant as filtering template and integral image by the different size box, then, on response image, adopt the non-maximum value of 3D to suppress, and ask for the spot of each different scale.Use 3 * 3 * 3 masterplate in three dimensions, to carry out non-maximum value inhibition, according to default Hessian threshold value H relatively, when h is greater than H, and the analog value of neighbour near 26 points is chosen as unique point when all large, then adopts three-dimensional linear interpolation to make it more accurate.

Centered by unique point, take 6s in the circle of radius, image is carried out to Haar small echo response computing, design one centered by direction, subtended angle is the fan-shaped moving window of PI/3d, and the step-length of take is 0.2 radian, rotates moving window, and in window Haar response a little add up, principal direction is direction corresponding to maximum Haar response accumulated value.

Carry out the coupling of unique point.

1) when calculating the H determinant of a matrix, calculate simultaneously matrix trace.By the brightness difference, unique point is divided into to two kinds: 1. unique point and the brightness of small neighbourhood on every side thereof are greater than background area, and the H matrix trace is for just; 2. unique point and the brightness of small neighbourhood on every side thereof are less than background area, and the H matrix trace is for negative.First the H matrix trace of two unique points compared: jack per line illustrates that two unique points have same contrast; Contrary sign, illustrate the contrast difference, abandons step 2) similarity measurement;

2), on the basis of step 1), to the similarity measurement of two unique point descriptors, adopt Euclidean distance to calculate:

${\mathrm{Dis}}_{\mathrm{ij}}={\left[\begin{array}{c}\underset{k=0}{\overset{k=n}{\mathrm{\∑}}}{(X_{\mathrm{ik}}-X_{\mathrm{jk}})}^2\end{array}\right]}^{1/2}---\left(3\right)$

Wherein, X _IkK the element that means i Feature Descriptor in figure subject to registration, X _JkK the element that means j Feature Descriptor in reference diagram, the dimension of n representation feature vector.

For the unique point on figure subject to registration, calculate its Euclidean distance to all unique points on reference picture, obtain a distance set.By the set of adjusting the distance, compare computing and obtain little Euclidean distance and time Euclidean distance.Setting threshold, when the ratio of minimum euclidean distance and time Euclidean distance is less than this threshold value, think that unique point mate with the unique point of corresponding minimum euclidean distance, otherwise, not and this unique point be not complementary.

Step 2, the three-dimensional reconstruction point cloud data registration problems of estimating and optimizing for pose, first adopt the RANSAC registration Algorithm slightly to mate, and carry out initial pose estimation in conjunction with depth information, to calculate the initial value of result as the ICP registration Algorithm, re-use depth information and carry out the ICP algorithm optimization of initial pose is realized to registration from coarse to fine.

1) utilize the RANSAC algorithm to mark off point not in the know and intra-office point, reject the coupling abnormity point, the random foundation point of point in the owning administration obtained is converged and closes, first slightly mate, if two point sets that obtain are respectively S and T, from set of source data S, randomly drawing not three point { S point-blank _a, S _b, S _c, in target point set T, three point { T that search is corresponding _a, T _b, T _c, utilize three pairs of reference mark to carry out the estimation of European transformation matrix, calculate H _c.Then calculate and estimating transformation relation H _cUnder error threshold δ, the consistent degree between two point sets, and utilize the depth information of corresponding point to obtain the estimation of initial pose;

2) using the result of step 1) as the input of ICP algorithm, avoid monotony and converge to local minimum, estimate registration parameter according to minimizing objective function<maths TranNum="93" num="0003"><![CDATA[<math> <mrow> <msub> <mi>H</mi> <mi>k</mi> </msub> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>R</mi> <mi>k</mi> </msub> </mtd> <mtd> <msub> <mi>T</mi> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow></math>]]></maths>And calculating evaluated error d<sub TranNum="94">k</sub>Use transformation matrix H<sub TranNum="95">k</sub>To data set P<sub TranNum="96">k</sub>Upgrade P<sub TranNum="97">K+1</sub>=H<sub TranNum="98">k</sub>(P<sub TranNum="99">0</sub>)=R<sub TranNum="100">k</sub>P<sub TranNum="101">O</sub>+ T<sub TranNum="102">k</sub>; Iteration, when the evaluated error of twice calculating changes while being less than threshold tau, i.e. ‖ d<sub TranNum="103">k</sub>-d<sub TranNum="104">K+1</sub>‖<during τ, stops;

Step 3, obtain the initial pose through optimizing after, using it as node, the position orientation relation of the interframe be adjacent adds in figure as limit, and then adopts Hog-man figure optimized algorithm to carry out the global optimization of map, obtains the high accuracy three-dimensional map.

The distance value of camera being observed to center is divided into a plurality of subgraphs as standard by original figure, with a node in subgraph, mean this subgraph, obtain the abstract of original graph one deck, carry out successively more abstract to the figure obtained, just obtain a multilayer graph structure, extract the topological structure in original graph.

$\mathrm{IP}=P{\left[\begin{array}{cccc}0& d& 0& 1\end{array}\right]}^T---\left(4\right)$

Wherein, O is the video camera center, and IP is the center of interest point, and d is the distance of IP to O.

When having new node to join in figure, at first be to join in original graph, then check that whether changing subgraph divides, if change, just need to upgrade high-level diagram, simultaneously top layer is optimized, when only having the topological structure of working as top layer that very large change occurs, ability to bottom figure back transfer, is upgraded bottom figure by top level diagram.

When the multiframe cloud data was merged, space body that comprises all frames of model, then processed each frame successively, revises the value of aerial image vegetarian refreshments.Null value pixel in last space body is the point on last curved surface, and the curved surface obtained like this is the curved surface with least square character.Adopt weight function to carry out the distance value stack, last stack equation such as formula (5), formula (6).

$D_{i+1}\left(x\right)=\frac{W_1\left(x\right)D_1\left(x\right)+W_{i-1}\left(x\right)D_{i-1}\left(x\right)}{W_1\left(x\right)+W_{i-1}\left(x\right)}---\left(5\right)$

W _i+1(x)=W _i(x)+W _i-1(x) (6)

$W\left(x\right)=\frac{{x-x}^{*}}{\mathrm{Err}\left(x^{*}\right)}---\left(7\right)$

Err(dist)=dist ²×0.0035 (8)

According to error equation (8) weight function of Kinect sensor, choose formula (7).Wherein, x * is the distance to image center of putting on the single frames curved surface, finally obtains the three-dimensional reconstruction design sketch of whole scene.

The invention has the beneficial effects as follows: at first the method adopts the SURF algorithm to replace the SIFT algorithm to carry out characteristic matching, secondly, according to the observation the method improvement at center the criteria for classifying of sub-map, make image pyramid after multilayer figure is abstract better reflect the topological structure of environment, more help to improve the speed that builds the large scene three-dimensional map.Simultaneously, in the map generating portion, proposed to reduce redundant points by the classical fusion method of the multiframe based on space body, improved reconstruction speed.After tested, the reconstruction speed of the inventive method has improved nearly twice than background technology.

Below in conjunction with embodiment, the present invention is elaborated.

Embodiment

The scene three-dimensional rebuilding method concrete steps that the present invention is based on the RGBD Multi-sensor Fusion are as follows:

1, feature extracting and matching.

Adopt the SURF algorithm to carry out feature extracting and matching.The extreme value of the Hessian determinant of a matrix that the extraction and application calculating of feature is approximate is determined the position of unique point.Input piece image I, to scheming upper each pixel x=(x, y), utilizing formula (1) to calculate it is the Hessian matrix H (X, σ) of σ at x place yardstick.

$H(x,\mathrm{\&sigma;})=\left[\begin{array}{cc}{L}_{\mathrm{xx}}(x,\mathrm{\&sigma;})& L_{\mathrm{xy}}(x,\mathrm{\&sigma;})\\ L_{\mathrm{xy}}(x,\mathrm{\&sigma;})& L_{\mathrm{yy}}(x,\mathrm{\&sigma;})\end{array}\right]---\left(1\right)$

Wherein, L _Xx(x, σ) is the Gaussian second-order partial differential coefficient

Convolution at the x place to image I.Then, calculate the determinant of each H:

det(H _approx)=D _xxD _yy-(0.9D _xy) ² (2)

Wherein, D _XxBe the convolution of box masterplate and image, use D _XxClosely to replace L _Xx.Then, the symbol of judgement determinant (2).If symbol is not Local Extremum for negative; If symbol is for just, this point classifies as extreme point.

Then, the metric space of design of graphics picture, the i.e. expression of image under different resolutions.To set up pyramidal principle opposite with SIFT, SURF asks for the response image of Hessian matrix determinant as filtering template and integral image by the different size box, then, on response image, adopt the non-maximum value of 3D to suppress, ask for the spot of each different scale.Use 3 * 3 * 3 masterplate in three dimensions, to carry out non-maximum value inhibition, according to default Hessian threshold value H relatively, when h is greater than H, and the analog value of neighbour near 26 points is chosen as unique point when all large, then adopts three-dimensional linear interpolation to make it more accurate.

In order to guarantee that eigenvector has rotational invariance, need to distribute a principal direction to each unique point.Centered by unique point, take 6s in the circle of radius, image is carried out to Haar small echo response computing, design one centered by direction, subtended angle is the fan-shaped moving window of PI/3d, and the step-length of take is 0.2 radian, rotates moving window, and in window Haar response a little add up, principal direction is direction corresponding to maximum Haar response accumulated value.

Carried out after feature point extraction carrying out the coupling of unique point, had the following steps:

1) when calculating the H determinant of a matrix, calculate simultaneously matrix trace.Due to by the brightness difference, unique point is divided into to two kinds: 1. unique point and the brightness of small neighbourhood on every side thereof are greater than background area, and the H matrix trace is for just; 2. unique point and the brightness of small neighbourhood on every side thereof are less than background area, and the H matrix trace is for negative.Characteristic according to this, first the H matrix trace of two unique points compared: jack per line illustrates that two unique points have same contrast; Contrary sign, illustrate the contrast difference, abandons 2) similarity measurement;

2) 1) basis on, to the similarity measurement of two unique point descriptors, adopt Euclidean distance to calculate:

${\mathrm{Dis}}_{\mathrm{ij}}={\left[\begin{array}{c}\underset{k=0}{\overset{k=n}{\mathrm{\&Sum;}}}{(X_{\mathrm{ik}}-X_{\mathrm{jk}})}^2\end{array}\right]}^{1/2}---\left(3\right)$

Wherein, X _Ik; K element of i Feature Descriptor from mean figure subject to registration, X _JkK the element that means j Feature Descriptor in reference diagram, the dimension of n representation feature vector.

For the unique point on figure subject to registration, calculate its Euclidean distance to all unique points on reference picture, obtain a distance set.By the set of adjusting the distance, compare computing and obtain little Euclidean distance and time Euclidean distance.Setting threshold, be generally 0.8, when the ratio of minimum euclidean distance and time Euclidean distance is less than this threshold value, thinks that unique point mate with the unique point of corresponding minimum euclidean distance, otherwise, not and this unique point be not complementary.Threshold value is less, mates more stablely, but extreme point is fewer.

2, pose is estimated and is optimized.

Three-dimensional reconstruction point cloud data registration problems for pose estimation and optimization, in order to prevent that the ICP algorithm convergence is to local smallest point, produce wrong registration results, first adopt the RANSAC registration Algorithm slightly to mate, and carry out initial pose estimation in conjunction with depth information, the initial value of result as the ICP registration Algorithm will be calculated, reusing depth information carries out the ICP algorithm optimization of initial pose is realized to registration strategies from coarse to fine, and the minimizing iterations, reach gratifying convergence result.

Concrete steps are as follows:

1) utilize the RANSAC algorithm to mark off " point not in the know " and " intra-office point ", reject the coupling abnormity point, the random foundation point of point in the owning administration obtained is converged and closes, first slightly mate, if two point sets that obtain are respectively S and T, from set of source data S, randomly drawing not three point { S point-blank _a, S _b, S _c, in target point set T, three point { T that search is corresponding _a, T _b, T _c, utilize three pairs of reference mark to carry out the estimation of European transformation matrix, calculate H _c.Then calculate and estimating transformation relation H _cUnder error threshold δ, the consistent degree between two point sets, and utilize the depth information of corresponding point to obtain the estimation of initial pose;

2) using 1) result that calculates is as the input of ICP algorithm, avoids monotony and converges to local minimum, estimates registration parameter according to minimizing objective function $H_k=\left[\begin{array}{cc}{R}_k& T_k\\ 0& 1\end{array}\right],$ And calculating evaluated error d _k; Use transformation matrix H _kTo data set P _kUpgrade P _K+1=H _k(P ₀)=R _kP ₀+ T _k; Iteration, when the evaluated error of twice calculating changes while being less than threshold tau, i.e. ‖ d _k-d _K+1During ‖<τ, stop;

3, the Hog-man map is optimized.

After obtaining the initial pose through optimizing, using it as node, the position orientation relation of the interframe be adjacent adds in figure as limit, and then adopts Hog-man figure optimized algorithm to carry out the global optimization of map, obtains the high accuracy three-dimensional map.

Due to when wanting to set up the complete information in zone, gather the foundation that more point belongs to a kind of enclosed around can be in this piece zone.Therefore, core concept is as standard, original figure to be divided into to a plurality of subgraphs according to the distance value (suc as formula (5)) at camera observation center, with a node in subgraph, mean this subgraph, thereby obtain the abstract of original graph one deck, to the figure obtained, carry out successively more abstract, just obtain a multilayer graph structure, extract the topological structure in original graph.

$\mathrm{IP}=P{\left[\begin{array}{cccc}0& d& 0& 1\end{array}\right]}^T---\left(4\right)$

Wherein, establishing O is the video camera center, and IP is the center of interest point, and d is the distance of IP to O, gets 2m here.The pose of supposing current camera means with the matrix P of 4 * 4, and the coordinate of IP is for using the observation center to replace image center as the subgraph segmentation standard, the node characteristic distributions when complete map is set up in reaction better.

When having new node to join in figure, at first be to join in original graph, then check that whether changing subgraph divides, if change, just need to upgrade high-level diagram, simultaneously top layer is optimized, when only having the topological structure of working as top layer that very large change occurs, ability to bottom figure back transfer, is upgraded bottom figure by top level diagram.So just guaranteed real-time optimization.

According to the optimum results obtained, carry out simple superposition and obtain the redundant points of bringing in order to remove all frames of direct stack, the cloud of the point based on the space body fusion method of employing.Its main thought is when the multiframe cloud data is merged, space body that comprises all frames of model, space body is divided into the aerial image vegetarian refreshments with certain resolution, on each point, will be recorded to the distance of object in environment, then successively each frame is processed, revised the value of aerial image vegetarian refreshments.Null value pixel in last space body is the point on last curved surface, and the curved surface obtained like this is the curved surface with least square character.In order to reflect the Some features of the every frame data that collect, when carrying out the distance value stack, adopt weight function simultaneously, last stack equation is suc as formula (5), formula (6).

$D_{i+1}\left(x\right)=\frac{W_1\left(x\right)D_1\left(x\right)+W_{i-1}\left(x\right)D_{i-1}\left(x\right)}{W_1\left(x\right)+W_{i-1}\left(x\right)}---\left(5\right)$

W _i+1(x)＝Ｗ _i(x)+W _i-1(x) (６)

$W\left(x\right)=\frac{{x-x}^{*}}{\mathrm{Err}\left(x^{*}\right)}---\left(7\right)$

Err(dist)＝dist ²×0.0035 (8)

In experiment, use the kinet camera as the RGBD camera, therefore, according to choosing as formula (7) of error equation (8) weight function of Kinect sensor.Wherein x* is the distance to image center of putting on the single frames curved surface, finally obtains the 3D point cloud chart, i.e. the three-dimensional reconstruction design sketch of whole scene.

Claims (1)

1. three-dimensional rebuilding method of the scene based on the RGBD Multi-sensor Fusion is characterized in that comprising the following steps:

Step 1, employing SURF algorithm carry out feature extracting and matching; The extreme value of the Hessian determinant of a matrix that the extraction and application calculating of feature is approximate is determined the position of unique point; Input piece image I, to scheming upper each pixel x=(x, y), utilizing formula (1) to calculate it is the Hessian matrix H (X, σ) of σ at x place yardstick;

$H(x,\mathrm{\&sigma;})=\left[\begin{array}{cc}{L}_{\mathrm{xx}}(x,\mathrm{\&sigma;})& L_{\mathrm{xy}}(x,\mathrm{\&sigma;})\\ L_{\mathrm{xy}}(x,\mathrm{\&sigma;})& L_{\mathrm{yy}}(x,\mathrm{\&sigma;})\end{array}\right]---\left(1\right)$

Wherein, L _Xx(x, σ) is the Gaussian second-order partial differential coefficient

Convolution at the x place to image I; Then, calculate the determinant of each H:

det(H _approx)=D _xxD _yy-(0.9D _xy) ² (2)

Wherein, D _XxBe the convolution of box masterplate and image, use D _XxThe approximate L that replaces _Xx; Then, the symbol of judgement determinant (2); If symbol is not Local Extremum for negative; If symbol is for just, this point classifies as extreme point;

Then, the metric space of design of graphics picture, SURF are asked for the response image of Hessian matrix determinant as filtering template and integral image by the different size box, then, on response image, adopt the non-maximum value of 3D to suppress, and ask for the spot of each different scale; Use 3 * 3 * 3 masterplate in three dimensions, to carry out non-maximum value inhibition, according to default Hessian threshold value H relatively, when h is greater than H, and the analog value of neighbour near 26 points is chosen as unique point when all large, then adopts three-dimensional linear interpolation to make it more accurate;

Centered by unique point, take 6s in the circle of radius, image is carried out to Haar small echo response computing, design one centered by direction, subtended angle is the fan-shaped moving window of PI/3d, and the step-length of take is 0.2 radian, rotates moving window, and in window Haar response a little add up, principal direction is direction corresponding to maximum Haar response accumulated value;

Carry out the coupling of unique point;

1) when calculating the H determinant of a matrix, calculate simultaneously matrix trace; By the brightness difference, unique point is divided into to two kinds: 1. unique point and the brightness of small neighbourhood on every side thereof are greater than background area, and the H matrix trace is for just; 2. unique point and the brightness of small neighbourhood on every side thereof are less than background area, and the H matrix trace is for negative; First the H matrix trace of two unique points compared: jack per line illustrates that two unique points have same contrast; Contrary sign, illustrate the contrast difference, abandons step 2) similarity measurement;

2), on the basis of step 1), to the similarity measurement of two unique point descriptors, adopt Euclidean distance to calculate:

${\mathrm{Dis}}_{\mathrm{ij}}={\left[\begin{array}{c}\underset{k=0}{\overset{k=n}{\mathrm{\&Sum;}}}{(X_{\mathrm{ik}}-X_{\mathrm{jk}})}^2\end{array}\right]}^{1/2}---\left(3\right)$

Wherein, X _IkK the element that means i Feature Descriptor in figure subject to registration, X _JkK the element that means j Feature Descriptor in reference diagram, the dimension of n representation feature vector;

For the unique point on figure subject to registration, calculate its Euclidean distance to all unique points on reference picture, obtain a distance set; By the set of adjusting the distance, compare computing and obtain little Euclidean distance and time Euclidean distance; Setting threshold, when the ratio of minimum euclidean distance and time Euclidean distance is less than this threshold value, think that unique point mate with the unique point of corresponding minimum euclidean distance, otherwise, not and this unique point be not complementary;

Step 2, the three-dimensional reconstruction point cloud data registration problems of estimating and optimizing for pose, first adopt the RANSAC registration Algorithm slightly to mate, and carry out initial pose estimation in conjunction with depth information, to calculate the initial value of result as the ICP registration Algorithm, re-use depth information and carry out the ICP algorithm optimization of initial pose is realized to registration from coarse to fine;

1) utilize the RANSAC algorithm to mark off point not in the know and intra-office point, reject the coupling abnormity point, the random foundation point of point in the owning administration obtained is converged and closes, first slightly mate, if two point sets that obtain are respectively S and T, from set of source data S, randomly drawing not three point { S point-blank _a, S _b, S _c, in target point set T, three point { T that search is corresponding _a, T _b, T _c, utilize three pairs of reference mark to carry out the estimation of European transformation matrix, calculate H _cThen calculate and estimating transformation relation H _cUnder error threshold δ, the consistent degree between two point sets, and utilize the depth information of corresponding point to obtain the estimation of initial pose;

2) using the result of step 1) as the input of ICP algorithm, avoid monotony and converge to local minimum, estimate registration parameter according to minimizing objective function $H_k=\left[\begin{array}{cc}{R}_k& T_k\\ 0& 1\end{array}\right],$ And calculating evaluated error d _k; Use transformation matrix H _kTo data set P _kUpgrade P _K+1=H _k(P ₀)=R _kP ₀+ T _k; Iteration, when the evaluated error of twice calculating changes while being less than threshold tau, i.e. ‖ d _k-d _K+1During ‖<τ, stop;

Step 3, obtain the initial pose through optimizing after, using it as node, the position orientation relation of the interframe be adjacent adds in figure as limit, and then adopts Hog-man figure optimized algorithm to carry out the global optimization of map, obtains the high accuracy three-dimensional map;

The distance value of camera being observed to center is divided into a plurality of subgraphs as standard by original figure, with a node in subgraph, mean this subgraph, obtain the abstract of original graph one deck, carry out successively more abstract to the figure obtained, just obtain a multilayer graph structure, extract the topological structure in original graph;

$\mathrm{IP}=P{\left[\begin{array}{cccc}0& d& 0& 1\end{array}\right]}^T---\left(4\right)$

Wherein, O is the video camera center, and IP is the center of interest point, and d is the distance of IP to O;

When having new node to join in figure, at first be to join in original graph, then check that whether changing subgraph divides, if change, just need to upgrade high-level diagram, simultaneously top layer is optimized, when only having the topological structure of working as top layer that very large change occurs, ability to bottom figure back transfer, is upgraded bottom figure by top level diagram;

When the multiframe cloud data was merged, space body that comprises all frames of model, then processed each frame successively, revises the value of aerial image vegetarian refreshments; Null value pixel in last space body is the point on last curved surface, and the curved surface obtained like this is the curved surface with least square character; Adopt weight function to carry out the distance value stack, last stack equation such as formula (5), formula (6);

$D_{i+1}\left(x\right)=\frac{W_1\left(x\right)D_1\left(x\right)+W_{i-1}\left(x\right)D_{i-1}\left(x\right)}{W_1\left(x\right)+W_{i-1}\left(x\right)}---\left(5\right)$

W _i+1(x)=W _i(x)+W _i-1(x) (6)

$W\left(x\right)=\frac{{x-x}^{*}}{\mathrm{Err}\left(x^{*}\right)}---\left(7\right)$

E _rr(dist)-dist ²×0.0035 (8)

According to error equation (8) weight function of Kinect sensor, choose formula (7); Wherein, x* is the distance to image center of putting on the single frames curved surface, finally obtains the three-dimensional reconstruction design sketch of whole scene.