#ifndef BASIC_FUNCTIONS_H #define BASIC_FUNCTIONS_H #include "basic_types.h" #include <chrono> using namespace chrono; int8_t checkTime(const double &time, const Time_Duration &timeDuration); int8_t checkTime(const double &time, const vector<Time_Duration> &timeDurations); vector<pair<uint32_t, uint32_t>> getIndexPeriod(const uint32_t size, const uint8_t threadCnt = std::thread::hardware_concurrency()); template <typename T> Eigen::Matrix<T, 3, 1> RotationQuaternionToEulerVector( const Eigen::Quaternion<T>& quaternion) { Eigen::Quaternion<T> n_q = quaternion.normalized(); // We choose the quaternion with positive 'w', i.e., the one with a smaller // angle that represents this orientation. if (n_q.w() < 0.) { // Multiply by -1. http://eigen.tuxfamily.org/bz/show_bug.cgi?id=560 n_q.w() = -1. * n_q.w(); n_q.x() = -1. * n_q.x(); n_q.y() = -1. * n_q.y(); n_q.z() = -1. * n_q.z(); } // We convert the normalized_quaternion(n_q) into a vector along the rotation // axis with length of the rotation angle. const T roll = atan2(2 * (n_q.w() * n_q.x() + n_q.y() * n_q.z()), 1 - 2 * (n_q.x() * n_q.x() + n_q.y() * n_q.y())); const T pitch = asin(2 * (n_q.w() * n_q.y() - n_q.z() * n_q.x())); const T yaw = atan2(2 * (n_q.w() * n_q.z() + n_q.x() * n_q.y()), 1 - 2 * (n_q.y() * n_q.y() + n_q.z() * n_q.z())); return Eigen::Matrix<T, 3, 1>(roll, pitch, yaw); } template<typename ItemType> void periodProcessThreadFunc(vector<ItemType> *Vector, pair<uint32_t, uint32_t> period, boost::function<void(ItemType &)> processItemFunc) { vector<ItemType> tempOutputVec; uint32_t start_index = period.first; uint32_t index_cnt = period.second; uint32_t final_index = start_index + index_cnt; for(uint32_t index = start_index; index < final_index; index++){ processItemFunc(Vector->at(index)); } } template<typename ItemType> void multiThreadProcess(vector<ItemType> &Vector, boost::function<void(ItemType &)> processItemFunc) { auto start = system_clock::now(); vector<pair<uint32_t, uint32_t>> indexPeriod = getIndexPeriod(Vector.size()); vector<ThreadPtr> threadVec; boost::mutex mutex; for(size_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){ threadVec.emplace_back( ThreadPtr(new boost::thread(boost::bind(&periodProcessThreadFunc<ItemType>, &Vector, indexPeriod[thread_index], processItemFunc)))); } for(uint8_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){ threadVec[thread_index]->join(); threadVec[thread_index].reset(); } auto end = system_clock::now(); auto duration = duration_cast<milliseconds>(end - start); LOG(INFO) << "process take " << double(duration.count()) / 1000 << " seconds," << " output size: " << Vector.size(); } template<typename ItemType> void periodProcessThreadFunc(const vector<ItemType> *inputVector, vector<ItemType> *outputVector, // boost::mutex *mutex, pair<uint32_t, uint32_t> period, boost::function<bool(const ItemType &, ItemType &)> processItemFunc) { vector<ItemType> tempOutputVec; uint32_t start_index = period.first; uint32_t index_cnt = period.second; uint32_t final_index = start_index + index_cnt; for(uint32_t index = start_index; index < final_index; index++){ ItemType outputItem; if(processItemFunc(inputVector->at(index), outputItem)){ outputVector->push_back(outputItem); } } static int i = 0; // boost::unique_lock<boost::mutex> guard(*mutex); // outputVector->insert(outputVector->end(), // tempOutputVec.begin(), // tempOutputVec.end()); } template<typename ItemType> void multiThreadProcess(const vector<ItemType> &inputVector, vector<ItemType> &outputVector, boost::function<bool(const ItemType &, ItemType &)> processItemFunc) { auto start = system_clock::now(); vector<pair<uint32_t, uint32_t>> indexPeriod = getIndexPeriod(inputVector.size()); vector<ThreadPtr> threadVec; vector<vector<ItemType>> outputVecs(indexPeriod.size()); boost::mutex mutex; for(size_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){ threadVec.emplace_back( ThreadPtr(new boost::thread(boost::bind(&periodProcessThreadFunc<ItemType>, &inputVector, &outputVecs[thread_index], // &mutex, indexPeriod[thread_index], processItemFunc)))); } for(uint8_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){ threadVec[thread_index]->join(); threadVec[thread_index].reset(); outputVector.insert(outputVector.end(), outputVecs[thread_index].begin(), outputVecs[thread_index].end()); } auto end = system_clock::now(); auto duration = duration_cast<milliseconds>(end - start); LOG(INFO) << "process take " << double(duration.count()) / 1000 << " seconds," << " output size: " << outputVector.size(); } template<typename ItemType> void periodProcessThreadFuncNoOutput(const vector<ItemType> *inputVector_1, const vector<ItemType> *inputVector_2, pair<uint32_t, uint32_t> period, boost::function<void(const ItemType &, const ItemType &)> processItemFunc) { uint32_t start_index = period.first; uint32_t index_cnt = period.second; uint32_t final_index = start_index + index_cnt; for(uint32_t index = start_index; index < final_index; index++){ processItemFunc(inputVector_1->at(index), inputVector_2->at(index)); } LOG(INFO) << start_index / index_cnt; } template<typename ItemType> void multiThreadProcessNoOutput(const vector<ItemType> &inputVector_1, const vector<ItemType> &inputVector_2, boost::function<void(const ItemType &, const ItemType &)> processItemFunc) { auto start = system_clock::now(); if(inputVector_1.size() != inputVector_2.size()){ return; } vector<pair<uint32_t, uint32_t>> indexPeriod = getIndexPeriod(inputVector_1.size()); vector<ThreadPtr> threadVec; for(size_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){ threadVec.emplace_back( ThreadPtr(new boost::thread(boost::bind(&periodProcessThreadFuncNoOutput<ItemType>, &inputVector_1, &inputVector_2, indexPeriod[thread_index], processItemFunc)))); } for(uint8_t thread_index = 0; thread_index < indexPeriod.size(); thread_index++){ threadVec[thread_index]->join(); threadVec[thread_index].reset(); } auto end = system_clock::now(); auto duration = duration_cast<milliseconds>(end - start); LOG(INFO) << "process take " << double(duration.count()) / 1000 << " seconds"; } /////////////////////////////////////////////////////////////////////////////////////////////// template<typename ItemType> void syncProcessThreadFunc(const vector<ItemType> *inputVector, vector<ItemType> *outputVector, int32_t *currentIndex, // boost::mutex *mutex, boost::function<bool(const ItemType &, ItemType &)> processItemFunc) { uint32_t size = inputVector->size(); int32_t index; while(true){ { // boost::unique_lock<boost::mutex> guard(*mutex); index = ++(*currentIndex); } if((uint32_t)index >= size){ // LOG(INFO) << "index: " << index; break; } ItemType outputItem; if(processItemFunc(inputVector->at((uint32_t)index), outputItem)){ outputVector->push_back(outputItem); } } } template<typename ItemType> void syncProcess(const vector<ItemType> &inputVector, vector<ItemType> &outputVector, boost::function<bool(const ItemType &, ItemType &)> processItemFunc) { auto start = system_clock::now(); int32_t currentIndex = -1; boost::mutex mutex; vector<ThreadPtr> threadVec; vector<vector<ItemType>> outputVecs(MAX_THREAD_CNT); for(size_t thread_index = 0; thread_index < MAX_THREAD_CNT; thread_index++){ threadVec.emplace_back( ThreadPtr(new boost::thread(boost::bind(&syncProcessThreadFunc<ItemType>, &inputVector, &outputVecs[thread_index], ¤tIndex, // &mutex, processItemFunc)))); } for(uint8_t thread_index = 0; thread_index < MAX_THREAD_CNT; thread_index++){ threadVec[thread_index]->join(); threadVec[thread_index].reset(); outputVector.insert(outputVector.end(), outputVecs[thread_index].begin(), outputVecs[thread_index].end()); } auto end = system_clock::now(); auto duration = duration_cast<milliseconds>(end - start); LOG(INFO) << "process take " << double(duration.count()) / 1000 << " seconds," << " output size: " << outputVector.size(); } template<typename ItemType> void syncProcessThreadFunc(const vector<ItemType> *inputVector, int32_t *currentIndex, // boost::mutex *mutex, boost::function<void(const ItemType &)> processItemFunc) { uint32_t size = inputVector->size(); int32_t index; while(true){ { // boost::unique_lock<boost::mutex> guard(*mutex); index = ++(*currentIndex); } if((uint32_t)index >= size){ LOG(INFO) << "index: " << index; break; } processItemFunc(inputVector->at((uint32_t)index)); } } template<typename ItemType> void syncProcess(const vector<ItemType> &inputVector, boost::function<void(const ItemType &)> processItemFunc) { auto start = system_clock::now(); int32_t currentIndex = -1; boost::mutex mutex; vector<ThreadPtr> threadVec; for(size_t thread_index = 0; thread_index < MAX_THREAD_CNT; thread_index++){ threadVec.emplace_back( ThreadPtr(new boost::thread(boost::bind(&syncProcessThreadFunc<ItemType>, &inputVector, ¤tIndex, // &mutex, processItemFunc)))); } for(uint8_t thread_index = 0; thread_index < MAX_THREAD_CNT; thread_index++){ threadVec[thread_index]->join(); threadVec[thread_index].reset(); } auto end = system_clock::now(); auto duration = duration_cast<milliseconds>(end - start); LOG(INFO) << "process take " << double(duration.count()) / 1000 << " seconds"; } template<typename ItemType> void syncProcessThreadFuncNoOutput(const vector<ItemType> *inputVector_1, const vector<ItemType> *inputVector_2, int32_t *currentIndex, boost::function<void(const ItemType &, const ItemType &)> processItemFunc) { uint32_t size = inputVector_1->size(); int32_t index; while(true){ index = ++(*currentIndex); if((uint32_t)index >= size){ // LOG(INFO) << "index: " << index; break; } processItemFunc(inputVector_1->at(index), inputVector_2->at(index)); } } template<typename ItemType> void syncProcessNoOutput(const vector<ItemType> &inputVector_1, const vector<ItemType> &inputVector_2, boost::function<void(const ItemType &, const ItemType &)> processItemFunc) { auto start = system_clock::now(); if(inputVector_1.size() != inputVector_2.size()){ return; } vector<ThreadPtr> threadVec; int32_t currentIndex = -1; for(size_t thread_index = 0; thread_index < MAX_THREAD_CNT; thread_index++){ threadVec.emplace_back( ThreadPtr(new boost::thread(boost::bind(&syncProcessThreadFuncNoOutput<ItemType>, &inputVector_1, &inputVector_2, ¤tIndex, processItemFunc)))); } for(uint8_t thread_index = 0; thread_index < MAX_THREAD_CNT; thread_index++){ threadVec[thread_index]->join(); threadVec[thread_index].reset(); } auto end = system_clock::now(); auto duration = duration_cast<milliseconds>(end - start); LOG(INFO) << "process take " << double(duration.count()) / 1000 << " seconds"; } template <typename PointType> void addOffset(const Vector3d &offset, pcl::PointCloud<PointType> &pointcloud){ for(auto &p : pointcloud){ p.x += offset.x(); p.y += offset.y(); p.z += offset.z(); } pointcloud.sensor_origin_ += Vector4f{offset.x(), offset.y(), offset.z(), 0}; } #include <boost/filesystem.hpp> #include <boost/optional.hpp> #include <pcl/filters/voxel_grid.h> using namespace boost::filesystem; template<typename PointType> void downSamplePcd(const string &path) { using PointCloud = pcl::PointCloud<PointType>; if(!exists(path)){ return; } directory_iterator file_iter(path); directory_iterator pcd_end_iter; for(; file_iter != pcd_end_iter; file_iter++) { string subffix = file_iter->path().extension().string(); if(subffix == ".pcd"){ string stem = file_iter->path().stem().string(); auto it = stem.find("view"); if(it != string::npos){ continue; } string pcd_path = file_iter->path().string(); PointCloud pointcloud; pcl::io::loadPCDFile(pcd_path, pointcloud); typename PointCloud::Ptr input, points_filtered; pcl::VoxelGrid<PointType> grid; points_filtered.reset(new PointCloud); input.reset(new PointCloud(pointcloud)); grid.setLeafSize(0.4, 0.4, 0.4); grid.setInputCloud(input); grid.filter(*points_filtered); string output = path + "/view/"; if(!exists(output)){ create_directories(output); } pcl::io::savePCDFileBinary(output + stem +"_view.pcd", *points_filtered); LOG(INFO) << output + stem +"_view.pcd saved"; } } // string output = "/data/lidar/2147/pcd_cart_new/"; // if(!exists(output)){ // create_directories(output); // } // for(; file_iter != pcd_end_iter; file_iter++) { // string stem = file_iter->path().stem().string(); // string subffix = file_iter->path().extension().string(); // if(subffix == ".pcd"){ // string pcd_path = file_iter->path().string(); // PointCloud pointcloud; // pcl::io::loadPCDFile(pcd_path, pointcloud); // Vector3d offset = Vector3d(-1, 0, 0); // addOffset<PointType>(offset, pointcloud); // pcl::io::savePCDFileBinary(output + stem +".pcd", pointcloud); // LOG(INFO) << output + stem +".pcd saved"; // } // } } #endif // BASIC_FUNCTIONS_H