diff --git a/include/laser/utils/laser3d_tools.h b/include/laser/utils/laser3d_tools.h index b96b709c2babc74975ad1d1f897843b403a169c0..308897875928b16390d5b0adb2a783668b71fd2a 100644 --- a/include/laser/utils/laser3d_tools.h +++ b/include/laser/utils/laser3d_tools.h @@ -40,12 +40,53 @@ typedef pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr RegistrationPtr; namespace wolf { -void loadData(std::string fname, pcl::PointCloud<pcl::PointXYZ>& cloud) +void loadData(std::string fname, pcl::PointCloud<pcl::PointXYZ> &cloud) { - pcl::io::loadPCDFile(fname, cloud); - // remove NAN points from the cloud - pcl::Indices indices; - pcl::removeNaNFromPointCloud(cloud, cloud, indices); + std::string s_pcd = ".pcd", s_bin = ".bin"; + if (fname.compare(fname.size() - 4, 4, s_pcd) == 0) + { + pcl::io::loadPCDFile(fname, cloud); + // remove NAN points from the cloud + pcl::Indices indices; + pcl::removeNaNFromPointCloud(cloud, cloud, indices); + } + else if (fname.compare(fname.size() - 4, 4, s_bin) == 0) + { // file is binary + pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(); + + FILE *stream; + stream = std::fopen(fname.c_str(), "rb"); + + + int32_t num = std::fread(NULL, sizeof(float), 10000000, stream) / 4; // size of point cloud + float *data = (float *)malloc(num * sizeof(float)); + + float *px = data + 0; + float *py = data + 1; + float *pz = data + 2; + float *pr = data + 3; + + pcl::PointXYZ tPoint; + for (int32_t i = 0; i < num; i++) + { + tPoint.x = *px; + tPoint.y = *py; + tPoint.z = *pz; + + pcl_ptr->push_back(tPoint); + // m.cloud -> push_back(*px, *py, *pz, *pr); + // point_cloud.points.push_back(tPoint(*px, *py, *pz, *pr)); + px += 4; + py += 4; + pz += 4; + pr += 4; + } + fclose(stream); + + // remove NAN points from the cloud + std::vector<int> indices; + pcl::removeNaNFromPointCloud(*pcl_ptr, *pcl_ptr, indices); + } }; // _cloud_ref: first PointCloud @@ -86,7 +127,8 @@ inline void pairAlign(const pcl::PointCloud<pcl::PointXYZ>::Ptr _cloud_ref, _transform_final = Eigen::Isometry3d(transform_pcl.cast<double>()); // DURATION -------------------------------------------------------- - auto duration = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start); + auto duration = + std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start); WOLF_INFO("Laser3d_tools.h: pairAlign: duration: ", 1e-3 * (duration).count(), " ms"); // DURATION -------------------------------------------------------- }