|
|
@@ -9,8 +9,7 @@
|
|
|
#include <pcl/filters/voxel_grid.h>
|
|
|
#include <pcl/segmentation/extract_clusters.h>
|
|
|
#include <fcntl.h>
|
|
|
-#include "publisher.h"
|
|
|
-#include "point_tool.h"
|
|
|
+
|
|
|
//欧式聚类*******************************************************
|
|
|
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> Ground_region::segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud)
|
|
|
{
|
|
|
@@ -24,24 +23,34 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> Ground_region::segmentation(pcl
|
|
|
ece.setSearchMethod(tree);
|
|
|
ece.extract(ece_inlier);
|
|
|
|
|
|
- std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation_clouds;
|
|
|
+ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation_clouds;
|
|
|
for (int i = 0; i < ece_inlier.size(); i++)
|
|
|
{
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_copy(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
std::vector<int> ece_inlier_ext = ece_inlier[i].indices;
|
|
|
- copyPointCloud(*sor_cloud, ece_inlier_ext, *cloud_copy);//按照索引提取点云数据
|
|
|
+ copyPointCloud(*sor_cloud, ece_inlier_ext, *cloud_copy); //按照索引提取点云数据
|
|
|
segmentation_clouds.push_back(cloud_copy);
|
|
|
}
|
|
|
return segmentation_clouds;
|
|
|
}
|
|
|
|
|
|
+/**
|
|
|
+ * @description: distance between two points
|
|
|
+ * @param {Point2f} p1
|
|
|
+ * @param {Point2f} p2
|
|
|
+ * @return the distance
|
|
|
+ */
|
|
|
double Ground_region::distance(cv::Point2f p1, cv::Point2f p2)
|
|
|
{
|
|
|
return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
|
|
|
}
|
|
|
|
|
|
-
|
|
|
-bool Ground_region::isRect(std::vector<cv::Point2f>& points)
|
|
|
+/**
|
|
|
+ * @description: point rectangle detect
|
|
|
+ * @param points detect if points obey the rectangle rule
|
|
|
+ * @return wether forms a rectangle
|
|
|
+ */
|
|
|
+bool Ground_region::isRect(std::vector<cv::Point2f> &points)
|
|
|
{
|
|
|
if (points.size() == 4)
|
|
|
{
|
|
|
@@ -54,7 +63,8 @@ bool Ground_region::isRect(std::vector<cv::Point2f>& points)
|
|
|
double l2 = L[2];
|
|
|
cv::Point2f ps = points[0], pt = points[1];
|
|
|
cv::Point2f pc = points[2];
|
|
|
- for (int i = 1; i < 3; ++i) {
|
|
|
+ for (int i = 1; i < 3; ++i)
|
|
|
+ {
|
|
|
if (L[i] > max_l)
|
|
|
{
|
|
|
max_l = L[i];
|
|
|
@@ -67,26 +77,27 @@ bool Ground_region::isRect(std::vector<cv::Point2f>& points)
|
|
|
}
|
|
|
|
|
|
//直角边与坐标轴的夹角 <20°
|
|
|
- float thresh=20.0*M_PI/180.0;
|
|
|
- cv::Point2f vct(pt.x-pc.x,pt.y-pc.y);
|
|
|
- float angle=atan2(vct.y,vct.x);
|
|
|
- if(!(fabs(angle)<thresh || (M_PI_2-fabs(angle)<thresh)))
|
|
|
+ float thresh = 20.0 * M_PI / 180.0;
|
|
|
+ cv::Point2f vct(pt.x - pc.x, pt.y - pc.y);
|
|
|
+ float angle = atan2(vct.y, vct.x);
|
|
|
+ if (!(fabs(angle) < thresh || (M_PI_2 - fabs(angle) < thresh)))
|
|
|
{
|
|
|
//std::cout<<" 4 wheel axis angle : "<<angle<<std::endl;
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
|
|
|
- if (fabs(cosa) >= 0.15) {
|
|
|
+ if (fabs(cosa) >= 0.15)
|
|
|
+ {
|
|
|
/*char description[255]={0};
|
|
|
sprintf(description,"angle cos value(%.2f) >0.13 ",cosa);
|
|
|
std::cout<<description<<std::endl;*/
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
- float width=std::min(l1,l2);
|
|
|
- float length=std::max(l1,l2);
|
|
|
- if(width<1.400 || width >1.900 || length >3.300 ||length < 2.200)
|
|
|
+ float width = std::min(l1, l2);
|
|
|
+ float length = std::max(l1, l2);
|
|
|
+ if (width < 1.400 || width > 1.900 || length > 3.300 || length < 2.200)
|
|
|
{
|
|
|
/*char description[255]={0};
|
|
|
sprintf(description,"width<1400 || width >1900 || length >3300 ||length < 2200 l:%.1f,w:%.1f",length,width);
|
|
|
@@ -97,7 +108,8 @@ bool Ground_region::isRect(std::vector<cv::Point2f>& points)
|
|
|
double d = distance(pc, points[3]);
|
|
|
cv::Point2f center1 = (ps + pt) * 0.5;
|
|
|
cv::Point2f center2 = (pc + points[3]) * 0.5;
|
|
|
- if (fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150) {
|
|
|
+ if (fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150)
|
|
|
+ {
|
|
|
/*std::cout << "d:" << d << " maxl:" << max_l << " center1:" << center1 << " center2:" << center2<<std::endl;
|
|
|
char description[255]={0};
|
|
|
sprintf(description,"Verify failed-4 fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150 ");
|
|
|
@@ -106,10 +118,9 @@ bool Ground_region::isRect(std::vector<cv::Point2f>& points)
|
|
|
}
|
|
|
//std::cout << "d:" << d << " maxl:" << max_l << " center1:" << center1 << " center2:" << center2<<std::endl;
|
|
|
|
|
|
-
|
|
|
return true;
|
|
|
}
|
|
|
- else if(points.size()==3)
|
|
|
+ else if (points.size() == 3)
|
|
|
{
|
|
|
double L[3] = {0.0};
|
|
|
L[0] = distance(points[0], points[1]);
|
|
|
@@ -121,8 +132,10 @@ bool Ground_region::isRect(std::vector<cv::Point2f>& points)
|
|
|
int max_index = 0;
|
|
|
cv::Point2f ps = points[0], pt = points[1];
|
|
|
cv::Point2f pc = points[2];
|
|
|
- for (int i = 1; i < 3; ++i) {
|
|
|
- if (L[i] > max_l) {
|
|
|
+ for (int i = 1; i < 3; ++i)
|
|
|
+ {
|
|
|
+ if (L[i] > max_l)
|
|
|
+ {
|
|
|
max_index = i;
|
|
|
max_l = L[i];
|
|
|
l1 = L[abs(i + 1) % 3];
|
|
|
@@ -134,31 +147,32 @@ bool Ground_region::isRect(std::vector<cv::Point2f>& points)
|
|
|
}
|
|
|
|
|
|
//直角边与坐标轴的夹角 <20°
|
|
|
- float thresh=20.0*M_PI/180.0;
|
|
|
- cv::Point2f vct(pt.x-pc.x,pt.y-pc.y);
|
|
|
- float angle=atan2(vct.y,vct.x);
|
|
|
- if(!(fabs(angle)<thresh || (M_PI_2-fabs(angle)<thresh)))
|
|
|
+ float thresh = 20.0 * M_PI / 180.0;
|
|
|
+ cv::Point2f vct(pt.x - pc.x, pt.y - pc.y);
|
|
|
+ float angle = atan2(vct.y, vct.x);
|
|
|
+ if (!(fabs(angle) < thresh || (M_PI_2 - fabs(angle) < thresh)))
|
|
|
{
|
|
|
//std::cout<<" 4 wheel axis angle : "<<angle<<std::endl;
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
|
|
|
- if (fabs(cosa) >= 0.15) {
|
|
|
+ if (fabs(cosa) >= 0.15)
|
|
|
+ {
|
|
|
/*char description[255]={0};
|
|
|
sprintf(description,"3 wheels angle cos value(%.2f) >0.13 ",cosa);
|
|
|
std::cout<<description<<std::endl;*/
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
- double l=std::max(l1,l2);
|
|
|
- double w=std::min(l1,l2);
|
|
|
- if(l>2.100 && l<3.300 && w>1.400 && w<2.100)
|
|
|
+ double l = std::max(l1, l2);
|
|
|
+ double w = std::min(l1, l2);
|
|
|
+ if (l > 2.100 && l < 3.300 && w > 1.400 && w < 2.100)
|
|
|
{
|
|
|
//生成第四个点
|
|
|
- cv::Point2f vec1=ps-pc;
|
|
|
- cv::Point2f vec2=pt-pc;
|
|
|
- cv::Point2f point4=(vec1+vec2)+pc;
|
|
|
+ cv::Point2f vec1 = ps - pc;
|
|
|
+ cv::Point2f vec2 = pt - pc;
|
|
|
+ cv::Point2f point4 = (vec1 + vec2) + pc;
|
|
|
points.push_back(point4);
|
|
|
|
|
|
/*char description[255]={0};
|
|
|
@@ -174,70 +188,75 @@ bool Ground_region::isRect(std::vector<cv::Point2f>& points)
|
|
|
std::cout<<description<<std::endl;*/
|
|
|
return false;
|
|
|
}
|
|
|
-
|
|
|
}
|
|
|
//std::cout<<" default false"<<std::endl;
|
|
|
return false;
|
|
|
-
|
|
|
}
|
|
|
-bool Ground_region::classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,double thresh_z,
|
|
|
- detect_wheel_ceres3d::Detect_result& result)
|
|
|
+
|
|
|
+/**
|
|
|
+ * @description: 3d wheel detect core func
|
|
|
+ * @param cloud input cloud for measure
|
|
|
+ * @param thresh_z z value to cut wheel
|
|
|
+ * @param result detect result
|
|
|
+ * @return wether successfully detected
|
|
|
+ */
|
|
|
+bool Ground_region::classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double thresh_z,
|
|
|
+ detect_wheel_ceres3d::Detect_result &result)
|
|
|
{
|
|
|
- if(m_detector== nullptr)
|
|
|
+ if (m_detector == nullptr)
|
|
|
return false;
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
- for(int i=0;i<cloud->size();++i)
|
|
|
+ for (int i = 0; i < cloud->size(); ++i)
|
|
|
{
|
|
|
- pcl::PointXYZ pt=cloud->points[i];
|
|
|
- if(pt.z< thresh_z)
|
|
|
+ pcl::PointXYZ pt = cloud->points[i];
|
|
|
+ if (pt.z < thresh_z)
|
|
|
{
|
|
|
cloud_filtered->push_back(pt);
|
|
|
}
|
|
|
}
|
|
|
//下采样
|
|
|
- pcl::VoxelGrid<pcl::PointXYZ> vox; //创建滤波对象
|
|
|
- vox.setInputCloud(cloud_filtered); //设置需要过滤的点云给滤波对象
|
|
|
- vox.setLeafSize(0.02f, 0.02f, 0.2f); //设置滤波时创建的体素体积为1cm的立方体
|
|
|
+ pcl::VoxelGrid<pcl::PointXYZ> vox; //创建滤波对象
|
|
|
+ vox.setInputCloud(cloud_filtered); //设置需要过滤的点云给滤波对象
|
|
|
+ vox.setLeafSize(0.02f, 0.02f, 0.2f); //设置滤波时创建的体素体积为1cm的立方体
|
|
|
vox.filter(*cloud_filtered); //执行滤波处理，存储输出
|
|
|
- if(cloud_filtered->size()==0)
|
|
|
+ if (cloud_filtered->size() == 0)
|
|
|
{
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
- if(cloud_filtered->size()==0)
|
|
|
+ if (cloud_filtered->size() == 0)
|
|
|
return false;
|
|
|
- pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; //创建滤波器对象
|
|
|
- sor.setInputCloud (cloud_filtered); //设置待滤波的点云
|
|
|
- sor.setMeanK (5); //设置在进行统计时考虑的临近点个数
|
|
|
- sor.setStddevMulThresh (3.0); //设置判断是否为离群点的阀值，用来倍乘标准差，也就是上面的std_mul
|
|
|
- sor.filter (*cloud_filtered); //滤波结果存储到cloud_filtered
|
|
|
+ pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; //创建滤波器对象
|
|
|
+ sor.setInputCloud(cloud_filtered); //设置待滤波的点云
|
|
|
+ sor.setMeanK(5); //设置在进行统计时考虑的临近点个数
|
|
|
+ sor.setStddevMulThresh(3.0); //设置判断是否为离群点的阀值，用来倍乘标准差，也就是上面的std_mul
|
|
|
+ sor.filter(*cloud_filtered); //滤波结果存储到cloud_filtered
|
|
|
|
|
|
- if(cloud_filtered->size()==0)
|
|
|
+ if (cloud_filtered->size() == 0)
|
|
|
{
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
|
|
|
- seg_clouds=segmentation(cloud_filtered);
|
|
|
+ seg_clouds = segmentation(cloud_filtered);
|
|
|
|
|
|
- if(!(seg_clouds.size()==4 || seg_clouds.size()==3))
|
|
|
+ if (!(seg_clouds.size() == 4 || seg_clouds.size() == 3))
|
|
|
{
|
|
|
return false;
|
|
|
}
|
|
|
std::vector<cv::Point2f> centers;
|
|
|
- for(int i=0;i<seg_clouds.size();++i)
|
|
|
+ for (int i = 0; i < seg_clouds.size(); ++i)
|
|
|
{
|
|
|
Eigen::Vector4f centroid;
|
|
|
pcl::compute3DCentroid(*seg_clouds[i], centroid);
|
|
|
- centers.push_back(cv::Point2f(centroid[0],centroid[1]));
|
|
|
-
|
|
|
+ centers.push_back(cv::Point2f(centroid[0], centroid[1]));
|
|
|
}
|
|
|
- bool ret= isRect(centers);
|
|
|
- if(ret)
|
|
|
+ bool ret = isRect(centers);
|
|
|
+ if (ret)
|
|
|
{
|
|
|
|
|
|
std::string error_str;
|
|
|
- if(m_detector->detect(seg_clouds,result,error_str))
|
|
|
+ if (m_detector->detect(seg_clouds, result, error_str))
|
|
|
{
|
|
|
return true;
|
|
|
}
|
|
|
@@ -249,409 +268,156 @@ bool Ground_region::classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
|
|
|
return ret;
|
|
|
}
|
|
|
|
|
|
-
|
|
|
-Ground_region::~Ground_region()
|
|
|
-{
|
|
|
- m_exit_condition.set_pass_ever(true);
|
|
|
- // Close Capturte Thread
|
|
|
- if( m_running_thread && m_running_thread->joinable() ){
|
|
|
- m_running_thread->join();
|
|
|
- m_running_thread->~thread();
|
|
|
- delete m_running_thread;
|
|
|
- m_running_thread = nullptr;
|
|
|
- }
|
|
|
-
|
|
|
- for(int i=0;i<m_lidars.size();++i)
|
|
|
- {
|
|
|
- if(m_lidars[i]!= nullptr)
|
|
|
- {
|
|
|
- m_lidars[i]->close();
|
|
|
- delete m_lidars[i];
|
|
|
- }
|
|
|
- }
|
|
|
- m_lidars.clear();
|
|
|
- if(m_detector)
|
|
|
- {
|
|
|
- delete m_detector;
|
|
|
- m_detector= nullptr;
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
-}
|
|
|
+// constructor
|
|
|
Ground_region::Ground_region()
|
|
|
{
|
|
|
- m_detector= nullptr ;
|
|
|
- m_running_thread=nullptr;
|
|
|
-}
|
|
|
-Error_manager Ground_region::init(std::string configure_file)
|
|
|
-{
|
|
|
- if(read_proto_param(configure_file,m_configure)==false)
|
|
|
- {
|
|
|
- return Error_manager(FAILED,NORMAL,"read proto failed");
|
|
|
- }
|
|
|
-
|
|
|
- pcl::PointCloud<pcl::PointXYZ>::Ptr left_model=ReadTxtCloud("/media/youchen/block_ntfs/yct/catkin_ws/src/feature_extra/left_model.txt");
|
|
|
- // std::cout << "left model:" << left_model->size() << std::endl;
|
|
|
- pcl::PointCloud<pcl::PointXYZ>::Ptr right_model = ReadTxtCloud("/media/youchen/block_ntfs/yct/catkin_ws/src/feature_extra/right_model.txt");
|
|
|
- // std::cout << "right model:" << right_model->size() << std::endl;
|
|
|
- m_detector=new detect_wheel_ceres3d(left_model,right_model);
|
|
|
- // std::cout << "????" << std::endl;
|
|
|
-
|
|
|
- return init(m_configure);
|
|
|
-
|
|
|
-}
|
|
|
-Error_manager Ground_region::init(ground_region::Configure configure)
|
|
|
-{
|
|
|
- //绑定通讯ip
|
|
|
- /*char connect_str[255]={0};
|
|
|
- sprintf(connect_str,"tcp://%s:%d",configure.communication_cfg().ip().c_str(),
|
|
|
- configure.communication_cfg().port());
|
|
|
- Publisher::get_instance_pointer()->communication_bind(connect_str);
|
|
|
- Publisher::get_instance_pointer()->communication_run();
|
|
|
-
|
|
|
- //创建雷达
|
|
|
- m_lidars.resize(configure.lidar_size());
|
|
|
- for(int i=0;i<configure.lidar_size();++i)
|
|
|
- {
|
|
|
- const boost::asio::ip::address address = boost::asio::ip::address::from_string( configure.lidar(i).ip() );
|
|
|
- const unsigned short port = configure.lidar(i).port();
|
|
|
- m_lidars[i]=new velodyne::VLP16Capture(address,port);
|
|
|
- if( !m_lidars[i]->isOpen() ){
|
|
|
- char description[255]={0};
|
|
|
- sprintf(description,"lidar [%d] open failed ip:%s ,port:%d",i,configure.lidar(i).ip().c_str(),port);
|
|
|
- return Error_manager(FAILED,NORMAL,description);
|
|
|
- }
|
|
|
- }
|
|
|
-*/
|
|
|
- m_configure=configure;
|
|
|
- m_running_thread=new std::thread(std::bind(thread_measure_func,this));
|
|
|
- return SUCCESS;
|
|
|
+ m_detector = nullptr;
|
|
|
+ m_measure_thread = nullptr;
|
|
|
}
|
|
|
|
|
|
-bool Ground_region::read_proto_param(std::string file, ::google::protobuf::Message& parameter)
|
|
|
-{
|
|
|
- int fd = open(file.c_str(), O_RDONLY);
|
|
|
- if (fd == -1) return false;
|
|
|
- FileInputStream* input = new FileInputStream(fd);
|
|
|
- bool success = google::protobuf::TextFormat::Parse(input, ¶meter);
|
|
|
- delete input;
|
|
|
- close(fd);
|
|
|
- return success;
|
|
|
-}
|
|
|
-
|
|
|
-pcl::PointCloud<pcl::PointXYZ>::Ptr Ground_region::scans2cloud(const velodyne::Frame& frame,
|
|
|
- const ground_region::Calib_parameter& calib)
|
|
|
-{
|
|
|
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
- for(int i=0;i<frame.scans.size();++i)
|
|
|
- {
|
|
|
- pcl::PointXYZ pt;
|
|
|
-
|
|
|
- }
|
|
|
- // 变换
|
|
|
- Eigen::Matrix3d rotation_matrix3 ;
|
|
|
- rotation_matrix3= Eigen::AngleAxisd(calib.r(), Eigen::Vector3d::UnitZ()) *
|
|
|
- Eigen::AngleAxisd(calib.p(), Eigen::Vector3d::UnitY()) *
|
|
|
- Eigen::AngleAxisd(calib.y(), Eigen::Vector3d::UnitX());
|
|
|
-
|
|
|
- Eigen::Affine3d transform_2 = Eigen::Affine3d::Identity();
|
|
|
- transform_2.translation() << calib.cx(),calib.cy(),calib.cz();
|
|
|
- transform_2.rotate (rotation_matrix3);
|
|
|
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transpose(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
- pcl::transformPointCloud (*cloud, *cloud_transpose, transform_2);
|
|
|
-
|
|
|
- return cloud_transpose;
|
|
|
-}
|
|
|
-
|
|
|
-Error_manager Ground_region::sync_capture(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& clouds,double timeout_second)
|
|
|
+// deconstructor
|
|
|
+Ground_region::~Ground_region()
|
|
|
{
|
|
|
- Tick timer;
|
|
|
- bool sync=false;
|
|
|
- clouds.clear();
|
|
|
- std::vector<velodyne::Frame> sync_frames;
|
|
|
- while((false==m_exit_condition.wait_for_ex(std::chrono::microseconds(1))) && sync==false)
|
|
|
- {
|
|
|
- bool sync_once=true;
|
|
|
- for(int i=0;i<m_lidars.size();++i)
|
|
|
- {
|
|
|
- velodyne::Frame frame;
|
|
|
- m_lidars[i]->get_frame(frame);
|
|
|
- if(frame.tic.tic()>0.2)
|
|
|
- {
|
|
|
- sync_frames.clear();
|
|
|
- sync_once=false;
|
|
|
- break;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- sync_frames.push_back(frame);
|
|
|
- }
|
|
|
- }
|
|
|
- if(timer.tic()>timeout_second)
|
|
|
- {
|
|
|
- return Error_manager(FAILED,NORMAL,"sync capture time out");
|
|
|
- }
|
|
|
- sync=sync_once;
|
|
|
- usleep(1);
|
|
|
- }
|
|
|
- if(sync)
|
|
|
- {
|
|
|
- for(int i=0;i<sync_frames.size();++i)
|
|
|
+ if(m_measure_thread){
|
|
|
+ m_measure_condition.kill_all();
|
|
|
+ // Close Capturte Thread
|
|
|
+ if (m_measure_thread->joinable())
|
|
|
{
|
|
|
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud=(scans2cloud(sync_frames[i],m_configure.lidar(i).calib()));
|
|
|
- clouds.push_back(cloud);
|
|
|
+ m_measure_thread->join();
|
|
|
+ delete m_measure_thread;
|
|
|
+ m_measure_thread = nullptr;
|
|
|
}
|
|
|
- return SUCCESS;
|
|
|
}
|
|
|
- return FAILED;
|
|
|
-
|
|
|
}
|
|
|
|
|
|
-Error_manager Ground_region::prehandle_cloud(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& clouds,
|
|
|
- message::Ground_measure_statu_msg& msg)
|
|
|
+Error_manager Ground_region::init(velodyne::Region region)
|
|
|
{
|
|
|
- if(msg.laser_statu_vector_size()!=m_lidars.size())
|
|
|
- return ERROR;
|
|
|
- //直通滤波
|
|
|
- ground_region::Box box = m_configure.box();
|
|
|
- std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> filtered_cloud_vector;
|
|
|
- for (int i = 0; i < clouds.size(); ++i)
|
|
|
- {
|
|
|
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
- for (int j = 0; j < clouds[i]->size(); ++j)
|
|
|
- {
|
|
|
- pcl::PointXYZ pt = clouds[i]->points[j];
|
|
|
- if (pt.x > box.minx() && pt.x < box.maxx()
|
|
|
- && pt.y > box.miny() && pt.y < box.maxy()
|
|
|
- && pt.z > box.minz() && pt.z < box.maxz())
|
|
|
- {
|
|
|
- cloud_filtered->push_back(pt);
|
|
|
- }
|
|
|
- }
|
|
|
- filtered_cloud_vector.push_back(cloud_filtered);
|
|
|
- }
|
|
|
- //根据点云判断雷达状态，若所有雷达滤波后都有点， 则正常,lidar_statu_bit 各个位代表雷达是否有点， 有点为0，无点为1
|
|
|
- int lidar_statu_bit = 0;
|
|
|
- for (int i = 0; i < filtered_cloud_vector.size(); ++i)
|
|
|
- {
|
|
|
- if (filtered_cloud_vector[i]->size() == 0)
|
|
|
- lidar_statu_bit |= (0x01 << i);
|
|
|
- else
|
|
|
- msg.set_laser_statu_vector(i, message::LASER_READY);//雷达正常
|
|
|
- }
|
|
|
- if (lidar_statu_bit == 0)
|
|
|
- {
|
|
|
- clouds = filtered_cloud_vector;
|
|
|
- return SUCCESS;
|
|
|
- }
|
|
|
-
|
|
|
- //判断是否有雷达故障
|
|
|
- for (int i = 0; i < filtered_cloud_vector.size(); ++i)
|
|
|
- {
|
|
|
- if (filtered_cloud_vector[i]->size() == 0)
|
|
|
- {
|
|
|
- //滤波之前也没有点，雷达故障
|
|
|
- if (clouds[i]->size() == 0)
|
|
|
- {
|
|
|
- clouds = filtered_cloud_vector;
|
|
|
- msg.set_laser_statu_vector(i, message::LASER_DISCONNECT);
|
|
|
- return Error_manager(DISCONNECT, NORMAL, "雷达故障");
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- //判断是不是所有雷达在该区域都没有点，表示该区域没有东西
|
|
|
- int temp = 0;
|
|
|
- if (~(((~temp) << (filtered_cloud_vector.size())) | lidar_statu_bit) == 0)
|
|
|
- {
|
|
|
- clouds = filtered_cloud_vector;
|
|
|
- return Error_manager(POINT_EMPTY, NORMAL, "区域无点");
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- clouds = filtered_cloud_vector;
|
|
|
- return Error_manager(CLOUD_INCOMPLETED, NORMAL, "点云不完整(雷达正常)");
|
|
|
- }
|
|
|
-
|
|
|
+ m_region = region;
|
|
|
+ m_measure_thread = new std::thread(&Ground_region::thread_measure_func, this);
|
|
|
+ return SUCCESS;
|
|
|
}
|
|
|
|
|
|
-
|
|
|
-bool computer_var(std::vector<double> data,double& var)
|
|
|
+// 计算均方误差
|
|
|
+bool computer_var(std::vector<double> data, double &var)
|
|
|
{
|
|
|
- if(data.size()==0)
|
|
|
+ if (data.size() == 0)
|
|
|
return false;
|
|
|
- Eigen::VectorXd dis_vec(data.size());
|
|
|
- for(int i=0;i<data.size();++i)
|
|
|
+ Eigen::VectorXd dis_vec(data.size());
|
|
|
+ for (int i = 0; i < data.size(); ++i)
|
|
|
{
|
|
|
- dis_vec[i]=data[i];
|
|
|
+ dis_vec[i] = data[i];
|
|
|
}
|
|
|
- double mean=dis_vec.mean();
|
|
|
+ double mean = dis_vec.mean();
|
|
|
Eigen::VectorXd mean_vec(data.size());
|
|
|
- Eigen::VectorXd mat=dis_vec-(mean_vec.setOnes()*mean);
|
|
|
- Eigen::MatrixXd result=(mat.transpose())*mat;
|
|
|
- var=sqrt(result(0)/double(data.size()));
|
|
|
+ Eigen::VectorXd mat = dis_vec - (mean_vec.setOnes() * mean);
|
|
|
+ Eigen::MatrixXd result = (mat.transpose()) * mat;
|
|
|
+ var = sqrt(result(0) / double(data.size()));
|
|
|
return true;
|
|
|
-
|
|
|
}
|
|
|
|
|
|
-
|
|
|
-Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
|
|
|
- const ground_region::Box& box,detect_wheel_ceres3d::Detect_result& last_result)
|
|
|
+Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, detect_wheel_ceres3d::Detect_result &last_result)
|
|
|
{
|
|
|
- if(cloud->size()==0)
|
|
|
- return Error_manager(POINT_EMPTY,NORMAL,"no point");
|
|
|
+ if (cloud->size() == 0)
|
|
|
+ return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "no point");
|
|
|
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
- for(int i=0;i<cloud->size();++i)
|
|
|
+ for (int i = 0; i < cloud->size(); ++i)
|
|
|
{
|
|
|
- pcl::PointXYZ pt=cloud->points[i];
|
|
|
- if(pt.x>box.minx()&&pt.x<box.maxx()
|
|
|
- &&pt.y>box.miny()&&pt.y<box.maxy()
|
|
|
- &&pt.z>box.minz()&&pt.z<box.maxz())
|
|
|
+ pcl::PointXYZ pt = cloud->points[i];
|
|
|
+ if (pt.x > m_region.minx() && pt.x < m_region.maxx() && pt.y > m_region.miny() && pt.y < m_region.maxy() && pt.z > m_region.minz() && pt.z < m_region.maxz())
|
|
|
{
|
|
|
cloud_filtered->push_back(pt);
|
|
|
}
|
|
|
}
|
|
|
- if(cloud_filtered->size()==0)
|
|
|
- return Error_manager(POINT_EMPTY,NORMAL,"filtered no point");
|
|
|
-
|
|
|
- float start_z=box.minz();
|
|
|
- float max_z=0.2;
|
|
|
- float center_z=(start_z+max_z)/2.0;
|
|
|
- float last_center_z=start_z;
|
|
|
- float last_succ_z=-1.0;
|
|
|
- int count=0;
|
|
|
+ if (cloud_filtered->size() == 0)
|
|
|
+ return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "filtered no point");
|
|
|
+
|
|
|
+ float start_z = m_region.minz();
|
|
|
+ float max_z = 0.2;
|
|
|
+ float center_z = (start_z + max_z) / 2.0;
|
|
|
+ float last_center_z = start_z;
|
|
|
+ float last_succ_z = -1.0;
|
|
|
+ int count = 0;
|
|
|
//二分法 找识别成功的 最高的z
|
|
|
std::vector<detect_wheel_ceres3d::Detect_result> results;
|
|
|
do
|
|
|
{
|
|
|
detect_wheel_ceres3d::Detect_result result;
|
|
|
- bool ret = classify_ceres_detect(cloud_filtered, center_z,result);
|
|
|
+ bool ret = classify_ceres_detect(cloud_filtered, center_z, result);
|
|
|
// std::cout << "z: " << center_z <<", "<<start_z<<"," << max_z << std::endl;
|
|
|
- if(ret)
|
|
|
+ if (ret)
|
|
|
{
|
|
|
results.push_back(result);
|
|
|
- last_succ_z=center_z;
|
|
|
- start_z=center_z;
|
|
|
- last_center_z=center_z;
|
|
|
-
|
|
|
+ last_succ_z = center_z;
|
|
|
+ start_z = center_z;
|
|
|
+ last_center_z = center_z;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- max_z=center_z;
|
|
|
- last_center_z=center_z;
|
|
|
-
|
|
|
+ max_z = center_z;
|
|
|
+ last_center_z = center_z;
|
|
|
}
|
|
|
- center_z=(start_z+max_z)/2.0;
|
|
|
+ center_z = (start_z + max_z) / 2.0;
|
|
|
count++;
|
|
|
-
|
|
|
+
|
|
|
} while (fabs(center_z - last_center_z) > 0.01);
|
|
|
|
|
|
//
|
|
|
- if(results.size()==0)
|
|
|
+ if (results.size() == 0)
|
|
|
{
|
|
|
- return Error_manager(FAILED,NORMAL,"nor car");
|
|
|
+ return Error_manager(FAILED, NORMAL, "no car detected");
|
|
|
}
|
|
|
/// to be
|
|
|
- float min_mean_loss=1.0;
|
|
|
- for(int i=0;i<results.size();++i)
|
|
|
+ float min_mean_loss = 1.0;
|
|
|
+ for (int i = 0; i < results.size(); ++i)
|
|
|
{
|
|
|
- detect_wheel_ceres3d::Detect_result result=results[i];
|
|
|
+ detect_wheel_ceres3d::Detect_result result = results[i];
|
|
|
std::vector<double> loss;
|
|
|
loss.push_back(result.loss.lf_loss);
|
|
|
loss.push_back(result.loss.rf_loss);
|
|
|
loss.push_back(result.loss.lb_loss);
|
|
|
loss.push_back(result.loss.rb_loss);
|
|
|
- double mean=(result.loss.lf_loss+result.loss.rf_loss+result.loss.lb_loss+result.loss.rb_loss)/4.0;
|
|
|
- double var=-1.;
|
|
|
- computer_var(loss,var);
|
|
|
- if(mean<min_mean_loss)
|
|
|
+ double mean = (result.loss.lf_loss + result.loss.rf_loss + result.loss.lb_loss + result.loss.rb_loss) / 4.0;
|
|
|
+ double var = -1.;
|
|
|
+ computer_var(loss, var);
|
|
|
+ if (mean < min_mean_loss)
|
|
|
{
|
|
|
- last_result=result;
|
|
|
- min_mean_loss=mean;
|
|
|
+ last_result = result;
|
|
|
+ min_mean_loss = mean;
|
|
|
}
|
|
|
-
|
|
|
}
|
|
|
printf("z : %.3f angle : %.3f front : %.3f wheel_base:%.3f,width:%.3f, mean:%.5f\n",
|
|
|
- center_z,last_result.theta,last_result.front_theta,last_result.wheel_base,last_result.width,
|
|
|
+ center_z, last_result.theta, last_result.front_theta, last_result.wheel_base, last_result.width,
|
|
|
min_mean_loss);
|
|
|
//m_detector->save_debug_data("/home/zx/zzw/catkin_ws/src/feature_extra/debug");
|
|
|
return SUCCESS;
|
|
|
}
|
|
|
|
|
|
#include "message/message_base.pb.h"
|
|
|
-void Ground_region::thread_measure_func(Ground_region* p)
|
|
|
+void Ground_region::thread_measure_func()
|
|
|
{
|
|
|
- return ;
|
|
|
- message::Ground_measure_statu_msg msg;
|
|
|
- message::Base_info base_info;
|
|
|
- base_info.set_msg_type(message::eGround_measure_statu_msg);
|
|
|
- base_info.set_sender(message::eEmpty);
|
|
|
- base_info.set_receiver(message::eEmpty);
|
|
|
- msg.mutable_base_info()->CopyFrom(base_info);
|
|
|
- msg.set_ground_statu(message::Nothing);
|
|
|
- const float fps=10.;
|
|
|
//保持 10 fps
|
|
|
- while(false==p->m_exit_condition.wait_for_millisecond(int(1000./fps)))
|
|
|
+ while (m_measure_condition.is_alive())
|
|
|
{
|
|
|
- for(int i=0;i<p->m_lidars.size();++i)
|
|
|
+ m_measure_condition.wait();
|
|
|
+ if (m_measure_condition.is_alive())
|
|
|
{
|
|
|
- msg.add_laser_statu_vector(
|
|
|
- p->m_lidars[i]->isRun()?message::LASER_READY:message::LASER_DISCONNECT);
|
|
|
+ // to be 发布结果
|
|
|
+ msg.set_ground_statu(message::Car_correct);
|
|
|
+ message::Locate_information locate_information;
|
|
|
+ locate_information.set_locate_x(result.cx);
|
|
|
+ locate_information.set_locate_y(result.cy);
|
|
|
+ locate_information.set_locate_angle(result.theta);
|
|
|
+ locate_information.set_locate_wheel_base(result.wheel_base);
|
|
|
+ locate_information.set_locate_length(result.wheel_base);
|
|
|
+ locate_information.set_locate_wheel_width(result.width);
|
|
|
+ locate_information.set_locate_width(result.body_width);
|
|
|
+ locate_information.set_locate_front_theta(result.front_theta);
|
|
|
+ locate_information.set_locate_correct(true);
|
|
|
+
|
|
|
+ Communication_message c_msg;
|
|
|
+ c_msg.reset(msg.base_info(), msg.SerializeAsString());
|
|
|
+ Publisher::get_instance_pointer()->publish_msg(&c_msg);
|
|
|
}
|
|
|
-
|
|
|
- std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
|
|
|
- Error_manager code=p->sync_capture(clouds,0.5);
|
|
|
- if(code!=SUCCESS)
|
|
|
- {
|
|
|
- LOG(WARNING)<<code.get_error_description();
|
|
|
- continue;
|
|
|
- }
|
|
|
-
|
|
|
- Tick tick;
|
|
|
- code=p->prehandle_cloud(clouds,msg);
|
|
|
- if(code!=SUCCESS)
|
|
|
- {
|
|
|
- LOG(WARNING)<<code.get_error_description();
|
|
|
- continue;
|
|
|
- }
|
|
|
- detect_wheel_ceres3d::Detect_result result;
|
|
|
- pcl::PointCloud<pcl::PointXYZ>::Ptr correct_cloud(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
- for(int i=0;i<clouds.size();++i)
|
|
|
- {
|
|
|
- *correct_cloud+=(*clouds[i]);
|
|
|
- }
|
|
|
- code=p->detect(correct_cloud,p->m_configure.box(),result);
|
|
|
-
|
|
|
- if(tick.tic()>1./fps)
|
|
|
- {
|
|
|
- LOG(WARNING)<<" detect fps < capture fps";
|
|
|
- }
|
|
|
- if(code!=SUCCESS)
|
|
|
- {
|
|
|
- LOG_EVERY_N(INFO,20)<<code.get_error_description();
|
|
|
- continue;
|
|
|
- }
|
|
|
-
|
|
|
- // to be 发布结果
|
|
|
- msg.set_ground_statu(message::Car_correct);
|
|
|
- message::Locate_information locate_information;
|
|
|
- locate_information.set_locate_x(result.cx);
|
|
|
- locate_information.set_locate_y(result.cy);
|
|
|
- locate_information.set_locate_angle(result.theta);
|
|
|
- locate_information.set_locate_wheel_base(result.wheel_base);
|
|
|
- locate_information.set_locate_length(result.wheel_base);
|
|
|
- locate_information.set_locate_wheel_width(result.width);
|
|
|
- locate_information.set_locate_width(result.body_width);
|
|
|
- locate_information.set_locate_front_theta(result.front_theta);
|
|
|
- locate_information.set_locate_correct(true);
|
|
|
-
|
|
|
- Communication_message c_msg;
|
|
|
- c_msg.reset(msg.base_info(),msg.SerializeAsString());
|
|
|
- Publisher::get_instance_pointer()->publish_msg(&c_msg);
|
|
|
-
|
|
|
-
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
-
|
|
|
-
|
