test for factors (#37)

* test for factors

* fix factor test

src/python/align.cpp

@@ -102,10 +102,12 @@ void define_align(py::module& m) {
       KdTreeOMP<PointCloud>::ConstPtr target_tree,
       const Eigen::Matrix4d& init_T_target_source,
       double max_correspondence_distance,
-      int num_threads) {
+      int num_threads,
+      int max_iterations) {
       Registration<GICPFactor, ParallelReductionOMP> registration;
       registration.rejector.max_dist_sq = max_correspondence_distance * max_correspondence_distance;
       registration.reduction.num_threads = num_threads;
+      registration.optimizer.max_iterations = max_iterations;
 
       if (target_tree == nullptr) {
         target_tree = std::make_shared<KdTreeOMP<PointCloud>>(target, num_threads);
@@ -117,15 +119,23 @@ void define_align(py::module& m) {
     py::arg("target_tree") = nullptr,
     py::arg("init_T_target_source") = Eigen::Matrix4d::Identity(),
     py::arg("max_correspondence_distance") = 1.0,
-    py::arg("num_threads") = 1);
+    py::arg("num_threads") = 1,
+    py::arg("max_iterations") = 20);
 
   // align
   m.def(
     "align",
-    [](const GaussianVoxelMap& target_voxelmap, const PointCloud& source, const Eigen::Matrix4d& init_T_target_source, double max_correspondence_distance, int num_threads) {
+    [](
+      const GaussianVoxelMap& target_voxelmap,
+      const PointCloud& source,
+      const Eigen::Matrix4d& init_T_target_source,
+      double max_correspondence_distance,
+      int num_threads,
+      int max_iterations) {
       Registration<GICPFactor, ParallelReductionOMP> registration;
       registration.rejector.max_dist_sq = max_correspondence_distance * max_correspondence_distance;
       registration.reduction.num_threads = num_threads;
+      registration.optimizer.max_iterations = max_iterations;
 
       return registration.align(target_voxelmap, source, target_voxelmap, Eigen::Isometry3d(init_T_target_source));
     },
@@ -133,5 +143,6 @@ void define_align(py::module& m) {
     py::arg("source"),
     py::arg("init_T_target_source") = Eigen::Matrix4d::Identity(),
     py::arg("max_correspondence_distance") = 1.0,
-    py::arg("num_threads") = 1);
+    py::arg("num_threads") = 1,
+    py::arg("max_iterations") = 20);
 }

src/test/python_test.py

@@ -138,6 +138,32 @@ def test_voxelmap(load_points):
   assert voxelmap.size() > 0
   assert voxelmap.size() == len(voxelmap)
 
+# Factor test
+def test_factors(load_points):
+  gt_T_target_source, target_raw_numpy, source_raw_numpy = load_points
+
+  target, target_tree = small_gicp.preprocess_points(target_raw_numpy, downsampling_resolution=0.25)
+  source, source_tree = small_gicp.preprocess_points(source_raw_numpy, downsampling_resolution=0.25)
+
+  result = small_gicp.align(target, source, target_tree, gt_T_target_source)
+  result = small_gicp.align(target, source, target_tree, result.T_target_source)
+
+  factors = [small_gicp.GICPFactor()]
+  rejector = small_gicp.DistanceRejector()
+
+  sum_H = numpy.zeros((6, 6))
+  sum_b = numpy.zeros(6)
+  sum_e = 0.0
+
+  for i in range(source.size()):
+    succ, H, b, e = factors[0].linearize(target, source, target_tree, result.T_target_source, i, rejector)
+    if succ:
+      sum_H += H
+      sum_b += b
+      sum_e += e
+
+  assert numpy.max(numpy.abs(result.H - sum_H) / result.H) < 0.05
+
 # Registration test
 def test_registration(load_points):
   gt_T_target_source, target_raw_numpy, source_raw_numpy = load_points