diff --git a/src/example/kitti_odometry.py b/src/example/kitti_odometry.py new file mode 100755 index 0000000..90c05c5 --- /dev/null +++ b/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() diff --git a/src/python/voxelmap.cpp b/src/python/voxelmap.cpp index 5bd9fe5..15306c8 100644 --- a/src/python/voxelmap.cpp +++ b/src/python/voxelmap.cpp @@ -15,22 +15,56 @@ namespace py = pybind11; using namespace small_gicp; -void define_voxelmap(py::module& m) { - // GaussianVoxelMap - py::class_(m, "GaussianVoxelMap") // - .def(py::init()) +template +auto define_class(py::module& m, const std::string& name) { + py::class_ vox(m, name.c_str()); + vox.def(py::init()) .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>(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>(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>(covs[0].data(), covs.size(), 16); + }); + } +}; + +void define_voxelmap(py::module& m) { + define_class, false, false>(m, "IncrementalVoxelMap"); + define_class, true, false>(m, "IncrementalVoxelMapNormal"); + define_class, false, true>(m, "IncrementalVoxelMapCov"); + define_class, true, true>(m, "FlatContainerNormalCov"); + define_class(m, "GaussianVoxelMap"); } \ No newline at end of file