a `§»eã@sŽddlZddlZddlmZddlmZmZddlmZddl m Z ddl m Z ddl ZddlZdd„Zd d „Zd d „Zdd d„Zdd„ZdS)éN)Úlog)Úget_pseudogeo_utmÚget_pseudogeo_scale)Ú transformer)ÚCRS)ÚgdalcCst |¡dS)zGet rotation as a 3x3 matrix.r)Úcv2Ú Rodrigues)Úrotation©r ú/code/opendm/shots.pyÚget_rotation_matrix sr cCs tj|td}t |¡d ¡S)N)Údtyper)ÚnpÚarrayÚfloatrr Úravel)Úrotation_matrixÚRr r r Úmatrix_to_rotationsrcCs&tt |d¡ƒj t |d¡¡ S)z,The origin of the pose in world coordinates.r Ú translation)r rrÚTÚdot)Úshotr r r Ú get_originsrc"CsBd}|dur®tj |¡r®tƒ}t |¡}| ¡\}}} } } } ||j|} | |j| } t   dt ƒddd|| dgddt ƒdd| | dggd¢gd¢g¡}d}d}|durºdSt t  |¡t  d ¡ƒ}tj |¡r2t|d ƒ,}t | ¡¡}g}i}|D]þ}| d i¡}| d i¡D]Ü}|d |}| d ¡}||vs$||vrXq$||}|durö|dd…dd…f|dd…df}}t j |¡}t|ƒ}t  ||¡|}| |d|d|d¡}tt   |d¡ƒ}tt  ||¡ƒ} |}!n¨|d} t|ƒ}|d|d|d|d|dg}|dur€tt   | ¡ |dd…dd…f¡ƒ} t| t  t   |¡df¡¡dd…ƒ}|}!| |d|d|d¡}| d|| d| d¡¡| dd¡| dd¡| dd¡t|!ƒt| ƒdœdt|ƒdœdœ¡d||<q$qWdƒn1s0Yd|dœStd|ƒ‚dS)z: Extract shots from OpenSfM's reconstruction.json Ngð?érg@)rrér)rrrrTÚ4326ÚrÚcamerasÚshotsÚcameraérr éÿÿÿÿÚFeatureÚfocalÚfocal_xÚwidthÚheightÚ capture_time)Úfilenamer%r'r(r)rr ÚPoint)ÚtypeÚ coordinates)r,Ú propertiesÚgeometryÚFeatureCollection)r,Úfeaturesz%s does not exist.) ÚosÚpathÚexistsrrÚOpenÚGetGeoTransformÚ RasterXSizeÚ RasterYSizerrrrrÚ from_proj4Ú from_epsgÚopenÚjsonÚloadsÚreadÚgetÚlinalgÚinvrrÚTransformPointr rÚlistÚhstackÚappendÚ RuntimeError)"Zreconstruction_fileZutm_srsÚ utm_offsetZpseudo_geotiffZa_matrixZpseudo_geocoordsZrasterZulxZxresÚ_ZulyZyresZlrxZlryZpseudoZcrstransZfinÚreconstructionsZfeatsZ added_shotsÚreconrr*rÚcamÚRsrZRs1ÚoriginZ utm_coordsZ trans_coordsrr rr r r Úget_geojson_shots_from_opensfmsŠ  ý     & þ $&   ù þõ0þrNc Csìi}i}|D] }t|dƒ}t | ¡¡}Wdƒn1s>0Yt|ƒdkr|| dg¡D]}d||dd<q`|}q | dg¡D]"}|dd|vrˆ|d |¡qˆq t|dƒ }| t |¡¡Wdƒn1sÞ0YdS)Nrrr1Tr.r*Úw) r;r<r=r>Úlenr?rEÚwriteÚdumps)Zgeojson_shots_filesZoutput_geojson_fileÚresultZ added_filesZ shot_fileÚfr Úfeatr r r Úmerge_geojson_shotss ,  rV)NNNN)r2r<ÚopendmrZopendm.pseudogeorrÚopendm.locationrÚpyprojrÚosgeorÚnumpyrrr rrrNrVr r r r Ús     g