Fix #19

beltransen · Feb 27, 2019 · 79aab46 · 79aab46
1 parent a432a16
commit 79aab46
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 8 deletions.
diff --git a/include/velo2cam_utils.h b/include/velo2cam_utils.h
@@ -210,15 +210,15 @@ void getCenterClusters(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointC
   }
 }
 
-Eigen::Affine3d getRotationMatrix(Eigen::Vector3d source, Eigen::Vector3d target){
-  Eigen::Vector3d rotation_vector = target.cross(source);
+Eigen::Affine3f getRotationMatrix(Eigen::Vector3f source, Eigen::Vector3f target){
+  Eigen::Vector3f rotation_vector = target.cross(source);
   rotation_vector.normalize();
   double theta = acos(source[2]/sqrt( pow(source[0],2)+ pow(source[1],2) + pow(source[2],2)));
 
   if(DEBUG) ROS_INFO("Rot. vector: (%lf %lf %lf) / Angle: %lf", rotation_vector[0], rotation_vector[1], rotation_vector[2], theta);
 
-  Eigen::Matrix3d rotation = Eigen::AngleAxis<double>(theta, rotation_vector) * Eigen::Scaling(1.0);
-  Eigen::Affine3d rot(rotation);
+  Eigen::Matrix3f rotation = Eigen::AngleAxis<float>(theta, rotation_vector) * Eigen::Scaling(1.0f);
+  Eigen::Affine3f rot(rotation);
   return rot;
 }
 

diff --git a/src/laser_pattern.cpp b/src/laser_pattern.cpp
@@ -200,7 +200,7 @@ void callback(const PointCloud2::ConstPtr& laser_cloud){
 
   // Rotate cloud to face pattern plane
   pcl::PointCloud<pcl::PointXYZ>::Ptr xy_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-  Eigen::Vector3d xy_plane_normal_vector, floor_plane_normal_vector;
+  Eigen::Vector3f xy_plane_normal_vector, floor_plane_normal_vector;
   xy_plane_normal_vector[0] = 0.0;
   xy_plane_normal_vector[1] = 0.0;
   xy_plane_normal_vector[2] = -1.0;
@@ -209,7 +209,7 @@ void callback(const PointCloud2::ConstPtr& laser_cloud){
   floor_plane_normal_vector[1] = coefficients->values[1];
   floor_plane_normal_vector[2] = coefficients->values[2];
 
-  Eigen::Affine3d rotation = getRotationMatrix(floor_plane_normal_vector, xy_plane_normal_vector);
+  Eigen::Affine3f rotation = getRotationMatrix(floor_plane_normal_vector, xy_plane_normal_vector);
   pcl::transformPointCloud(*circles_cloud, *xy_cloud, rotation);
 
   pcl::PointCloud<pcl::PointXYZ>::Ptr aux_cloud(new pcl::PointCloud<pcl::PointXYZ>);

diff --git a/src/stereo_pattern.cpp b/src/stereo_pattern.cpp
@@ -236,7 +236,7 @@ void callback(const PointCloud2::ConstPtr& camera_cloud,
 
   // 3.ROTATE CLOUD TO FACE PATTERN PLANE
   pcl::PointCloud<pcl::PointXYZ>::Ptr xy_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-  Eigen::Vector3d xy_plane_normal_vector, floor_plane_normal_vector;
+  Eigen::Vector3f xy_plane_normal_vector, floor_plane_normal_vector;
   xy_plane_normal_vector[0] = 0.0;
   xy_plane_normal_vector[1] = 0.0;
   xy_plane_normal_vector[2] = -1.0;
@@ -248,7 +248,7 @@ void callback(const PointCloud2::ConstPtr& camera_cloud,
   std::vector<int> indices;
   pcl::removeNaNFromPointCloud (*cam_plane_cloud, *cam_plane_cloud, indices);
 
-  Eigen::Affine3d rotation = getRotationMatrix(floor_plane_normal_vector, xy_plane_normal_vector);
+  Eigen::Affine3f rotation = getRotationMatrix(floor_plane_normal_vector, xy_plane_normal_vector);
   pcl::transformPointCloud(*cam_plane_cloud, *xy_cloud, rotation);
 
   pcl::PointCloud<pcl::PointXYZ>::Ptr aux_cloud(new pcl::PointCloud<pcl::PointXYZ>);