Reconstruction of Rigid Body Models from Motion Distorted Laser Range Data Using Optical Flow

Transcription

1 Reconstructon of Rgd Body Models from Moton Dstorted Laser Range Data Usng Optcal Flow Eddy Ilg Raner Kümmerle Wolfram Burgard Thomas Brox Abstract The setup of tltng a 2D laser range fnder up and down s a wdespread strategy to acqure 3D pont clouds. Ths setup requres that the scene s statc whle the robot takes a 3D scan. If an object moves through the scene durng the measurement process and one does not take nto account these movements, the resultng model wll get dstorted. Ths paper presents an approach to reconstruct the 3D model of a movng rgd object from the nconsstent set of 2D measurements by the help of a camera. Our approach utlzes optcal flow n the camera mages to estmate the moton n the mage plane and pont-lne constrants to compensate the mssng nformaton about the moton n depth. We combne multple sweeps and/or vews nto to a sngle consstent model usng a pont-to-plane ICP approach and optmze sngle sweeps by smoothng the resultng trajectory. Experments obtaned n real outdoor scenaros wth movng cars demonstrate that our approach yelds accurate models. noddng 2D laser range fnder camera I. INTRODUCTION Compared to stereo cameras, laser range fnders have become popular devces for the acquston of 3D pont clouds n large-scale scenaros because of ther ablty to obtan accurate long-range measurements [1]. A wdely used and cost-effectve setup s to mount a 2D range scanner on a tltng actuator. By noddng the laser scanner ths approach s able to generate accurate models of statc scenes. However, f an object moves durng an entre sweep, whch wth typcal confguratons takes one second or even more, the ndvdual measurements wll not be consstent (see Fgure 1a). In ths paper, we present an approach that utlzes the optcal flow calculated from mages grabbed durng the pont cloud acquston and combnes t wth the laser measurements to estmate the movement of the correspondng object. Accordng to the estmated movement, t then calculates an accurate model of ths object (see Fgures 1b and 1c). The key dea of our approach s to estmate and perform a compensaton for the moton of the movng object. To acheve ths, we employ a vdeo camera that we calbrate to the noddng laser range fnder. As the camera operates at a hgher framerate compared to the 3D pont cloud acquston (1 Hz vs. 1 Hz), our approach estmates the moton of the object n the mage plane from frame to frame usng optcal flow [2] and employs ths nformaton to reproject the ndvdual 2D laser range scanlnes accordng to the movement of the object. Ths, however, requres that we estmate the depth of the ndvdual mage pxels after the moton. To acheve ths, we assume that the perceved object All authors are wth the Unversty of Freburg. Ths work has been partally supported by DFG grant BR 3815/5-1 and the EC under contract numbers ERC LfeNav, FP7-689-ROVINA, and FP EUROPA2. Fg. 1. Top vew of a bulk pont cloud recorded durng 15 sweeps of a noddng 2D laser range fnder of a car movng along the trajectory ndcated by red arrows. The car used as movng object. (c) The colored pont cloud model reconstructed from the dstorted pont cloud above. s rgd. If, at one nstant of tme, we know a partal model of the rgd body, the optcal flow provdes pont-lne constrants for the pose of ths model at another nstant of tme. As a mnmum model we have a non-degenerated sngle scanlne (recorded n one rotaton of the mrror deflectng the laser), whch we assume to be recorded nstantaneously. Trackng the pose of ths model and addng more scanlnes over tme allows for reconstructng the model of the rgd object. Pose trackng wth optcal flow naturally accumulates errors (drft), especally n the vewng drecton of the camera. Therefore, as soon as we record a new sweep of the object, we use the model from ths sweep as a reference and regster the old model to t usng an ICP approach. Ths restrcts drft to the tme wndow between two sweeps. The model recorded durng one sweep s dense n the drecton of the rotaton of the mrror of the laser range fnder, but sparse n the noddng drecton. Due to ths sparseness, the algnment of sweeps s not an easy task. We estmate the underlyng surface of the exstng (possbly denser) model by estmatng ts normals and regster the ponts of the newly recorded sweep by mnmzng ther pont-to-plane dstances to ths surface. To reduce drft, we furthermore constran the moton of the object to the ground plane. Furthermore, we mprove the trackng result by smoothng t wth the algnment determned by ICP. Ths allows us to (c)

