| 204 | } |
| 205 | |
| 206 | void |
| 207 | scaleInPlace (pcl::PCLPointCloud2 &cloud, double* multiplier) |
| 208 | { |
| 209 | // Obtain the x, y, and z indices |
| 210 | int x_idx = pcl::getFieldIndex (cloud, "x"); |
| 211 | int y_idx = pcl::getFieldIndex (cloud, "y"); |
| 212 | int z_idx = pcl::getFieldIndex (cloud, "z"); |
| 213 | Eigen::Array3i xyz_offset (cloud.fields[x_idx].offset, cloud.fields[y_idx].offset, cloud.fields[z_idx].offset); |
| 214 | |
| 215 | if (cloud.fields[x_idx].datatype == pcl::PCLPointField::BOOL) { |
| 216 | PCL_WARN("Datatype of point was deduced as boolean. Please check, there might be " |
| 217 | "an error somewhere"); |
| 218 | } |
| 219 | |
| 220 | for (uindex_t cp = 0; cp < cloud.width * cloud.height; ++cp) |
| 221 | { |
| 222 | // Assume all 3 fields are the same (XYZ) |
| 223 | assert ((cloud.fields[x_idx].datatype == cloud.fields[y_idx].datatype)); |
| 224 | assert ((cloud.fields[x_idx].datatype == cloud.fields[z_idx].datatype)); |
| 225 | #define MULTIPLY(CASE_LABEL) \ |
| 226 | case CASE_LABEL: { \ |
| 227 | for (int i = 0; i < 3; ++i) \ |
| 228 | multiply<pcl::traits::asType_t<(CASE_LABEL)>>( \ |
| 229 | cloud, xyz_offset[i], multiplier[i]); \ |
| 230 | break; \ |
| 231 | } |
| 232 | switch (cloud.fields[x_idx].datatype) |
| 233 | { |
| 234 | MULTIPLY(pcl::PCLPointField::BOOL) |
| 235 | MULTIPLY(pcl::PCLPointField::INT8) |
| 236 | MULTIPLY(pcl::PCLPointField::UINT8) |
| 237 | MULTIPLY(pcl::PCLPointField::INT16) |
| 238 | MULTIPLY(pcl::PCLPointField::UINT16) |
| 239 | MULTIPLY(pcl::PCLPointField::INT32) |
| 240 | MULTIPLY(pcl::PCLPointField::UINT32) |
| 241 | MULTIPLY(pcl::PCLPointField::INT64) |
| 242 | MULTIPLY(pcl::PCLPointField::UINT64) |
| 243 | MULTIPLY(pcl::PCLPointField::FLOAT32) |
| 244 | MULTIPLY(pcl::PCLPointField::FLOAT64) |
| 245 | } |
| 246 | #undef MULTIPLY |
| 247 | xyz_offset += cloud.point_step; |
| 248 | } |
| 249 | } |
| 250 | |
| 251 | |
| 252 | /* ---[ */ |