|
@@ -9,8 +9,7 @@
|
|
|
#include <time.h>
|
|
|
#include <sys/time.h>
|
|
|
#include <chrono>
|
|
|
-using namespace std;
|
|
|
-using namespace chrono;
|
|
|
+#include <thread>
|
|
|
|
|
|
void save_rgb_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZRGB>& cloud)
|
|
|
{
|
|
@@ -86,7 +85,7 @@ Error_manager Point_sift_segmentation::init(std::string graph, std::string cpkt)
|
|
|
float* cloud_data = (float*)malloc(m_point_num * 3 * sizeof(float));
|
|
|
float* output = (float*)malloc(m_point_num * m_cls_num * sizeof(float));
|
|
|
|
|
|
- auto start = system_clock::now();
|
|
|
+ auto start = std::chrono::system_clock::now();
|
|
|
|
|
|
if (false == Predict(cloud_data, output))
|
|
|
{
|
|
@@ -102,10 +101,10 @@ Error_manager Point_sift_segmentation::init(std::string graph, std::string cpkt)
|
|
|
free(output);
|
|
|
|
|
|
}
|
|
|
- auto end = system_clock::now();
|
|
|
- auto duration = duration_cast<microseconds>(end - start);
|
|
|
- cout << "花费了"
|
|
|
- << double(duration.count()) * microseconds::period::num / microseconds::period::den << "秒" << endl;
|
|
|
+ auto end = std::chrono::system_clock::now();
|
|
|
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
|
|
|
+ std::cout << "花费了"
|
|
|
+ << double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den << "秒" << std::endl;
|
|
|
|
|
|
|
|
|
return SUCCESS;
|
|
@@ -116,7 +115,7 @@ Error_manager Point_sift_segmentation::init(std::string graph, std::string cpkt)
|
|
|
//分割函数,输入点云,输出各类的点云,并保存中间文件方便查看识别结果.
|
|
|
//cloud:输入点云
|
|
|
//clouds:输出分类好的多个点云(附带颜色,红色为杂物,绿色为车,白色为地面)(此时里面没有内存)
|
|
|
-Error_manager Point_sift_segmentation::segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
|
|
|
+Error_manager Point_sift_segmentation::segmentation(int lidar_id, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
|
|
|
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector,
|
|
|
bool save_flag, std::string save_dir)
|
|
|
{
|
|
@@ -178,6 +177,10 @@ Error_manager Point_sift_segmentation::segmentation(pcl::PointCloud<pcl::PointXY
|
|
|
}
|
|
|
std::cout << "p_cloud_select = "<<p_cloud_select->size() << std::endl;
|
|
|
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ /*
|
|
|
//第六步
|
|
|
//把pcl点云转化为float, 因为TensorFlow算法只能识别float
|
|
|
//提前分配内存, 后续记得回收....
|
|
@@ -209,6 +212,77 @@ Error_manager Point_sift_segmentation::segmentation(pcl::PointCloud<pcl::PointXY
|
|
|
<< double(duration.count()) * microseconds::period::num / microseconds::period::den << "秒" << endl;
|
|
|
cout << double(duration.count()) << " "
|
|
|
<< microseconds::period::num << " " << microseconds::period::den << "秒" << endl;
|
|
|
+*/
|
|
|
+
|
|
|
+ //重写6和7步, 将TensorFlow算法放在服务器上, 使用socket通信去发送点云并获取结果
|
|
|
+ //第六步
|
|
|
+ //把pcl点云转化为float, 因为TensorFlow算法只能识别float
|
|
|
+ //提前分配内存, 后续记得回收....
|
|
|
+ float* p_data_point = (float*)malloc(m_point_num * 3 * sizeof(float)); //8192*3 float, 8192个三维点
|
|
|
+ int* p_data_type = (int*)malloc(m_point_num*sizeof(int)); //8192*2 float, 8192个类别百分比(车轮和车身2类)
|
|
|
+ memset(p_data_point, 0, m_point_num * 3 * sizeof(float));
|
|
|
+ memset(p_data_type, 0, m_point_num * sizeof(int));
|
|
|
+ //将点云数据转换到float*
|
|
|
+ t_error = translate_cloud_to_float(p_cloud_select, p_data_point);
|
|
|
+ if ( t_error != Error_code::SUCCESS )
|
|
|
+ {
|
|
|
+ free(p_data_point);
|
|
|
+ free(p_data_type);
|
|
|
+ return t_error;
|
|
|
+ }
|
|
|
+
|
|
|
+ //第七步
|
|
|
+ //tensonflow预测点云, 计算出每个三维点的类别百分比,
|
|
|
+ auto start = std::chrono::system_clock::now();
|
|
|
+ //预测, 发送socket通信, 在服务器上进行点云算法
|
|
|
+ t_error = m_predict_with_network.predict_for_send(lidar_id, p_data_point);
|
|
|
+ if ( t_error != Error_code::SUCCESS )
|
|
|
+ {
|
|
|
+ free(p_data_point);
|
|
|
+ free(p_data_type);
|
|
|
+ return t_error;
|
|
|
+ }
|
|
|
+
|
|
|
+ //循环接受, 超时等待5000ms, (后续可以在这里加上强制取消)
|
|
|
+ while (std::chrono::system_clock::now() - start < std::chrono::milliseconds(5000))
|
|
|
+ {
|
|
|
+ t_error = m_predict_with_network.predict_for_receive(lidar_id, p_data_type);
|
|
|
+ if ( t_error == Error_code::SUCCESS )
|
|
|
+ {
|
|
|
+ //算法成功, 直接退出循环即可
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ else if ( t_error == Error_code::NODATA )
|
|
|
+ {
|
|
|
+ //继续循环, 延迟1us
|
|
|
+ std::this_thread::yield();
|
|
|
+ std::this_thread::sleep_for(std::chrono::microseconds(1));
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ free(p_data_point);
|
|
|
+ free(p_data_type);
|
|
|
+ return t_error;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if ( std::chrono::system_clock::now() - start >= std::chrono::milliseconds(5000) )
|
|
|
+ {
|
|
|
+ free(p_data_point);
|
|
|
+ free(p_data_type);
|
|
|
+ return Error_manager(Error_code::LOCATER_SIFT_PREDICT_TIMEOUT, Error_level::MINOR_ERROR,
|
|
|
+ " m_predict_with_network.predict_for_receive error ");
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ auto end = std::chrono::system_clock::now();
|
|
|
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
|
|
|
+ std::cout << "花费了"
|
|
|
+ << double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den << "秒" << std::endl;
|
|
|
+ std::cout << double(duration.count()) << " "
|
|
|
+ << std::chrono::microseconds::period::num << " " << std::chrono::microseconds::period::den << "秒" << std::endl;
|
|
|
+
|
|
|
+
|
|
|
|
|
|
//第八步
|
|
|
//恢复点云,并填充到cloud_vector
|
|
@@ -242,6 +316,11 @@ Error_manager Point_sift_segmentation::segmentation(pcl::PointCloud<pcl::PointXY
|
|
|
}
|
|
|
|
|
|
|
|
|
+Predict_with_network& Point_sift_segmentation::get_predict_with_network()
|
|
|
+{
|
|
|
+ return m_predict_with_network;
|
|
|
+}
|
|
|
+
|
|
|
|
|
|
//使用体素网格, 抽稀点云, //将点云空间分为小方格,每个小方格只保留一个点.
|
|
|
Error_manager Point_sift_segmentation::filter_cloud_with_voxel_grid(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
|
|
@@ -334,7 +413,7 @@ Error_manager Point_sift_segmentation::filter_cloud_with_my_grid(pcl::PointCloud
|
|
|
// std::cout << "---------------------------"<< std::endl;
|
|
|
|
|
|
//创建网格->三维点的map, 每个网格里面只保留一个点,
|
|
|
- map<int, pcl::PointXYZ> grid_point_map;
|
|
|
+ std::map<int, pcl::PointXYZ> grid_point_map;
|
|
|
//遍历输入点云, 将每个点的索引编号写入对应的网格.
|
|
|
for (int i = 0; i < p_cloud->size(); ++i)
|
|
|
{
|
|
@@ -492,7 +571,7 @@ Error_manager Point_sift_segmentation::translate_cloud_to_float(pcl::PointCloud<
|
|
|
}
|
|
|
|
|
|
//恢复点云, 将float*转换成点云
|
|
|
-//p_data_type:输入点云对应的类别,大小为 点数*类别
|
|
|
+//p_data_type:输入点云对应的类别, 大小为 点数*m_cls_num*float
|
|
|
//p_data_point:输入点云数据(xyz)
|
|
|
//cloud_vector::输出带颜色的点云vector
|
|
|
Error_manager Point_sift_segmentation::recovery_float_to_cloud(float* p_data_type, float* p_data_point, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector)
|
|
@@ -579,6 +658,83 @@ Error_manager Point_sift_segmentation::recovery_float_to_cloud(float* p_data_typ
|
|
|
return Error_code::SUCCESS;
|
|
|
}
|
|
|
|
|
|
+//恢复点云, 将float*转换成点云
|
|
|
+//p_data_type:输入点云对应的类别(取值 0 ~ m_cls_num-1 ),大小为 点数*int
|
|
|
+//p_data_point:输入点云数据(xyz)
|
|
|
+//cloud_vector::输出带颜色的点云vector
|
|
|
+Error_manager Point_sift_segmentation::recovery_float_to_cloud(int* p_data_type, float* p_data_point, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector)
|
|
|
+{
|
|
|
+ if ( p_data_type == NULL || p_data_point == NULL )
|
|
|
+ {
|
|
|
+ return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
|
|
|
+ " recovery_float_to_cloud p_data_type or p_data_point POINTER_IS_NULL ");
|
|
|
+ }
|
|
|
+
|
|
|
+ //为cloud_vector 添加m_cls_num个点云, 提前分配内存
|
|
|
+ for (int k = 0; k < m_cls_num; ++k)
|
|
|
+ {
|
|
|
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
|
|
|
+ cloud_vector.push_back(cloud_rgb);
|
|
|
+ }
|
|
|
+
|
|
|
+ //遍历data数据,
|
|
|
+ for (int i = 0; i < m_point_num; ++i)
|
|
|
+ {
|
|
|
+ pcl::PointXYZRGB t_point;
|
|
|
+ t_point.x = *(p_data_point + i * 3);
|
|
|
+ t_point.y = *(p_data_point + i * 3 + 1);
|
|
|
+ t_point.z = *(p_data_point + i * 3 + 2);
|
|
|
+ if (t_point.x > m_maxp.x || t_point.x<m_minp.x
|
|
|
+ || t_point.y>m_maxp.y || t_point.y<m_minp.y
|
|
|
+ || t_point.z>m_maxp.z || t_point.z < m_minp.z)
|
|
|
+ {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+
|
|
|
+ //当前点 的类别, 初始化为0
|
|
|
+ int t_cls = p_data_type[i];
|
|
|
+
|
|
|
+ //根据点的类别, 添加颜色, 并且分别加入到各自的点云里面
|
|
|
+ switch ( t_cls )
|
|
|
+ {
|
|
|
+ case 0: //第0类, 绿色, 轮胎
|
|
|
+ t_point.r = 0;
|
|
|
+ t_point.g = 255;
|
|
|
+ t_point.b = 0;
|
|
|
+ break;
|
|
|
+ case 1://第1类, 白色, 车身
|
|
|
+ t_point.r = 255;
|
|
|
+ t_point.g = 255;
|
|
|
+ t_point.b = 255;
|
|
|
+ break;
|
|
|
+// case 2:
|
|
|
+// ;
|
|
|
+// break;
|
|
|
+ default:
|
|
|
+
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ cloud_vector[t_cls]->push_back(t_point);
|
|
|
+ }
|
|
|
+
|
|
|
+ //校验点云的数量, 要求size>0
|
|
|
+ if(cloud_vector[0]->size() <=0)
|
|
|
+ {
|
|
|
+ return Error_manager(Error_code::LOCATER_SIFT_PREDICT_NO_WHEEL_POINT, Error_level::MINOR_ERROR,
|
|
|
+ " recovery_float_to_cloud NO_WHEEL_POINT ");
|
|
|
+ }
|
|
|
+ if(cloud_vector[1]->size() <=0)
|
|
|
+ {
|
|
|
+ return Error_manager(Error_code::LOCATER_SIFT_PREDICT_NO_CAR_POINT, Error_level::MINOR_ERROR,
|
|
|
+ " recovery_float_to_cloud NO_CAR_POINT ");
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ return Error_code::SUCCESS;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
//保存点云数据
|
|
|
Error_manager Point_sift_segmentation::save_cloud(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector,std::string save_dir)
|
|
|
{
|