2 reduce the nfluence of the drft on the qualty of the model estmated by our approach. Afterwards, we use the smooth trajectory to recalculate the pont cloud of the current sweep before ntegratng t nto the model. II. RELATED WORK The typcal methods to obtan 3D data wth laser range fnders can be roughly dvded nto two categores. The frst operates wth laser range fnders that are rgdly connected to a movng platform, such as a robot [3]. These methods requre a precse estmate of the poston of the robot to obtan an accurate model. The second category actvely actuates the laser range fnder ether by noddng or rotatng t [4], where the laser range fnder tself may have a sngle or multple beams. Note that there are also exceptons. For example, Zbedee by Bosse et al. [5] employs a sprngmounted laser range fnder whch s passvely actuated due to the vbratons nduced by drvng over non-flat surfaces. Obvously, the data of such a 3D scannng devce can be used for varous tasks ncludng object reconstructon or mappng an envronment. In addton to the categores mentoned above, dedcated approaches for model acquston have been proposed. A popular approach to obtan the model s to use a robot for movng the sensor around the object [6], [7]. Krann et al. [8] suggested a method that uses a robotc arm to grasp the object and to move t n front of a depth camera. In contrast to our approach, these methods ether obtan a model of a non-movng object or move the object by themselves, whereas our approach consders the data of a contnuously noddng 2D laser range fnder to obtan a 3D model of a rgd object, whch s movng along an unknown trajectory. Blas et al. [9] present an approach that explctly takes nto account that the scanned object moves. Ther approach teratvely refnes the model but reles on a Lssajous pattern to obtan the range data. Wese et al. [1] propose an approach to correct the dstorton for actve llumnaton stereo n short tme frames. Instead of obtanng a model of an object by scannng a sngle object from multple vews, Ruhnke et al. [11] explot the presence of multple nstances of the objects n the scene to obtan complete 3D models. Ther approach apples spectral clusterng to merge the ndvdual nstances nto one object model. A related problem n computer vson s 3D reconstructon from a movng camera n the presence of ndependently movng objects [12]. Whle a statc scene would come down to a structure-from-moton problem, a movng rgd object requres segmentaton of the object and estmaton of the relatve rgd moton. Our approach combnes the data of a laser range fnder wth vson. Ths combnaton has been appled successfully for other applcatons, such as extendng the range for terran classfcaton [13] or ncreasng the resoluton of the range data [14]. Held et al. [15] apply upsamplng to the sparse range data of a Velodyne scanner. They show that the denser range data leads to better velocty estmates for trackng a movng object. In contrast to our approach, ther method Fg. 2. The calbraton pattern n the camera data and n the remsson data of the laser range fnder. Fg. 3. The deal model of the calbraton pattern. The red dots ndcate the corners of the pattern. The error functon used to detect the rgd n-plane transformaton. Darker areas ndcate lower error. receves a full 3D scan wth 1 Hz, whereas we receve such a scan at only 1 Hz and scanlne-by-scanlne. We explot that the camera operates at a much hgher frame rate than our noddng 2D laser range fnder and track the object n the mage space to account for the dstorton. III. CALIBRATION AND SYNCHRONIZATION A. Calbraton For calbratng the tltng laser range scanner and the camera we employ the calbraton pattern depcted n Fgure 2. To have a dstnct pattern that can be well observed by both the laser range fnder and the camera, the brght areas consst of hghly reflectve materal conductng a scatterng of the laser beam. Ths results n low remsson values (black patches n Fgure 2b) and makes t possble to detect the correspondng areas n the 3D scan. For calbraton, we nod the laser range fnder at a very slow velocty and record multple sweeps to obtan hgh resoluton scans. To establsh a camera projecton matrx P, whch maps the 3D ponts to mage ponts, we are nterested n fndng the set of correspondng patch corners n mage and 3D space. In the mage we compute the ntersectons of the lnes detected from the edges of the patches. In 3D space, we frst ft a plane through the ponts p wth low remsson values (black patches n Fgure 2b) usng a RANSAC approach. Gven ths plane we then nterpolate corners of the patches by fttng an

