| 73 | } |
| 74 | |
| 75 | void |
| 76 | transform (const pcl::PCLPointCloud2::ConstPtr &input, pcl::PCLPointCloud2 &output) |
| 77 | { |
| 78 | // Check for 'normals' |
| 79 | bool has_normals = false; |
| 80 | for (const auto &field : input->fields) |
| 81 | if (field.name == "normals") |
| 82 | has_normals = true; |
| 83 | |
| 84 | // Estimate |
| 85 | TicToc tt; |
| 86 | tt.tic (); |
| 87 | print_highlight (stderr, "Transforming "); |
| 88 | |
| 89 | // Convert data to PointCloud<T> |
| 90 | if (has_normals) |
| 91 | { |
| 92 | PointCloud<PointNormal> xyznormals; |
| 93 | fromPCLPointCloud2 (*input, xyznormals); |
| 94 | pcl::transformPointCloud<PointNormal> (xyznormals, xyznormals, translation.head<3> (), orientation); |
| 95 | // Copy back the xyz and normals |
| 96 | pcl::PCLPointCloud2 output_xyznormals; |
| 97 | toPCLPointCloud2 (xyznormals, output_xyznormals); |
| 98 | concatenateFields (*input, output_xyznormals, output); |
| 99 | } |
| 100 | else |
| 101 | { |
| 102 | PointCloud<PointXYZ> xyz; |
| 103 | fromPCLPointCloud2 (*input, xyz); |
| 104 | pcl::transformPointCloud<PointXYZ> (xyz, xyz, translation.head<3> (), orientation); |
| 105 | // Copy back the xyz and normals |
| 106 | pcl::PCLPointCloud2 output_xyz; |
| 107 | toPCLPointCloud2 (xyz, output_xyz); |
| 108 | concatenateFields (*input, output_xyz, output); |
| 109 | } |
| 110 | |
| 111 | translation = Eigen::Vector4f::Zero (); |
| 112 | orientation = Eigen::Quaternionf::Identity (); |
| 113 | |
| 114 | print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n"); |
| 115 | } |
| 116 | |
| 117 | void |
| 118 | saveCloud (const std::string &filename, const pcl::PCLPointCloud2 &output) |