[Personalrobots-commit] SF.net SVN: personalrobots:[11956] pkg/trunk/vision/visual_odometry

[<jam...@us...>]

Revision: 11956
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11956&view=rev
Author:   jamesbowman
Date:     2009-02-28 22:11:17 +0000 (Sat, 28 Feb 2009)

Log Message:
-----------
Edge deletion

Modified Paths:
--------------
    pkg/trunk/vision/visual_odometry/manifest.xml
    pkg/trunk/vision/visual_odometry/src/py.cpp
    pkg/trunk/vision/visual_odometry/test/pe.py

Modified: pkg/trunk/vision/visual_odometry/manifest.xml
===================================================================
--- pkg/trunk/vision/visual_odometry/manifest.xml	2009-02-28 20:11:15 UTC (rev 11955)
+++ pkg/trunk/vision/visual_odometry/manifest.xml	2009-02-28 22:11:17 UTC (rev 11956)
@@ -33,4 +33,6 @@
   <sysdepend os="ubuntu" version="8.04-hardy" package="python-matplotlib"/>
   <sysdepend os="ubuntu" version="7.04-feisty" package="libfltk-dev"/>
   <sysdepend os="ubuntu" version="8.04-hardy" package="libfltk-dev"/>
+  <sysdepend os="ubuntu" version="7.04-feisty" package="python-scipy"/>
+  <sysdepend os="ubuntu" version="8.04-hardy" package="python-scipy"/>
 </package>

Modified: pkg/trunk/vision/visual_odometry/src/py.cpp
===================================================================
--- pkg/trunk/vision/visual_odometry/src/py.cpp	2009-02-28 20:11:15 UTC (rev 11955)
+++ pkg/trunk/vision/visual_odometry/src/py.cpp	2009-02-28 22:11:17 UTC (rev 11956)
@@ -919,6 +919,7 @@
 typedef TreeOptimizer3::Rotation Rotation;
 typedef TreeOptimizer3::Pose Pose;
 typedef TreeOptimizer3::Vertex Vertex;
+typedef TreeOptimizer3::Edge Edge;
 
 typedef struct {
   PyObject_HEAD
@@ -953,6 +954,19 @@
   Py_RETURN_NONE;
 }
 
+PyObject *pgremoveEdge(PyObject *self, PyObject *args)
+{
+  int i0, i1;
+
+  if (!PyArg_ParseTuple(args, "ii", &i0, &i1))
+    return NULL;
+
+  TreeOptimizer3 *to = ((treeoptimizer3_t*)self)->to;
+  Edge *e = to->edge(i0, i1);
+  to->removeEdge(e);
+  Py_RETURN_NONE;
+}
+
 PyObject *pgaddIncrementalEdge(PyObject *self, PyObject *args)
 {
 
@@ -988,11 +1002,8 @@
   inf[4][4] = e;
   inf[5][5] = f;
 
-#if 0
-  to->addIncrementalEdge(i0, i1, t, Information::I(6));
-#else
   to->addIncrementalEdge(i0, i1, t, inf);
-#endif
+
   Py_RETURN_NONE;
 }
 
@@ -1012,6 +1023,10 @@
   Pose *pp = &(pv->pose);
   return Py_BuildValue("(ddd)(ddd)", pp->x(), pp->y(), pp->z(), pp->roll(), pp->pitch(), pp->yaw());
 #else
+  if (pv == NULL) {
+    PyErr_SetString(PyExc_TypeError, "no such vertex");
+    return NULL;
+  }
   Pose pp = pv->transformation.toPoseType();
   return Py_BuildValue("(ddd)(ddd)", pp.x(), pp.y(), pp.z(), pp.roll(), pp.pitch(), pp.yaw());
 #endif
@@ -1041,6 +1056,7 @@
   {"save", pgsave, METH_VARARGS},
   {"load", pgload, METH_VARARGS},
   {"addIncrementalEdge", pgaddIncrementalEdge, METH_VARARGS },
+  {"removeEdge", pgremoveEdge, METH_VARARGS },
   {"initializeOnlineIterations", pginitializeOnlineIterations, METH_VARARGS},
   {"error", pgerror, METH_VARARGS},
   {"iterate", pgiterate, METH_VARARGS},

Modified: pkg/trunk/vision/visual_odometry/test/pe.py
===================================================================
--- pkg/trunk/vision/visual_odometry/test/pe.py	2009-02-28 20:11:15 UTC (rev 11955)
+++ pkg/trunk/vision/visual_odometry/test/pe.py	2009-02-28 22:11:17 UTC (rev 11956)
@@ -1,5 +1,5 @@
-import rostools
-rostools.update_path('visual_odometry')
+import roslib
+roslib.update_path('visual_odometry')
 import rostest
 import rospy
 
@@ -48,7 +48,8 @@
 
 #(f0,f1) = [ ComputedDenseStereoFrame(Image.open("../vslam/kk2/%06dL.png" % i), Image.open("../vslam/kk2/%06dR.png" % i)) for i in [670, 671] ]
 #(f0,f1) = [ ComputedDenseStereoFrame(Image.open("f%d-left.png" % i), Image.open("f%d-right.png" % i)) for i in [0, 1] ]
-(f0,f1) = [ ComputedDenseStereoFrame(Image.open("../vslam/trial/%06dL.png" % i), Image.open("../vslam/trial/%06dR.png" % i)) for i in [276, 278] ]
+d = "/u/jamesb/ros/ros-pkg/vision/vslam/trial"
+(f0,f1) = [ ComputedDenseStereoFrame(Image.open("%s/%06dL.png" % (d,i)), Image.open("%s/%06dR.png" % (d,i))) for i in [276, 278] ]
 #(f0,f1) = [ ComputedDenseStereoFrame(Image.open("../vslam/trial/%06dL.png" % i), Image.open("../vslam/trial/%06dR.png" % i)) for i in [277, 278] ]
 #(f0,f1) = [ ComputedDenseStereoFrame(Image.open("../vslam/trial/%06dL.png" % i), Image.open("../vslam/trial/%06dR.png" % i)) for i in [0, 1] ]
 
@@ -77,7 +78,7 @@
   return (nx, ny, nz)
 
 def mype(cam, cp1, cp0, pairs):
-  p0 = [ stereo_cam.pix2cam(*p) for p in cp0 ]
+  p0 = [ cam.pix2cam(*p) for p in cp0 ]
   xyz0 = [ p0[i] for (j,i) in pairs ]
 
   x0 = vop.array([ x for (x,y,z) in xyz0 ])
@@ -114,7 +115,7 @@
 
     # Check Cont inliers for RT against xyz0 -> uvd0 vs uvd1
     # Takes 15
-    (u0,v0,d0) = stereo_cam.cam2pix(*xform(RT, x0, y0, z0))
+    (u0,v0,d0) = cam.cam2pix(*xform(RT, x0, y0, z0))
     pred_inl = vop.where(vop.maximum(vop.maximum(abs(u0 - u1), abs(v0 - v1)), abs(d0 - d1)) > 3.0, 0.0, 1.0)
     inliers = int(pred_inl.sum())
     if inliers > best[0]:
@@ -186,6 +187,9 @@
 print "original error", img_rms(f0, f1, RT)
 
 (inliers, (R,T)) = mype(stereo_cam, f0.kp, f1.kp, pairs)
+print (inliers, (R,T))
+sys.exit(0)
+
 R = list(R)
 RT = R[0:3] + [T[0]] + R[3:6] + [T[1]] + R[6:9] + [T[2]]
 img_img(f0, f1, RT).save("pe.png")


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.