Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[10304] pkg/trunk
|
From: <jam...@us...> - 2009-01-28 00:56:27
|
Revision: 10304
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10304&view=rev
Author: jamesbowman
Date: 2009-01-28 00:56:23 +0000 (Wed, 28 Jan 2009)
Log Message:
-----------
New FAST implementation that behaves more predictably wrt threshold. VO switched to new interface.
Modified Paths:
--------------
pkg/trunk/3rdparty/fast_detector/manifest.xml
pkg/trunk/3rdparty/fast_detector/src/fast.h
pkg/trunk/3rdparty/fast_detector/src/fast_9.c
pkg/trunk/3rdparty/fast_detector/src/nonmax.c
pkg/trunk/3rdparty/fast_detector/src/pyfast.c
pkg/trunk/vision/visual_odometry/src/visualodometer.py
Modified: pkg/trunk/3rdparty/fast_detector/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/fast_detector/manifest.xml 2009-01-28 00:25:14 UTC (rev 10303)
+++ pkg/trunk/3rdparty/fast_detector/manifest.xml 2009-01-28 00:56:23 UTC (rev 10304)
@@ -3,7 +3,7 @@
FAST
</description>
<author>Edward Rosten and Tom Drummond</author>
- <license>LGPL</license>
+ <license>BSD</license>
<review status="3rdparty" notes=""/>
</package>
Modified: pkg/trunk/3rdparty/fast_detector/src/fast.h
===================================================================
--- pkg/trunk/3rdparty/fast_detector/src/fast.h 2009-01-28 00:25:14 UTC (rev 10303)
+++ pkg/trunk/3rdparty/fast_detector/src/fast.h 2009-01-28 00:56:23 UTC (rev 10304)
@@ -6,10 +6,11 @@
#endif
typedef struct { int x, y; } xy;
+typedef struct { int x, y, r; } xyr;
typedef unsigned char byte;
-xy* fast_nonmax(const byte* im, int xsize, int ysize, xy* corners, int numcorners, int barrier, int* numnx);
-xy* fast_corner_detect_9(const byte* im, int xsize, int ysize, int barrier, int* numcorners);
+xyr* fast_nonmax(const byte* im, int xsize, int ysize, xyr* corners, int numcorners, int barrier, int* numnx);
+xyr* fast_corner_detect_9(const byte* im, int xsize, int ysize, int barrier, int* numcorners);
xy* fast_corner_detect_10(const byte* im, int xsize, int ysize, int barrier, int* numcorners);
xy* fast_corner_detect_11(const byte* im, int xsize, int ysize, int barrier, int* numcorners);
xy* fast_corner_detect_12(const byte* im, int xsize, int ysize, int barrier, int* numcorners);
Modified: pkg/trunk/3rdparty/fast_detector/src/fast_9.c
===================================================================
--- pkg/trunk/3rdparty/fast_detector/src/fast_9.c 2009-01-28 00:25:14 UTC (rev 10303)
+++ pkg/trunk/3rdparty/fast_detector/src/fast_9.c 2009-01-28 00:56:23 UTC (rev 10304)
@@ -4,12 +4,13 @@
*/
#include <stdlib.h>
#include "fast.h"
-xy* fast_corner_detect_9(const byte* im, int xsize, int ysize, int barrier, int* num)
+xyr* fast_corner_detect_9(const byte* im, int xsize, int ysize, int barrier, int* num)
{
int boundary = 3, y, cb, c_b;
+ int score;
const byte *line_max, *line_min;
int rsize=512, total=0;
- xy *ret = (xy*)malloc(rsize*sizeof(xy));
+ xyr *ret = (xyr*)malloc(rsize*sizeof(xyr));
const byte* cache_0;
const byte* cache_1;
const byte* cache_2;
@@ -2639,10 +2640,11 @@
if(total >= rsize)
{
rsize *=2;
- ret=(xy*)realloc(ret, rsize*sizeof(xy));
+ ret=(xyr*)realloc(ret, rsize*sizeof(xyr));
}
ret[total].x = cache_0-line_min;
- ret[total++].y = y;
+ ret[total].y = y;
+ total++;
}
}
*num = total;
Modified: pkg/trunk/3rdparty/fast_detector/src/nonmax.c
===================================================================
--- pkg/trunk/3rdparty/fast_detector/src/nonmax.c 2009-01-28 00:25:14 UTC (rev 10303)
+++ pkg/trunk/3rdparty/fast_detector/src/nonmax.c 2009-01-28 00:56:23 UTC (rev 10304)
@@ -179,7 +179,7 @@
}
/*void fast_nonmax(const BasicImage<byte>& im, const vector<ImageRef>& corners, int barrier, vector<ReturnType>& nonmax_corners)*/
-xy* fast_nonmax(const byte* im, int xsize, int ysize, xy* corners, int numcorners, int barrier, int* numnx)
+xyr* fast_nonmax(const byte* im, int xsize, int ysize, xyr* corners, int numcorners, int barrier, int* numnx)
{
/*Create a list of integer pointer offstes, corresponding to the */
@@ -187,14 +187,13 @@
int pointer_dir[16];
int* row_start = (int*) malloc(ysize * sizeof(int));
int* scores = (int*) malloc(numcorners * sizeof(int));
- xy* nonmax_corners=(xy*)malloc(numcorners* sizeof(xy));
+ xyr* nonmax_corners=(xyr*)malloc(numcorners* sizeof(xyr));
int num_nonmax=0;
int prev_row = -1;
int i, j;
int point_above = 0;
int point_below = 0;
-
pointer_dir[0] = 0 + 3 * xsize;
pointer_dir[1] = 1 + 3 * xsize;
pointer_dir[2] = 2 + 2 * xsize;
@@ -249,7 +248,7 @@
for(i=0; i < numcorners; i++)
{
int score = scores[i];
- xy pos = corners[i];
+ xyr pos = corners[i];
//Check left
if(i > 0)
@@ -305,6 +304,7 @@
nonmax_corners[num_nonmax].x = corners[i].x;
nonmax_corners[num_nonmax].y = corners[i].y;
+ nonmax_corners[num_nonmax].r = score;
num_nonmax++;
Modified: pkg/trunk/3rdparty/fast_detector/src/pyfast.c
===================================================================
--- pkg/trunk/3rdparty/fast_detector/src/pyfast.c 2009-01-28 00:25:14 UTC (rev 10303)
+++ pkg/trunk/3rdparty/fast_detector/src/pyfast.c 2009-01-28 00:56:23 UTC (rev 10304)
@@ -1,27 +1,113 @@
#include "Python.h"
#include "fast.h"
+static inline int corner_score(const byte* imp, const int *pointer_dir, int barrier)
+{
+ /*The score for a positive feature is sum of the difference between the pixels
+ and the barrier if the difference is positive. Negative is similar.
+ The score is the max of those two.
+
+ B = {x | x = points on the Bresenham circle around c}
+ Sp = { I(x) - t | x E B , I(x) - t > 0 }
+ Sn = { t - I(x) | x E B, t - I(x) > 0}
+
+ Score = max sum(Sp), sum(Sn)*/
+
+ int cb = *imp + barrier;
+ int c_b = *imp - barrier;
+ int sp=0, sn = 0;
+
+ int i=0;
+
+#if 0
+ for(i=0; i<16; i++)
+ {
+ int p = imp[pointer_dir[i]];
+
+ if(p > cb)
+ sp += p-cb;
+ else if(p < c_b)
+ sn += c_b-p;
+ }
+#else
+{
+ int p;
+#define ACC(i) p = imp[pointer_dir[i]]; if (p > cb) sp += p-cb; else sn += c_b-p;
+ ACC(0)
+ ACC(1)
+ ACC(2)
+ ACC(3)
+ ACC(4)
+ ACC(5)
+ ACC(6)
+ ACC(7)
+ ACC(8)
+ ACC(9)
+ ACC(10)
+ ACC(11)
+ ACC(12)
+ ACC(13)
+ ACC(14)
+ ACC(15)
+#undef ACC
+}
+#endif
+
+ if(sp > sn)
+ return sp;
+ else
+ return sn;
+}
+
PyObject *fast(PyObject *self, PyObject *args)
{
int imdata_size;
char *imdata;
- int xsize, ysize, threshold, barrier;
- if (!PyArg_ParseTuple(args, "s#iiii", &imdata, &imdata_size, &xsize, &ysize, &threshold, &barrier))
+ int xsize, ysize, barrier, threshold;
+ if (!PyArg_ParseTuple(args, "s#iiii", &imdata, &imdata_size, &xsize, &ysize, &barrier, &threshold))
return NULL;
int numcorners = 0, num_nonmax = 0;
- xy *corners = fast_corner_detect_9(imdata, xsize, ysize, threshold, &numcorners);
+ xyr *corners = fast_corner_detect_9(imdata, xsize, ysize, barrier, &numcorners);
+#if 0
+ xyr *nm = fast_nonmax(imdata, xsize, ysize, corners, numcorners, 3, &num_nonmax);
+#else
+ num_nonmax = numcorners;
+ xyr *nm = corners;
+ corners = NULL;
+#endif
- xy *nm = fast_nonmax(imdata, xsize, ysize, corners, numcorners, barrier, &num_nonmax);
- PyObject *r = PyList_New(num_nonmax);
+ int pixel[16];
+ pixel[0] = 0 + 3 * xsize;
+ pixel[1] = 1 + 3 * xsize;
+ pixel[2] = 2 + 2 * xsize;
+ pixel[3] = 3 + 1 * xsize;
+ pixel[4] = 3 + 0 * xsize;
+ pixel[5] = 3 + -1 * xsize;
+ pixel[6] = 2 + -2 * xsize;
+ pixel[7] = 1 + -3 * xsize;
+ pixel[8] = 0 + -3 * xsize;
+ pixel[9] = -1 + -3 * xsize;
+ pixel[10] = -2 + -2 * xsize;
+ pixel[11] = -3 + -1 * xsize;
+ pixel[12] = -3 + 0 * xsize;
+ pixel[13] = -3 + 1 * xsize;
+ pixel[14] = -2 + 2 * xsize;
+ pixel[15] = -1 + 3 * xsize;
+
+ PyObject *r = PyList_New(0);
int i;
-
for (i = 0; i < num_nonmax; i++) {
- PyObject *t = PyTuple_New(2);
- PyTuple_SetItem(t, 0, PyInt_FromLong(nm[i].x));
- PyTuple_SetItem(t, 1, PyInt_FromLong(nm[i].y));
- PyList_SetItem(r, i, t);
+ int x = nm[i].x, y = nm[i].y;
+ int s = corner_score(imdata + x + y * xsize, pixel, barrier);
+ if (s > threshold) {
+ PyObject *t = PyTuple_New(3);
+ PyTuple_SetItem(t, 0, PyInt_FromLong(x));
+ PyTuple_SetItem(t, 1, PyInt_FromLong(y));
+ PyTuple_SetItem(t, 2, PyInt_FromLong(s));
+ PyList_Append(r, t);
+ }
}
if (corners)
free(corners);
@@ -31,8 +117,66 @@
return r;
}
+PyObject *nonmax(PyObject *self, PyObject *args)
+{
+ PyObject *ppts;
+ Py_ssize_t i, j;
+ int xi, yi, ri;
+ int xj, yj, rj;
+ int dist2;
+
+ if (!PyArg_ParseTuple(args, "O", &ppts))
+ return NULL;
+ Py_ssize_t sz = PyList_Size(ppts);
+
+ char suppress[sz];
+ for (i = 0; i < sz; i++)
+ suppress[i] = 0;
+ xyr pts[sz];
+
+ for (i = 0; i < sz; i++) {
+ PyObject *t = PyList_GET_ITEM(ppts, i);
+ pts[i].x = PyInt_AsLong(PyTuple_GET_ITEM(t, 0));
+ pts[i].y = PyInt_AsLong(PyTuple_GET_ITEM(t, 1));
+ pts[i].r = PyInt_AsLong(PyTuple_GET_ITEM(t, 2));
+ }
+
+ for (i = 0; i < sz; i++) {
+ xi = pts[i].x;
+ yi = pts[i].y;
+ ri = pts[i].r;
+ for (j = i + 1; j < sz; j++) {
+ yj = pts[j].y;
+ if ((yj - yi) > 1) {
+ continue;
+ }
+ rj = pts[j].r;
+ xj = pts[j].x;
+ dist2 = (xi-xj)*(xi-xj) + (yi-yj)*(yi-yj);
+ if (dist2 < 2) {
+ if (ri < rj)
+ suppress[i] = 1;
+ else
+ suppress[j] = 1;
+ }
+ }
+ }
+
+ PyObject *r = PyList_New(0);
+ for (i = 0; i < sz; i++) {
+ if (!suppress[i]) {
+ PyObject *t = PyList_GET_ITEM(ppts, i);
+ Py_INCREF(t);
+ PyList_Append(r, t);
+ }
+ }
+
+ return r;
+}
+
static PyMethodDef methods[] = {
{"fast", fast, METH_VARARGS},
+ {"nonmax", nonmax, METH_VARARGS},
{NULL, NULL},
};
Modified: pkg/trunk/vision/visual_odometry/src/visualodometer.py
===================================================================
--- pkg/trunk/vision/visual_odometry/src/visualodometer.py 2009-01-28 00:25:14 UTC (rev 10303)
+++ pkg/trunk/vision/visual_odometry/src/visualodometer.py 2009-01-28 00:56:23 UTC (rev 10304)
@@ -185,6 +185,7 @@
hi = self.thresh
if len(features) > target_points:
lo = self.thresh
+ self.thresh = 0.5 * (lo + hi)
# Try to be a bit adaptive for next time
if len(features) > (target_points * 1.1):
@@ -193,20 +194,48 @@
self.thresh *= 0.95
return features
-class FeatureDetectorFast(FeatureDetector):
+# Feature detectors that return features in order (i.e. strongest first)
+# can be simpler. Just always keep the threshold high enough to give
+# too many responses, and take the top N.
-# default_thresh = 10
-# threshrange = (5,127)
+class FeatureDetectorOrdered:
+
+ def name(self):
+ return self.__class__.__name__
+
+ def __init__(self):
+ self.thresh = self.default_thresh
+ self.cold = True
+
+ def detect(self, frame, target_points):
+
+ features = self.get_features(frame, target_points)
+ # Too few features, so lower threshold
+ while (len(features) < target_points) and (self.thresh > self.threshrange[0]):
+ self.thresh = float(max(self.threshrange[0], self.thresh / 2))
+ features = self.get_features(frame, target_points)
+
+ # Try to be a bit adaptive for next time
+ if len(features) > (target_points * 2):
+ self.thresh *= 2
+ if len(features) < (target_points * 1.25):
+ self.thresh *= 0.95
+ return features[:target_points]
+
+def FAST(imdata, xsize, ysize, thresh):
+ kp = fast.fast(imdata, xsize, ysize, 9, thresh)
+ return sorted(fast.nonmax(kp), key = lambda x:(x[2],x[0],x[1]), reverse = True)
+
+class FeatureDetectorFast(FeatureDetectorOrdered):
+
default_thresh = 10
- threshrange = (5,127)
+ threshrange = (0.5,300)
def get_features(self, frame, target_points):
assert len(frame.rawdata) == (frame.size[0] * frame.size[1])
- feat = fast.fast(frame.rawdata, frame.size[0], frame.size[1], int(self.thresh), 40)
+ feat = FAST(frame.rawdata, frame.size[0], frame.size[1], self.thresh)
+ return [ (x,y) for (x,y,r) in feat if (16 <= x and x <= (640-16) and (16 <= y) and y < (480-16)) ]
- return [ (x,y) for (x,y) in feat if (16 <= x and x <= (640-16) and (16 <= y) and y < (480-16)) ]
- return feat
-
class FeatureDetector4x4:
def __init__(self, fd):
@@ -419,6 +448,16 @@
self.timer['disparity'].start()
disparities = [frame.lookup_disparity(x,y) for (x,y) in frame.kp2d]
frame.kp = [ (x,y,z) for ((x,y),z) in zip(frame.kp2d, disparities) if z]
+ #print "disparities", len(frame.kp2d), len(frame.kp)
+
+ if 0:
+ import pylab
+ pylab.imshow(numpy.fromstring(frame.lf.tostring(), numpy.uint8).reshape(480,640), cmap=pylab.cm.gray)
+ pylab.scatter([x for (x,y) in frame.kp2d], [y for (x,y) in frame.kp2d], label = '2d', c = 'red')
+ pylab.scatter([x for (x,y,d) in frame.kp], [y for (x,y,d) in frame.kp], label = 'disparities', c = 'green')
+ pylab.legend()
+ pylab.show()
+
self.timer['disparity'].stop()
lgrad = " " * (640 * 480)
@@ -455,12 +494,26 @@
#ps = Pose(numpy.mat([[1,0,0],[0,1,0],[0,0,1]]), numpy.array(shift))
#return pr * ps
+ def show_pairs(self, pairs, f0, f1):
+ print "*** SHOWING PAIRS FOR FRAMES ", f0.id, f1.id, "***"
+ print f0.id, "has", len(f0.kp), "keypoints"
+ print f1.id, "has", len(f1.kp), "keypoints"
+ print "There are", len(pairs), "pairs"
+ import pylab
+ for (a,b) in pairs:
+ pylab.plot([ f0.kp[a][0], f1.kp[b][0] ], [ f0.kp[a][1], f1.kp[b][1] ])
+ pylab.imshow(numpy.fromstring(f0.lf.tostring(), numpy.uint8).reshape(480,640), cmap=pylab.cm.gray)
+ pylab.scatter([x for (x,y,d) in f0.kp], [y for (x,y,d) in f0.kp], label = '%d kp' % f0.id, c = 'red')
+ pylab.scatter([x for (x,y,d) in f1.kp], [y for (x,y,d) in f1.kp], label = '%d kp' % f1.id, c = 'green')
+ pylab.legend()
+ pylab.show()
+
def proximity(self, f0, f1, scavenger = False):
"""Given frames f0, f1, returns (inliers, pose) where pose is the transform that maps f1's frame to f0's frame.)"""
self.num_frames += 1
pairs = self.temporal_match(f0, f1)
- #print "frames", (f0.id, f1.id), "got", len(pairs), "pairs",
+ #self.show_pairs(pairs, f0, f1)
if len(pairs) > 10:
solution = self.solve(f0.kp, f1.kp, pairs, True)
if scavenger and solution and solution[0] > 10:
@@ -646,19 +699,13 @@
self.ext_frames += 1
frame.externals.append((fext, VO.frame_pose(fext.id, fext.pose.tolist())))
- def handle_frame(self, frame):
- self.find_keypoints(frame)
- self.find_disparities(frame)
- self.collect_descriptors(frame)
- frame.id = self.num_frames
- return self.handle_frame_0(frame)
-
# just set up the frame with descriptors, no VO processing
def setup_frame(self, frame):
self.find_keypoints(frame)
self.find_disparities(frame)
self.collect_descriptors(frame)
frame.id = self.num_frames
+ frame.ref_frame_id = None
# return inliers from a match
def check_inliers(self, frame1, frame2):
@@ -677,6 +724,13 @@
self.maintain_tracks(oldkey, newkey)
self.sba_handle_frame(newkey)
+ def handle_frame(self, frame):
+ self.find_keypoints(frame)
+ self.find_disparities(frame)
+ self.collect_descriptors(frame)
+ frame.id = self.num_frames
+ return self.handle_frame_0(frame)
+
def handle_frame_0(self, frame):
if self.prev_frame:
# If the key->current is good, use it
@@ -684,6 +738,7 @@
ref = self.keyframe
self.pairs = self.temporal_match(ref, frame)
+ #if frame.id == 202 and ref.id == 199: self.show_pairs(self.pairs, ref, frame)
solution = self.solve(ref.kp, frame.kp, self.pairs)
if solution and solution[0] > 5:
(inl, rot, shift) = solution
@@ -749,3 +804,18 @@
Top = Tok * Tkp
print "** CORRECTED by", f.pose.d(Top), "**", "key", f.ref_frame_id, "of frame", f.id
f.pose = Top
+
+ def report_frame(self, frame):
+ import md5
+ print "*** FRAME", "id", frame.id, "key", frame.ref_frame_id, "***"
+ print "use_grad_img", frame.use_grad_img
+ print "limage md5:", " ".join([ "%02x" % ord(x) for x in md5.new(frame.lf.tostring()).digest()])
+ print "rimage md5:", " ".join([ "%02x" % ord(x) for x in md5.new(frame.rf.tostring()).digest()])
+ print "lgrad md5:", " ".join([ "%02x" % ord(x) for x in md5.new(frame.lgrad).digest()])
+ print "rimage md5:", " ".join([ "%02x" % ord(x) for x in md5.new(frame.rgrad).digest()])
+ print "kp2d length", len(frame.kp2d)
+ print " ", frame.kp2d[:7]
+ print "kp length", len(frame.kp)
+ for k in frame.kp:
+ print " ", k
+ print
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