3 Rotaton Axs r Screw Moton (Twst) p 3 Translaton v p 2 p 1 Fg. 5. Illustraton of a twst ξ. The moton corresponds to a rotaton around the axs ω (red) and a translaton along v (blue) resultng n the screw moton from p 1 to p 3 (green). Rotaton Fg. 4. Calbraton object used to determne the tme shft between the laser range fnder and the camera. Green dots ndcate ponts of the object detected by the laser range fnder due to ther remsson value. The whte arrows ndcate the drecton of movement. Synchronzed laser range fnder and camera data of a movng object. deal model (see Fgure 3a) of the calbraton pattern to the data. To ths end, we defne an error functon E( ) (see Fgure 3b) for the pont locatons. To determne the pose (R α, t) of the pattern on the prevously extracted plane we mnmze α,t N E (R α p + t) (1) =1 usng gradent descent, where R α s a 2D rotaton matrx wth angle α and t s a 2D translaton. Once we know the correspondng locatons of the corners n 3D space and the mage, we establsh a homography [16]. Snce the calbraton object s planar, we take multple rotated vews of the pattern to unquely determne the nternal parameters of the camera and the projecton matrx P. B. Synchronzaton To determne the tme shft between the laser range fnder and the tlt actuator, we explot that objects must have the same coordnates wthn subsequent upward and downward sweeps. Hence, we determne the angle between the ground planes n two subsequent sweeps and teratvely shft the tmestamps of the tlt actuator untl ths angle approaches zero. By movng another calbraton object at hgh veloctes from left to rght, we calbrate the tme shft between the laser and the camera. The surface of ths object (depcted n Fgure 4a) conssts of the same reflectve materal as the calbraton pattern descrbed before. For the synchronzaton, we do not nod the laser range fnder. We detect the pattern n the laser range fnder data by ts low remsson values and project the ponts to mage space (depcted as green dots n Fgure 4). Along the lne (shown n whte) extrapolated from these ponts, we search for nearby pxels wth the color of the calbraton object to determne ts locaton n the mage. For a gven scanlne, we then seek for a camera frame close n tme whch best matches the poston of the object. We store the dfference n tme between the scanlne and the mage and ft a normal dstrbuton to a hstogram over all values. We then use the mean of ths dstrbuton to shft the camera tmestamps to match the laser range fnder tmestamps. Fgure 4b shows a typcal result. IV. MODEL RECONSTRUCTION A. Pont-Lne Correspondences from Optcal Flow To compensate the moton, we assume that a partal model s known at one nstant of tme. In the begnnng, ths s the frst scanlne of the laser range fnder. Let x denote the 3D ponts of ths model. Wth the projecton matrx P from the prevous secton, these ponts can be projected nto the camera mage. We denote the ponts n the mage plane by x. We use large dsplacement optcal flow [2] to compute the dsplacements of x to the next camera frame. The dsplaced mage locaton x = x + restrcts x to the projecton ray of x. 3D lnes can be represented mplctly by so-called Plücker lnes [17]. Let L = (m, n ) be the Plücker lne wth a unt vector n and a moment m that corresponds to the projecton ray x. The Plücker lne representaton then yelds drectly the dstance of an arbtrary 3D pont x to L va [18]: d(l, x) = x n m. (2) Based on these pont-lne constrants, we estmate the sx degrees of freedom of the rgd body moton to move the 3D ponts to ther new poston at the tme when the current camera mage was taken. B. Pose Estmaton For a gven set of non-occluded ponts x we seek for a rgd body transformaton T = (R t) wth a rotaton R and a translaton t that mnmzes d(l, x ). For the purpose of pose estmaton, the twst representaton [19] of T s well suted. As llustrated n Fgure 5, a twst s a screw moton around a rotaton axs ω and a translaton v. Such a twst can be represented as a vector ξ = (ω 1, ω 2, ω 3, v 1, v 2, v 3 ) or n matrx form ˆω = ω 3 ω 2 ( ) ω 3 ω 1 ˆω v, ˆξ = ω 2 ω 1. (3) Multplyng ˆξ by a factor θ allows for an arbtrary scalng of the moton [2]. We can map a scaled twst to a transformaton matrx and back by takng the exponental or logarthm: 1 T = exp(θˆξ), ˆξ = log(t), (4) θ

