ó °ÉúWc@s…ddlZddlZddlZddlmZddlmZmZdefd„ƒYZ ddd„Z d„Z d „Z dS( iÿÿÿÿN(t LandmarkMap(t angle_betweentget2dRotmatFromYawt LocalizercBs>eZd„Zd„Zd„Zd„Zd„Zd„ZRS(cCst|ƒ|_dS(N(Rt landmarkMap(tselft mapFilename((sB/home/andrew/catkin_ws/src/landmark_localizer/scripts/Localizer.pyt__init__scCs«t|jjƒdkrQtjddtƒ}|jjd|d*tjd}n:tj|jjddtƒ}tjddddgƒ}d||fd|d dfgS( Niitdtypeiiig…ëQžÕ?gq= ×£på?i(tlenRtsizetnptonestfloattpitarray(Rt seenLandmarkstpositiont orientation((sB/home/andrew/catkin_ws/src/landmark_localizer/scripts/Localizer.pytlocalizes c Cshd}d}d}d}dGt|ƒGdGHt|ƒ|krFdGHdS|j|ttt||||ƒS(Niitsawt landmarkss%not enough landmarks seen to localize(R tNonetransacLocalizetgetModelFromPlaneWorldMatchestisPlaneWorldInliertcomputePlaneWorldError(RRtntmtktd((sB/home/andrew/catkin_ws/src/landmark_localizer/scripts/Localizer.pytplaneWorldLocalizes cCs[|d }g|D]"}||jj|ƒdf^q}t|ƒ\}}d||fgS(Niii(RtgetLandmarkMatchesR(RRtusingLandmarkstlt usingMatchestcamPostcamYaw((sB/home/andrew/catkin_ws/src/landmark_localizer/scripts/Localizer.pytexact2dLocalize,s  ,c Cs\d} g} d} d} x| |kr/| d7} t| ƒdkretj||ƒ} g| D]!}||jj||ƒf^q\}gt|ƒD]}|f^q}xKt|dƒD]9}gt|ƒD] }|D]}||f^qÓqÉ}q¶Wt|dtdt ƒ}xW|D]L}gt |ƒD]&\}\}}||||f^q%}| j |ƒqWn| j ƒ}||ƒ}t |ƒt |dƒ}g|D]!}|||||jƒf^qž}td„|ƒ}t|ƒ|kr||}|||Œ}| dks|| kr,|} |} q,qqW| dkrDdGHnd| d| dfgS(so The "data points" selected for ransac are matches between objects in seenLandmarks and objects in the map. seenLandmarks: the collection of observed landmarks to fit to the map. getModelFromMatches: a function that computes a transform from a set of point matches. isInlier: a function that determines whether a landmark should or should not be considered an "inlier" given a model transform and a map. computeError: measures the error of a set of matches given a model. n: number of landmarks to assume are inliers from seenLandmarks. n = 2 for the 2D and 2D+scale case, n = 4 in full 3D. m: number of best matches of seen landmarks to consider from the map. k: number of iterations before terminating. d: number of inliers that a model must have to be considered iitkeytreversecSs|dtk S(Ni(tFalse(R((sB/home/andrew/catkin_ws/src/landmark_localizer/scripts/Localizer.pytcssno good model found!N(RR trandomtsampleRR trangetsortedtsumtTruet enumeratetappendtpoptsettfilter(RRtgetModelFromMatchestisInliert computeErrorRRRRtitconsideredMatchest bestErrort bestModelt nLandmarksR"t matchSetstjttuplest_ttt sortedTuplest indexTupletmst newMatchSett testMatchestmodeltseenLandmarksToChecktslt foundMatchest inlierMatchesterror((sB/home/andrew/catkin_ws/src/landmark_localizer/scripts/Localizer.pyR3sB +"7 6  +  cCsdS(N((RRR6((sB/home/andrew/catkin_ws/src/landmark_localizer/scripts/Localizer.pytqualityLocalizess(t__name__t __module__RRRR&RRN(((sB/home/andrew/catkin_ws/src/landmark_localizer/scripts/Localizer.pyR s     @ic Cs’|\}}|j||ƒ}|j}|j||ƒ} |j||ƒ} g| D]} | | krR| ^qR} t| ƒdkr†tS| dSdS(s> How do we decide what is and is not an inlier? it should be a function of the distances to the nearest point that is, both in space and in the distance between their feature vectors we could make the inlier status be: is the physically-closest landmark the same as the best match in the map? This is rigourous, maybe too rigourous. Could be top-N closest and/or top-N matches? So if the best match is in the top N closest, or if the closest is in the top N matches...or if at least one of the top N closest is in the top M matches... iN(tgetLandmarkInWorldCoordsRtgetClosestLandmarksR R R)( t seenLandmarkRHRtnClosesttnBestR$R%tmapSeenLandmarkt mapSeenPostclosestLandmarkst bestMatchestlmt bestInClosest((sB/home/andrew/catkin_ws/src/landmark_localizer/scripts/Localizer.pyRzs  %cCsy|d \\}}\}}|j|j}tjj|ƒ}|j|j}t||j ƒ}t|j|jƒ} |tj|ƒtj| ƒ} tjdtjj|jƒƒ} | tj| ƒ} tj|d|dƒ} |j|j}tj|d|dƒ}|| }t| ƒ}|j |ƒ}|j | }|j|}tj |d|d| gƒ}||fS(s Given a pair of matches between landmarks in the image and in the map, finds the transform between the world coordinate frame and the camera's coordinate frame. It assumes fixed roll and pitch, so it just returns the x and y coordinates, the height above the map, and the yaw. A few notes on notation: a and b represent the locations of the two landmarks c represents the position of the camera in world coordinates Ci represents the projection of the camera along z onto the world plane (that is, Ci.x = c.x, Ci.y = c.y, and Ci.z = 0) angle_xyz represents the angle between lines from points x to y and y to z angle_vVsu represents the angle between vectors v and u x2y represents a line from point x to point y iii( RR tlinalgtnormRtsintarctan2ttanRtgetRotatedLandmarkR(tmatchestaCamtaMaptbCamtbMapt a2bVecMapt a2bDistMapt b2aVecCamt angle_abCit angle_aCibt a2CiDistMapt angle_Ciact camAltitudetangle_a2bVsMapXt a2bVecCamtangle_a2bVsCamXR%trotmattaCamRott a2CiVecMaptCiPosR$((sB/home/andrew/catkin_ws/src/landmark_localizer/scripts/Localizer.pyR™s& !    cCscd}xL|D]D\}}|j||ƒ}tjj|j|jƒ}||7}q W|t|ƒS(Ni(RQR R\R]RR (RbR$tcamOrientationt totalErrortseenLmtmapLmt worldSeenLmRM((sB/home/andrew/catkin_ws/src/landmark_localizer/scripts/Localizer.pyRÇs ( tpdbR+tnumpyR Rt geometryUtilsRRtobjectRRRR(((sB/home/andrew/catkin_ws/src/landmark_localizer/scripts/Localizer.pyts   n .