Merge pull request #1304 from jspricke/icp_test_fix

Fix registration unit tests
PointCloudLibrary · Aug 26, 2015 · 8f7bb55 · 8f7bb55
2 parents d61e697 + b21368b
commit 8f7bb55
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 3 deletions.
diff --git a/registration/src/gicp6d.cpp b/registration/src/gicp6d.cpp
@@ -191,10 +191,18 @@ namespace pcl
     mahalanobis_.resize (N, Eigen::Matrix3d::Identity ());
 
     // Compute target cloud covariance matrices
-    computeCovariances<PointTarget> (target_, tree_, *target_covariances_);
+    if ((!target_covariances_) || (target_covariances_->empty ()))
+    {
+      target_covariances_.reset (new MatricesVector);
+      computeCovariances<PointTarget> (target_, tree_, *target_covariances_);
+    }
     // Compute input cloud covariance matrices
-    computeCovariances<PointSource> (input_, tree_reciprocal_,
-        *input_covariances_);
+    if ((!input_covariances_) || (input_covariances_->empty ()))
+    {
+      input_covariances_.reset (new MatricesVector);
+      computeCovariances<PointSource> (input_, tree_reciprocal_,
+          *input_covariances_);
+    }
 
     base_transformation_ = guess;
     nr_iterations_ = 0;
@@ -312,6 +320,9 @@ namespace pcl
     final_transformation_ (0, 3) = previous_transformation_ (0, 3) + guess (0, 3);
     final_transformation_ (1, 3) = previous_transformation_ (1, 3) + guess (1, 3);
     final_transformation_ (2, 3) = previous_transformation_ (2, 3) + guess (2, 3);
+
+    // Transform the point cloud
+    pcl::transformPointCloud (*input_, output, final_transformation_);
   }
 
 }
diff --git a/test/registration/test_registration.cpp b/test/registration/test_registration.cpp
@@ -203,6 +203,7 @@ TEST (PCL, IterativeClosestPoint)
 void
 sampleRandomTransform (Eigen::Affine3f &trans, float max_angle, float max_trans)
 {
+    srand(0);
     // Sample random transform
     Eigen::Vector3f axis((float)rand() / RAND_MAX, (float)rand() / RAND_MAX, (float)rand() / RAND_MAX);
     axis.normalize();