kitti_odometry.py (#28)

* kitti_odometry.py

* IncrementalVoxelMap

src/example/kitti_odometry.py

@@ -0,0 +1,98 @@
+#!/usr/bin/python3
+import os
+import time
+import argparse
+import collections
+import numpy
+import small_gicp
+from pyridescence import *
+
+# Odometry estimation based on scan-to-scan matching
+class ScanToScanMatchingOdometry(object):
+  def __init__(self, num_threads):
+    self.num_threads = num_threads
+    self.T_last_current = numpy.identity(4)
+    self.T_world_lidar = numpy.identity(4)
+    self.target = None
+  
+  def estimate(self, raw_points):
+    downsampled, tree = small_gicp.preprocess_points(raw_points, 0.25, num_threads=self.num_threads)
+    
+    if self.target is None:
+      self.target = (downsampled, tree)
+      return self.T_world_lidar
+
+    result = small_gicp.align(self.target[0], downsampled, self.target[1], self.T_last_current, num_threads=self.num_threads)
+    
+    self.T_last_current = result.T_target_source
+    self.T_world_lidar = self.T_world_lidar @ result.T_target_source
+    self.target = (downsampled, tree)
+    
+    return self.T_world_lidar
+    
+# Odometry estimation based on scan-to-model matching
+class ScanToModelMatchingOdometry(object):
+  def __init__(self, num_threads):
+    self.num_threads = num_threads
+    self.T_last_current = numpy.identity(4)
+    self.T_world_lidar = numpy.identity(4)
+    self.target = small_gicp.GaussianVoxelMap(1.0)
+    self.target.set_lru(horizon=100, clear_cycle=10)
+  
+  def estimate(self, raw_points):
+    downsampled, tree = small_gicp.preprocess_points(raw_points, 0.25, num_threads=self.num_threads)
+    
+    if self.target.size() == 0:
+      self.target.insert(downsampled)
+      return self.T_world_lidar
+    
+    result = small_gicp.align(self.target, downsampled, self.T_world_lidar @ self.T_last_current, num_threads=self.num_threads)
+
+    self.T_last_current = numpy.linalg.inv(self.T_world_lidar) @ result.T_target_source
+    self.T_world_lidar = result.T_target_source
+    self.target.insert(downsampled, self.T_world_lidar)
+    
+    guik.viewer().update_drawable('target', glk.create_pointcloud_buffer(self.target.voxel_points()[:, :3]), guik.Rainbow())
+    
+    return self.T_world_lidar
+  
+
+def main():
+  parser = argparse.ArgumentParser()
+  parser.add_argument('dataset_path', help='/path/to/kitti/velodyne')
+  parser.add_argument('--num_threads', help='Number of threads', type=int, default=4)
+  parser.add_argument('-m', '--model', help='Use scan-to-model matching odometry', action='store_true')
+  args = parser.parse_args()
+  
+  dataset_path = args.dataset_path
+  filenames = sorted([dataset_path + '/' + x for x in os.listdir(dataset_path) if x.endswith('.bin')])
+  
+  if not args.model:
+    odom = ScanToScanMatchingOdometry(args.num_threads)
+  else:
+    odom = ScanToModelMatchingOdometry(args.num_threads)
+  
+  viewer = guik.viewer()
+  viewer.disable_vsync()
+  time_queue = collections.deque(maxlen=500)
+
+  for i, filename in enumerate(filenames):
+    raw_points = numpy.fromfile(filename, dtype=numpy.float32).reshape(-1, 4)[:, :3]
+    
+    t1 = time.time()
+    T = odom.estimate(raw_points)
+    t2 = time.time()
+   
+    time_queue.append(t2 - t1)
+    viewer.lookat(T[:3, 3])
+    viewer.update_drawable('points', glk.create_pointcloud_buffer(raw_points), guik.FlatOrange(T).add('point_scale', 2.0))
+    
+    if i % 10 == 0:
+      viewer.update_drawable('pos_{}'.format(i), glk.primitives.coordinate_system(), guik.VertexColor(T))
+      viewer.append_text('avg={:.3f} msec/scan  last={:.3f} msec/scan'.format(1000 * numpy.mean(time_queue), 1000 * time_queue[-1]))
+    
+    if not viewer.spin_once():
+      break    
+
+if __name__ == '__main__':
+  main()

src/python/voxelmap.cpp

@@ -15,22 +15,56 @@
 namespace py = pybind11;
 using namespace small_gicp;
 
-void define_voxelmap(py::module& m) {
-  // GaussianVoxelMap
-  py::class_<GaussianVoxelMap>(m, "GaussianVoxelMap")  //
-    .def(py::init<double>())
+template <typename VoxelMap, bool has_normals, bool has_covs>
+auto define_class(py::module& m, const std::string& name) {
+  py::class_<VoxelMap> vox(m, name.c_str());
+  vox.def(py::init<double>())
     .def(
       "__repr__",
-      [](const GaussianVoxelMap& voxelmap) {
+      [=](const VoxelMap& voxelmap) {
         std::stringstream sst;
-        sst << "small_gicp.GaussianVoxelMap (" << 1.0 / voxelmap.inv_leaf_size << " m / " << voxelmap.size() << " voxels)" << std::endl;
+        sst << "small_gicp." << name << "(" << 1.0 / voxelmap.inv_leaf_size << " m / " << voxelmap.size() << " voxels)" << std::endl;
         return sst.str();
       })
-    .def("__len__", [](const GaussianVoxelMap& voxelmap) { return voxelmap.size(); })
-    .def("size", &GaussianVoxelMap::size)
+    .def("__len__", [](const VoxelMap& voxelmap) { return voxelmap.size(); })
+    .def("size", &VoxelMap::size)
     .def(
       "insert",
-      [](GaussianVoxelMap& voxelmap, const PointCloud& points, const Eigen::Matrix4d& T) { voxelmap.insert(points, Eigen::Isometry3d(T)); },
+      [](VoxelMap& voxelmap, const PointCloud& points, const Eigen::Matrix4d& T) { voxelmap.insert(points, Eigen::Isometry3d(T)); },
       py::arg("points"),
-      py::arg("T") = Eigen::Matrix4d::Identity());
+      py::arg("T") = Eigen::Matrix4d::Identity())
+    .def(
+      "set_lru",
+      [](VoxelMap& voxelmap, size_t horizon, size_t clear_cycle) {
+        voxelmap.lru_horizon = horizon;
+        voxelmap.lru_clear_cycle = clear_cycle;
+      },
+      py::arg("horizon") = 100,
+      py::arg("clear_cycle") = 10)
+    .def("voxel_points", [](const VoxelMap& voxelmap) -> Eigen::MatrixXd {
+      auto points = traits::voxel_points(voxelmap);
+      return Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(points[0].data(), points.size(), 4);
+    });
+
+  if constexpr (has_normals) {
+    vox.def("voxel_normals", [](const VoxelMap& voxelmap) -> Eigen::MatrixXd {
+      auto normals = traits::voxel_normals(voxelmap);
+      return Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(normals[0].data(), normals.size(), 4);
+    });
+  }
+
+  if constexpr (has_covs) {
+    vox.def("voxel_covs", [](const VoxelMap& voxelmap) -> Eigen::MatrixXd {
+      auto covs = traits::voxel_covs(voxelmap);
+      return Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(covs[0].data(), covs.size(), 16);
+    });
+  }
+};
+
+void define_voxelmap(py::module& m) {
+  define_class<IncrementalVoxelMap<FlatContainerPoints>, false, false>(m, "IncrementalVoxelMap");
+  define_class<IncrementalVoxelMap<FlatContainerNormal>, true, false>(m, "IncrementalVoxelMapNormal");
+  define_class<IncrementalVoxelMap<FlatContainerCov>, false, true>(m, "IncrementalVoxelMapCov");
+  define_class<IncrementalVoxelMap<FlatContainerNormalCov>, true, true>(m, "FlatContainerNormalCov");
+  define_class<GaussianVoxelMap, false, true>(m, "GaussianVoxelMap");
 }