4 whch can be computed effcently by the Rodrgues formula [19]. Here, we set θ = 1 and seek for a twst that mnmzes the dstance d(l, x ): ξ (exp(ˆξ) π ( x 1 )) 2 n m, (5) where π( ) denotes the projecton from homogeneous to Eucldean coordnates. To reduce drft, we restrct the moton to the ground plane, whch we detect usng RANSAC, by algnng the z-axs of the reference frame to the ground normal and by settng ω 1 =, ω 2 =, and v 3 =. (6) Eq. (5) states a non-lnear least squares problem, whch we solve wth the Gauss-Newton method,.e., we teratvely lnearze exp(ˆξ) I + ˆξ. Hence, Eq. (5) becomes a lnear system n each teraton: ξ ((I + ˆω) x + v) n m 2. (7) We map the twst correspondng to the soluton of ths lnear system to the correspondng transformaton matrx and apply t to the 3D ponts to perform the next teraton. C. Treatment of Outlers Both the estmaton of the optcal flow and the pose of the object are affected by resdual errors. Hence, repeatedly estmatng a new pose gven the prevous result leads to an accumulaton of errors,.e., a drft n the poston and the orentaton of the object. Close to the object boundary, ths can result n model ponts that need to be projected to a background pxel, where the optcal flow does not correspond to the moton of the model. Fgure 6a llustrates such a case. Another cause for such outlers can be local errors of the optcal flow. To be robust to such outlers, we apply robust statstcs and replace the squared error norm n Eq. (5) by a truncated Huber norm [21]. Ths s equvalent to teratvely reweghted least squares. Usng the Gauss-Newton method, we teratvely solve the lnear system of Eq. (7). Let us denote ths system as ξ A ξ b 2. (8) If the correspondence x, x s an outler, the resdual r (ξ) = A ξ b of ths pont s large. To reduce ts nfluence on the soluton ξ, we ntroduce the weght { f r (ξ) > τ w (ξ) =, (9) otherwse 1 r (ξ) +ε whch corresponds to replacng the quadratc norm by the truncated Huber norm wth truncaton at τ. The parameter ɛ lmts the weghts of ponts wth low resdual and, consequently, the condton number of the system matrx. We start wth w = 1 and then solve Eq. (8) by computng ξ k w (ξ k 1 ) A ξ k b 2 (1) n each teraton k. Fgure 6b shows the result of enablng teratvely weghted least squares. Fg. 6. Illustraton of the pose estmaton drft durng the reconstructon of the frst sx sweeps of a car movng to the rght. The area n red shows the optcal flow whle the ponts ndcate the projecton of the model and ts estmated pose. The drft leads to outlers (ndcated by the whte arrows). Applyng Eq. (9) weakens the nfluence of the outlers. Fg. 7. Illustraton of model mergng wth the frst two sweeps of a movng car. Intal msalgnment of the frst two sweeps. The frst sweep s shown n red and the second one n blue. The model after algnng the frst two sweeps wth our approach. One can also see that the sweeps are dense n the drecton of mrror rotaton and sparse n the drecton of noddng. D. Model Mergng Applyng the pose estmaton process descrbed above allows us to track the object and buld ts model by accumulatng ponts. As already mentoned, ths method produces drft. To keep the error of the drft bounded, we buld a new model for each sweep. As the pose of the most recently recorded model s most certan, we algn the formerly tracked model wth the most recent one. Afterwards, we merge the models to an accumulated one, whch s then algned and merged wth the next sweep n the same manner. Let Γ be the accumulated model and let Λ be the model of the most recent sweep. We want to regster both models n a common coordnate frame. To ths end, we consder the trackng result provded by optcal flow as an ntal guess for regsterng both models wth an ICP approach (see Fgure 7a). We, however, have to take nto account that the range data s sparse and that the scanlnes ht the object at dfferent heght levels. Applyng the commonly used pontto-pont metrc would algn the dense scanlnes and yeld sub-optmal results. Thus, we nstead consder a pont-toplane metrc. Consequently, we have to estmate the normal vectors. Snce the qualty of the estmate of a normal vector depends on the densty of the underlyng pont cloud, we compute the normal vectors on the accumulated model Γ, whch s n general denser than the newly recorded model Λ. To estmate the normal vector n of a pont p Γ, we

5 Tn 1 yn TA y1 y1 Fg. 8. The trajectory optmzaton. Image shows the pose estmates between two specfc poses y1 and yn. Blue crcles ndcate the estmated poses drectly after a laser sweep was completed. Red lnes ndcate pose updates due to optcal flow pose estmaton whle blue lnes ndcate pose updates due to an ICP algnment. Correspondng optmzed trajectory. frst fnd the neghbors wthn a certan radus. We compute the normal vector n as the Egenvector correspondng to the smallest Egenvalue of the covarance matrx of these neghborng ponts. Addtonally, we determne the sgn of the surface normal such that t ponts towards the camera. We furthermore assume that the surfaces of the captured objects are convex. Ths allows us to detect occlusons when the object s movng by dentfyng ponts whose normals do no longer pont n the drecton of the camera. Specfcally, we mark ponts as occluded f the angle between the vector from the pont to the camera and the normal exceeds 6. We do not consder occluded ponts n the pose estmaton step. A representaton of the tangental plane to a pont p wth normal n s n x d =, (11) where d = n p. Furthermore, n x d represents the dstance of a pont x to ths plane. We teratvely seek for the nearest neghbors q Λ to p bounded by a maxmum dstance dmax and algn Λ to Γ by mnmzng X 2 n (TA q ) d. (12) TA Representng TA as a twst (see Eq. (4)) allows us to apply the constrant of Eq. (6), namely that the object moves on the ground plane. Agan, we fnd the soluton of Eq. (12) usng the Gauss-Newton method analogously to the pose estmaton problem descrbed before. Fgure 7b depcts the outcome of our regstraton approach. We found that the robustness of the algnment of the sparse clouds heavly depends on the parameter dmax. In contrast to other ICP mplementatons, our approach starts wth dmax = 1 cm n each teraton and only ncreases dmax f the number of neghbors found s nsuffcent (less than one-thrd of the sze of Λ). E. Trajectory Optmzaton As mentoned above, the pose of the reconstructed object s more accurate after we regstered our exstng model Γ and the pont cloud Λ of the current sweep. The algnment step corresponds to a correcton of the error accumulated by channg up the pose estmates (T1,..., Tn 1 ) from optcal flow. Fgure 8a llustrates such a chan along wth the ICP algnment that yelds the transformaton matrx TA. To account for the pose estmaton errors made durng the constructon of Λ, we perform a smoothng of the trajectory to obtan the maxmum lkelhood estmate for the trajectory (c) Fg. 9. Pctures and pont clouds of the dfferent objects: Aud TT, Van, (c) Polo. RMS [cm] T1 yn Aud TT Van Polo A1 A3 A2 Aud TT V1 V2 Van V3 P1 P2 Polo P3 Fg. 1. RMS errors of the dfferent datasets from Fgure 11 compared to the dfferent ground truths from Fgure 9. As vsble, the RMS to the correct ground truth s always the smallest and therefore allows for a classfcaton. of the object. Let (y1,..., yn ) be the poses of the movng object whle Λ s acqured. Addtonally, let e(y, yj, T) be an error functon whch computes the dfference between a gven estmated transformaton T and the expected value gven the current state of the poses y and yj. We determne y1,...,n by solvng ke(y1, yn, TA )k2ψ + y1,...,n n 1 X ke(y, y+1, T )k2σ, (13) =1 where k k2σ s the squared Mahalanobs dstance weghted by the covarance matrx Σ. Here, Ψ and Σ allow us to balance the nfluence of the measurements. Agan, we solve Eq. (13) wth Gauss-Newton as mplemented n g2 o [22]. Ths leads to a smoothed trajectory y1,...,n (see Fgure 8b) that we use to rebuld the model acqured durng the most recent sweep. V. R ESULTS We evaluate our approach on several real-world data sets n whch three dfferent cars are movng through the scene. A Hokuyo UTM-3LX laser rotatng wth 4 Hz mounted on a servo that s tltng from -5 to +3 provded the 3D range measurements. Addtonally, we recorded the mage data of one camera of a Pont Grey Bumblebee2 stereo camera runnng at 1 Hz and a resoluton of pxels. We use the mplementaton from Brox and Malk [2] for computng the optcal flow. For a quanttatve evaluaton, we acqured dense pont clouds of the stll standng vehcles depcted n Fgure 9 by manually steerng a robot around the vehcle. We appled sparse surface adjustment [23] to jon the vews and to obtan ground truth models. To assess the qualty of our model reconstructon from a movng vehcle, we constran the models to the ground plane and algn the reconstructon to the ground truth wth the prevously ntroduced pont-toplane ICP approach. As an ntalzaton we algn the four contact ponts of the wheels. For each model pont we then

6 Aud Aud TT TT - A1 - A Sweeps Aud Aud TT TT - A1 - A Sweeps Aud Aud TT TT- A1 - A Sweeps Aud Van TT - V1 A Sweeps Aud Van TT - V2 A Sweeps Aud Van TT - V3 A Sweeps Aud Polo TT - A1 P1 141 Sweeps Aud Polo TT - P2 A Sweeps Aud Polo TT - A1 P Sweeps Fg. 11. Illustraton of the dfferent datasets. The top row shows the trajectory of the movng object estmated by our approach along wth a top vew of the bulk pont cloud. The bottom row llustrates the models reconstructed by our approach. fnd the nearest neghbor n the ground truth and compute the root mean squared error (RMS). We not only compute the RMS of a vehcle compared to tself but also to the other models. The results are shown n Fgure 1. For the evaluaton we recorded three dfferent moton scenes of dfferent lengths (from 3 to 3 seconds) for each vehcle, as llustrated n Fgure 11. The trajectores nclude lnear motons towards the camera, from left-to-rght and rght-to-left, stops, curves and crcles. For each vehcle, n case of a crcle, we are able to reconstruct a 36 model. The RMS wth respect to the correspondng ground truth model typcally les between 3 cm and 9 cm. As vsble from Fgure 1, n all cases (even wth only 3 sweeps), the RMS of the reconstructed models suffces to dstngush between dfferent vehcles wthout the use of further features. Ths ndcates that we can use our approach to classfy movng objects based on ther 3D shape nstead of relyng on the vsual appearance alone. VI. CONCLUSION In ths paper, we propose a method to reconstruct models of movng rgd objects captured by a noddng laser range fnder and a vdeo camera. To ths end, our approach consders optcal flow to track the object n the camera frame and to correct the dstorton n the range data nduced by the moton of the object. The results from dfferent moton scenes of vehcles ndcate that our approach allows us to obtan accurate reconstructons. Our evaluaton shows that reconstructon errors le n the range of 3 cm to 9 cm. Furthermore, one could use our approach to classfy the movng object based on correspondng, prevously recorded 3D models. There are several optons for future work. Potental extensons regard a real-tme mplementaton or the relaxaton of the ground plane or rgdty assumpton. REFERENCES [1] S. Kumar, D. Gupta, and S. Yadav, Sensor fuson of laser and stereo vson camera for depth estmaton and obstacle avodance, Internatonal Journal of Computer Applcatons, vol. 1, no. 26, 21. [2] T. Brox and J. Malk, Large dsplacement optcal flow: descrptor matchng n varatonal moton estmaton, IEEE Transactons on Pattern Analyss and Machne Intellgence, vol. 33, pp , 211. [3] G. Sbley, C. Me, I. Red, and P. Newman, Vast scale outdoor navgaton usng adaptve relatve bundle adjustment, Int. Journal of Robotcs Research, vol. 29, no. 8, pp , July 21. [4] O. Wulf and B. Wagner, Fast 3D-scannng methods for laser measurement systems, n Int. Conf. on Control Systems and Computer Scence (CSCS), 23. [5] M. Bosse, R. Zlot, and P. Flck, Zebedee: Desgn of a sprng-mounted 3-D range sensor wth applcaton to moble mappng, IEEE Trans. on Robotcs, vol. 28, no. 5, pp , 212. [6] R. Trebel, B. Frank, J. Meyer, and W. Burgard, Frst steps towards a robotc system for flexble volumetrc mappng of ndoor envronments, Proc. of the IFAC/EURON Symposum on Intellgent Autonomous Vehcles (IAV), 24. [7] T. Fossotte, O. Stasse, A. Escande, P.-B. Weber, and A. Kheddar, A two-steps next-best-vew algorthm for autonomous 3D object modelng by a humanod robot, n Proc. of the IEEE Int. Conf. on Robotcs & Automaton (ICRA), 29. [8] M. Krann, B. Curless, and D. Fox, Autonomous generaton of complete 3D object models usng next best vew manpulaton plannng, n Proc. of the IEEE Int. Conf. on Robotcs & Automaton (ICRA), 211. [9] F. Blas, M. Pcard, and G. Godn, Accurate 3D acquston of freely movng objects, n Proc. of the 2nd Int. Symposum on 3D Data Processng, Vsualzaton and Transmsson, 24. [1] T. Wese, B. Lebe, and L. Van Gool, Fast 3D scannng wth automatc moton compensaton, n Proc. of the IEEE Conf. on Comp. Vson and Pattern Recognton (CVPR), 27. [11] M. Ruhnke, B. Steder, G. Grsett, and W. Burgard, Unsupervsed learnng of 3D object models from partal vews, n Proc. of the IEEE Int. Conf. on Robotcs & Automaton (ICRA), 29. [12] K. E. Ozden, K. Schndler, and L. Van Gool, Multbody structurefrom-moton n practce, IEEE Transactons on Pattern Analyss and Machne Intellgence, vol. 32, no. 6, pp , 21. [13] H. Dahlkamp, A. Kaehler, D. Stavens, S. Thrun, and G. Bradsk, Selfsupervsed monocular road detecton n desert terran, n Proc. of Robotcs: Scence and Systems (RSS), 26. [14] J. Dolson, J. Baek, C. Plagemann, and S. Thrun, Upsamplng range data n dynamc envronments, n Proc. of the IEEE Conf. on Comp. Vson and Pattern Recognton (CVPR), 21. [15] D. Held, J. Levnson, and S. Thrun, Precson trackng wth sparse 3D and dense color 2D data, n Proc. of the IEEE Int. Conf. on Robotcs & Automaton (ICRA), 213. [16] Z. Zhang, A flexble new technque for camera calbraton, IEEE Trans. on Pattern Analyss and Machne Intellgence (PAMI), vol. 22, no. 11, pp , 2. [17] F. Shevln, Analyss of orentaton problems usng Plücker lnes. n Internatonal Conference on Pattern Recognton (ICPR), vol. 1, Brsbane, 1998, pp [18] T. Brox, B. Rosenhahn, J. Gall, and D. Cremers, Combned regonand moton-based 3D trackng of rgd and artculated objects, IEEE Transactons on Pattern Analyss and Machne Intellgence, vol. 32, no. 3, pp , 21. [19] R. M. Murray, Z. L, and S. S. Sastry, Mathematcal Introducton to Robotc Manpulaton. Baton Rouge: CRC Press, [2] B. Rosenhahn, T. Brox, and H.-P. Sedel, Scaled moton dynamcs for markerless moton capture, n Internatonal Conference on Computer Vson and Pattern Recognton (CVPR), 27. [21] P. J. Huber, Robust Statstcs. New York: Wley, [22] R. Kümmerle, G. Grsett, H. Strasdat, K. Konolge, and W. Burgard, g2o: A general framework for graph optmzaton, n Proc. of the IEEE Int. Conf. on Robotcs & Automaton (ICRA), 211. [23] M. Ruhnke, R. Kümmerle, G. Grsett, and W. Burgard, Hghly accurate 3D surface models by sparse surface adjustment, n Proc. of the IEEE Int. Conf. on Robotcs & Automaton (ICRA), 212.