diff --git a/exp/polar_hough.py b/exp/polar_hough.py --- a/exp/polar_hough.py +++ b/exp/polar_hough.py @@ -21,9 +21,9 @@ class PolarHough: k=int(item[0]//self._anglePrecision) self._acc[k].append(item) - def extract(self,count): + def extract(self,count,trueVs): vanishingPoints=[] - angles=self._extractAngles(count) + angles=self._extractAngles(count,tuple(v[0] for v in trueVs)) angles=[alpha for (alpha,prominence) in angles] bins=self._mapAngles(angles) for (alpha,bin) in zip(angles,bins): @@ -31,12 +31,16 @@ class PolarHough: vanishingPoints.append((alpha,length)) return vanishingPoints - def _extractAngles(self,k): - lens=np.array(list(map(len,self._acc))) - log.debug(lens) + def _extractAngles(self,k,trueAngles): + lens=np.array([0]+list(map(len,self._acc))+[0]) + marked=np.copy(lens[1:-1]) + for alpha in trueAngles: + key=int(alpha/self._anglePrecision) + marked[key]=-marked[key]-1 + log.debug(marked) (peakKeys,info)=scipy.signal.find_peaks(lens,prominence=0) res=sorted(zip(info["prominences"],peakKeys),reverse=True)[:k] - res=[(key*self._anglePrecision,prominence) for (prominence,key) in res] + res=[((key-1)*self._anglePrecision,prominence) for (prominence,key) in res] log.debug("(angle, prominence): %s ... %s",res,[alpha/self._anglePrecision for (alpha,_) in res]) return res