// // Created by zx on 2021/5/20. // #include "ground_region.h" #include #include #include #include #include #include "../tool/point_tool.h" // 测量结果滤波,不影响现有结构 #include "../tool/measure_filter.h" // 增加车辆停止状态判断 #include "../tool/region_status_checker.h" #include "../system/system_communication_mq.h" //欧式聚类******************************************************* std::vector::Ptr> Ground_region::segmentation(pcl::PointCloud::Ptr sor_cloud) { std::vector ece_inlier; pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); pcl::EuclideanClusterExtraction ece; ece.setInputCloud(sor_cloud); ece.setClusterTolerance(0.07); ece.setMinClusterSize(20); ece.setMaxClusterSize(20000); ece.setSearchMethod(tree); ece.extract(ece_inlier); std::vector::Ptr> segmentation_clouds; for (int i = 0; i < ece_inlier.size(); i++) { pcl::PointCloud::Ptr cloud_copy(new pcl::PointCloud); std::vector ece_inlier_ext = ece_inlier[i].indices; 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)); // } /** * @description: point rectangle detect * @param points detect if points obey the rectangle rule * @return wether forms a rectangle */ bool Ground_region::isRect(std::vector &points) { if (points.size() == 4) { double L[3] = {0.0}; L[0] = distance(points[0], points[1]); L[1] = distance(points[1], points[2]); L[2] = distance(points[0], points[2]); double max_l = L[0]; double l1 = L[1]; double l2 = L[2]; 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) { max_l = L[i]; l1 = L[abs(i + 1) % 3]; l2 = L[abs(i + 2) % 3]; ps = points[i % 3]; pt = points[(i + 1) % 3]; pc = points[(i + 2) % 3]; } } //直角边与坐标轴的夹角 <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))) { //std::cout<<" 4 wheel axis angle : "<= 0.15) { /*char description[255]={0}; sprintf(description,"angle cos value(%.2f) >0.13 ",cosa); std::cout< 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); std::cout< max_l * 0.1 || distance(center1, center2) > 0.150) { /*std::cout << "d:" << d << " maxl:" << max_l << " center1:" << center1 << " center2:" << center2< max_l * 0.1 || distance(center1, center2) > 0.150 "); std::cout< max_l) { max_index = i; max_l = L[i]; l1 = L[abs(i + 1) % 3]; l2 = L[abs(i + 2) % 3]; ps = points[i % 3]; pt = points[(i + 1) % 3]; pc = points[(i + 2) % 3]; } } //直角边与坐标轴的夹角 <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))) { //std::cout<<" 4 wheel axis angle : "<= 0.15) { /*char description[255]={0}; sprintf(description,"3 wheels angle cos value(%.2f) >0.13 ",cosa); std::cout< 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; points.push_back(point4); /*char description[255]={0}; sprintf(description,"3 wheels rectangle cos angle=%.2f,L=%.1f, w=%.1f",cosa,l,w); std::cout<::Ptr cloud, double thresh_z, detect_wheel_ceres3d::Detect_result &result) { if (m_detector == nullptr) return false; pcl::PointCloud::Ptr cloud_filtered(new pcl::PointCloud); for (int i = 0; i < cloud->size(); ++i) { pcl::PointXYZ pt = cloud->points[i]; if (pt.z < thresh_z) { cloud_filtered->push_back(pt); } } //下采样 pcl::VoxelGrid vox; //创建滤波对象 vox.setInputCloud(cloud_filtered); //设置需要过滤的点云给滤波对象 vox.setLeafSize(0.01f, 0.01f, 0.01f); //设置滤波时创建的体素体积为1cm的立方体 vox.filter(*cloud_filtered); //执行滤波处理,存储输出 if (cloud_filtered->size() == 0) { return false; } pcl::StatisticalOutlierRemoval 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) { return false; } std::vector::Ptr> seg_clouds; seg_clouds = segmentation(cloud_filtered); if (!(seg_clouds.size() == 4 || seg_clouds.size() == 3)) { return false; } std::vector centers; cv::Point2f temp_centers(0,0); 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])); temp_centers += cv::Point2f(centroid[0], centroid[1]); } temp_centers /= 4.0f; bool ret = isRect(centers); if (ret) { std::string error_str; // LOG(WARNING) << "region id: "<< m_region.region_id(); if (m_detector->detect(seg_clouds, result, error_str)) { return true; } else { // LOG(WARNING) << error_str; return false; } } return ret; } // constructor Ground_region::Ground_region() { m_region_status = E_UNKNOWN; m_detector = nullptr; m_measure_thread = nullptr; } // deconstructor Ground_region::~Ground_region() { // LOG(WARNING) << "start deconstruct ground region"; if (m_measure_thread) { m_measure_condition.kill_all(); // Close Capturte Thread if (m_measure_thread->joinable()) { m_measure_thread->join(); delete m_measure_thread; m_measure_thread = nullptr; } } // LOG(WARNING) << "thread released"; // 将创建的检测器析构 if(m_detector) { delete m_detector; m_detector = nullptr; } Region_status_checker::get_instance_references().Region_status_checker_uninit(); // LOG(WARNING) << "detector released"; } Error_manager Ground_region::init(velodyne::Region region, pcl::PointCloud::Ptr left_model, pcl::PointCloud::Ptr right_model) { m_range_status_last = 0; Region_status_checker::get_instance_references().Region_status_checker_init(); m_region = region; m_detector = new detect_wheel_ceres3d(left_model,right_model); mp_cloud_collection = pcl::PointCloud::Ptr(new pcl::PointCloud); mp_cloud_filtered = pcl::PointCloud::Ptr(new pcl::PointCloud); mp_cloud_detect_z = pcl::PointCloud::Ptr(new pcl::PointCloud); m_measure_thread = new std::thread(&Ground_region::thread_measure_func, this); m_measure_condition.reset(); m_region_status = E_READY; return SUCCESS; } // 计算均方误差 bool computer_var(std::vector data, double &var) { if (data.size() == 0) return false; Eigen::VectorXd dis_vec(data.size()); for (int i = 0; i < data.size(); ++i) { dis_vec[i] = data[i]; } 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())); return true; } Error_manager Ground_region::detect(pcl::PointCloud::Ptr cloud, detect_wheel_ceres3d::Detect_result &last_result) { // 1.*********点云合法性检验********* if (cloud->size() == 0){ // 更新过滤点 m_filtered_cloud_mutex.lock(); mp_cloud_filtered->clear(); m_filtered_cloud_mutex.unlock(); return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "no input point"); } // 2.*********点云预处理********* std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); pcl::PointCloud::Ptr cloud_cut(new pcl::PointCloud); pcl::PointCloud::Ptr cloud_clean(new pcl::PointCloud); pcl::PointCloud::Ptr cloud_filtered(new pcl::PointCloud); for (int i = 0; i < cloud->size(); ++i) { 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_cut->push_back(pt); } } if(cloud_cut->empty()) { // 更新过滤点 m_filtered_cloud_mutex.lock(); mp_cloud_filtered->clear(); m_filtered_cloud_mutex.unlock(); return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "cut no point"); } //下采样 pcl::VoxelGrid vox; //创建滤波对象 vox.setInputCloud(cloud_cut); //设置需要过滤的点云给滤波对象 vox.setLeafSize(0.02f, 0.02f, 0.02f); //设置滤波时创建的体素体积为1cm的立方体 vox.filter(*cloud_clean); //执行滤波处理,存储输出 // cloud_filtered = cloud_clean; if (cloud_clean->empty()){ // 更新过滤点 m_filtered_cloud_mutex.lock(); mp_cloud_filtered->clear(); m_filtered_cloud_mutex.unlock(); return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "ApproximateVoxelGrid no point"); } //离群点过滤 pcl::StatisticalOutlierRemoval sor; sor.setInputCloud(cloud_clean); sor.setMeanK(5); //K近邻搜索点个数 sor.setStddevMulThresh(3.0); //标准差倍数 sor.setNegative(false); //保留未滤波点(内点) sor.filter(*cloud_filtered); //保存滤波结果到cloud_filter // 20221211 changed by yct, 10 point threshold if (cloud_filtered->size() <= 10){ // 更新过滤点 m_filtered_cloud_mutex.lock(); mp_cloud_filtered->clear(); m_filtered_cloud_mutex.unlock(); return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "StatisticalOutlierRemoval no point"); } // 更新过滤点 m_filtered_cloud_mutex.lock(); mp_cloud_filtered->clear(); mp_cloud_filtered->operator+=(*cloud_filtered); m_filtered_cloud_mutex.unlock(); // 3.*********位姿优化,获取中心xy与角度********* pcl::PointCloud::Ptr cloud_detect_z(new pcl::PointCloud); double car_pose_x, car_pose_y, car_pose_theta, car_pose_width, z_value=0.2; // if(!Car_pose_detector::get_instance_references().detect_pose(cloud_filtered, cloud_detect_z, car_pose_x, car_pose_y, car_pose_theta, car_pose_width, z_value, false)) if(!Car_pose_detector::get_instance_references().detect_pose_mat(cloud_filtered, cloud_detect_z, car_pose_x, car_pose_y, car_pose_theta, false)) { return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, "find general car pose value failed."); } // 20230112 added by yct for rough angle border detection if(fabs(car_pose_theta * 180.0 / M_PI) > 8) { last_result.cx = car_pose_x; last_result.cy = car_pose_y; last_result.theta = - car_pose_theta * 180.0 / M_PI; return Error_manager(VELODYNE_REGION_ANGLE_PRECHECK_FAILED, NEGLIGIBLE_ERROR, "find general car pose value failed."); } // LOG_IF(INFO, m_region.region_id() == 0) << "car pose :::: cx: " << car_pose_x+m_region.plc_offsetx() << ", cy: " << car_pose_y+m_region.plc_offsety() << ", theta: " << car_pose_theta*180.0/M_PI << std::endl; std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now(); std::chrono::duration time_used_bowl = std::chrono::duration_cast>(t2 - t1); // 4.*********xoz优化获取底盘高度********* // 重新取包含地面点的点云,用于底盘优化 double z_solver_x = car_pose_x; double z_solver_y = car_pose_y; double z_solver_theta = car_pose_theta; double z_solver_width = 1.0; chassis_ceres_solver t_solver; // !!!重新筛选地面点并滤波,融入第一步位姿使用的滤波后点云,传入第二步获取底盘高度 pcl::PointCloud::Ptr cloud_z_solver(new pcl::PointCloud); for (int i = 0; i < cloud->size(); ++i) { 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()) { cloud_z_solver->push_back(pt); } } if (cloud_z_solver->size() == 0){ return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "cloud z no point for outlier removal"); } //离群点过滤 sor.setInputCloud(cloud_z_solver); sor.setMeanK(5); //K近邻搜索点个数 sor.setStddevMulThresh(3.0); //标准差倍数 sor.setNegative(false); //保留未滤波点(内点) sor.filter(*cloud_z_solver); //保存滤波结果到cloud_filter // 补上车身与车轮点 cloud_z_solver->operator+=(*mp_cloud_filtered); // 去中心,角度调正 Car_pose_detector::get_instance_references().inv_trans_cloud(cloud_z_solver, z_solver_x, z_solver_y, z_solver_theta); if (cloud_z_solver->size() == 0) return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "z solver no point"); // 给予底盘z中心与高度初值 double mid_z = 0.05, height = 0.08; z_solver_x = 0.0; // Error_manager ec = t_solver.solve(cloud_z_solver, z_solver_x, mid_z, z_solver_width, height); Error_manager ec = t_solver.solve_mat(cloud_z_solver, z_solver_x, mid_z, z_solver_width, height, false); // 切除大于height高度以外点,并显示width直线 // 根据z值切原始点云 // std::cout<<"before clip: "<size()< pass; pass.setInputCloud(cloud_z_solver); pass.setFilterFieldName("z"); pass.setFilterLimits(m_region.minz(), mid_z + height / 2.0); pass.setFilterLimitsNegative(false); pass.filter(*cloud_z_solver); // std::cout<<"after clip: "<size()<push_back(pcl::PointXYZ(-width/2.0, i, 0)); // cloud_z_solver->push_back(pcl::PointXYZ(width/2.0, i, 0)); // } // std::cout << "\n------------------------------------ chassis z1: " << mid_z + height / 2.0 << ", mid z: "<< mid_z << std::endl; if(ec != SUCCESS) return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NORMAL, "failed to find chassis z value."); std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now(); std::chrono::duration time_used_block = std::chrono::duration_cast>(t3 - t2); // 更新z中间点 m_detect_z_cloud_mutex.lock(); mp_cloud_detect_z->clear(); mp_cloud_detect_z->operator+=(*cloud_z_solver); m_detect_z_cloud_mutex.unlock(); // 二分法存在错误弃用!!!直接使用底盘z值 std::vector results; detect_wheel_ceres3d::Detect_result result; double chassis_z = mid_z + height / 2.0; // + 0.02; if(chassis_z > m_region.maxz() || chassis_z < m_region.minz()) { return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, (std::string("optimized chassis z value out of range: ")+std::to_string(chassis_z)).c_str()); } bool ret = false; while (chassis_z > (mid_z - height / 2.0)) { // LOG_IF(WARNING, m_region.region_id() == 0) << "chassis z: " << chassis_z << ", midz: " << mid_z - height / 2.0; // 初值中x使用第一步优化的值 result.cx = car_pose_x; // 传入整车角度,用于准确判断轮宽,此处需要的是点云的角度,通常逆时针90度到x轴 result.theta = -(M_PI_2 - car_pose_theta); ret = classify_ceres_detect(cloud_filtered, chassis_z, result); // changed by yct, 根据底盘z识别,失败则向下移动直至成功或超过底盘内空中点 if(ret) { results.push_back(result); break; }else{ chassis_z -= 0.01; } } // } while (fabs(center_z - last_center_z) > 0.01); std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now(); std::chrono::duration time_used_div = std::chrono::duration_cast>(t4 - t3); // std::cout << "\n------------------------------------------------------------ " << std::to_string(time_used_bowl.count()) << " " << std::to_string(time_used_block.count()) << " " << std::to_string(time_used_div.count())<<" || "<< std::to_string(time_used_bowl.count()+time_used_block.count()+time_used_div.count())<< std::endl; if (results.size() == 0) { // std::cout << "\n-------- no result: " << std::endl; //LOG_IF(INFO, m_region.region_id() == 4) << "failed with midz, currz: " << mid_z << ", " << chassis_z; return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, "3d wheel detect failed."); } else { // // 20201010, may lead to problem in chutian, uncomment in debug only // // changed by yct, save 3d wheel detect result. // static int save_debug = 0; // if (m_region.region_id() == 0 && save_debug++ == 5) // m_detector->save_debug_data("/home/zx/yct/chutian_measure_2021/build"); // LOG_IF(INFO, m_region.region_id() == 4) << "detected with suitable z value: " << chassis_z; } /// to be float min_mean_loss = 1.0; for (int i = 0; i < results.size(); ++i) { detect_wheel_ceres3d::Detect_result result = results[i]; std::vector 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) { 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, // min_mean_loss); // std::cout << "\n-------- final z: " << chassis_z << std::endl; // std::cout << "cx: " << last_result.cx << ", cy: " << last_result.cy << ", theta: " << last_result.theta // << ", front: " << last_result.front_theta << ", wheelbase: " << last_result.wheel_base << ", width: " << last_result.width << std::endl << std::endl; // last_result.cx -= x; // last_result.cy -= y; // last_result.theta -= theta; // 角度校验,理论上car pose检测使用车身点云,应与车轮点云识别得到的车身 x偏移(0.035)与角度(<1°)一致 double car_pose_theta_deg = 90 - car_pose_theta * 180.0 / M_PI; if (fabs(car_pose_x - last_result.cx) > 0.035 || fabs(car_pose_theta_deg - last_result.theta) > 1) { char valid_info[255]; sprintf(valid_info, "validation failed for x and theta, carpose: (%.3f, %.3f), result: (%.3f, %.3f)", car_pose_x, car_pose_theta_deg, last_result.cx, last_result.theta); return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, valid_info); } // 车宽精度优化 { pcl::PointCloud::Ptr t_width_rough_cloud = pcl::PointCloud::Ptr(new pcl::PointCloud); pcl::PointCloud::Ptr t_width_extract_cloud = pcl::PointCloud::Ptr(new pcl::PointCloud); t_width_rough_cloud->operator+=(*mp_cloud_filtered); t_width_extract_cloud->operator+=(m_detector->m_left_front_cloud); t_width_extract_cloud->operator+=(m_detector->m_right_front_cloud); t_width_extract_cloud->operator+=(m_detector->m_left_rear_cloud); t_width_extract_cloud->operator+=(m_detector->m_right_rear_cloud); pcl::PointXYZ t_rough_min_p, t_rough_max_p; //add by zzw pcl::PointCloud::Ptr cloud_for_width_upwheel(new pcl::PointCloud); for (int i = 0; i < t_width_rough_cloud->size(); ++i) { pcl::PointXYZ pt = t_width_rough_cloud->points[i]; if (pt.z > chassis_z) { cloud_for_width_upwheel->push_back(pt); } } // rough width for human detect { //离群点过滤 sor.setInputCloud(cloud_for_width_upwheel); sor.setMeanK(5); //K近邻搜索点个数 sor.setStddevMulThresh(1.0); //标准差倍数 sor.setNegative(false); //保留未滤波点(内点) sor.filter(*cloud_for_width_upwheel); //保存滤波结果到cloud_filter Eigen::Affine3d t_affine = Eigen::Affine3d::Identity(); t_affine.prerotate(Eigen::AngleAxisd((90 - last_result.theta) * M_PI / 180.0, Eigen::Vector3d::UnitZ())); pcl::transformPointCloud(*cloud_for_width_upwheel, *cloud_for_width_upwheel, t_affine.matrix()); // 车宽重计算,并赋值到当前车宽 pcl::getMinMax3D(*cloud_for_width_upwheel, t_rough_min_p, t_rough_max_p); } double rough_width = t_rough_max_p.x - t_rough_min_p.x; //离群点过滤 sor.setInputCloud(t_width_extract_cloud); sor.setMeanK(5); //K近邻搜索点个数 sor.setStddevMulThresh(1.0); //标准差倍数 sor.setNegative(false); //保留未滤波点(内点) sor.filter(*t_width_extract_cloud); //保存滤波结果到cloud_filter Eigen::Affine3d t_affine = Eigen::Affine3d::Identity(); t_affine.prerotate(Eigen::AngleAxisd((90 - last_result.theta) * M_PI / 180.0, Eigen::Vector3d::UnitZ())); pcl::transformPointCloud(*t_width_extract_cloud, *t_width_extract_cloud, t_affine.matrix()); // 车宽重计算,并赋值到当前车宽 pcl::PointXYZ t_min_p, t_max_p; pcl::getMinMax3D(*t_width_extract_cloud, t_min_p, t_max_p); double accurate_width = t_max_p.x - t_min_p.x; last_result.width = accurate_width; if(fabs(rough_width - accurate_width) > 0.15) { std::string disp_str = std::string("width difference too large, assume noise situation, ")+std::to_string(rough_width)+", "+std::to_string(accurate_width); LOG_EVERY_N(WARNING,100) << disp_str; return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, MINOR_ERROR, disp_str.c_str()); } // !!!暂时不限制宽度数据 // char valid_info[255]; // sprintf(valid_info, "validation for car width, origin/accurate: (%.3f, %.3f)", last_result.width, accurate_width); // // 允许一边5cm误差,且保证车宽应比车轮识别计算出的大,否则为错误宽度数据 // if (accurate_width - last_result.width > 0.1 || accurate_width < last_result.width) // { // return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, valid_info); // } // if (m_region.region_id() == 0 || m_region.region_id() == 4) // { // LOG(WARNING) << valid_info; // // 保存车宽数据,统计用 // if (m_file_out.is_open()) // m_file_out << std::setprecision(5) << std::to_string(1.883) << "," << last_result.width << "," << accurate_width << std::endl; // } } // LOG_IF(INFO, m_region.region_id() == 4) << last_result.to_string(); return SUCCESS; } int Ground_region::outOfRangeDetection(pcl::PointCloud::Ptr cloud, double cx, double cy, double plate_cx, double plate_cy, double theta) { // LOG(WARNING) << m_range_status_last; if(cloud->size() <= 0) return Range_status::Range_correct; int res = Range_status::Range_correct; // 计算转盘旋转后点云坐标 double theta_rad = theta * M_PI / 180.0; pcl::PointCloud t_cloud; for (size_t i = 0; i < cloud->size(); i++) { Eigen::Vector2d t_point(cloud->points[i].x - plate_cx, cloud->points[i].y - plate_cy); pcl::PointXYZ t_pcl_point; t_pcl_point.x = (t_point.x() * cos(theta_rad) - t_point.y() * sin(theta_rad)) + plate_cx; t_pcl_point.y = (t_point.x() * sin(theta_rad) + t_point.y() * cos(theta_rad)) + plate_cy; t_pcl_point.z = cloud->points[i].z; t_cloud.push_back(t_pcl_point); } pcl::PointXYZ min_p, max_p; pcl::getMinMax3D(t_cloud, min_p, max_p); #ifndef ANTI_SHAKE // 判断左右超界情况 if(min_p.x < m_region.border_minx()) res |= Range_status::Range_left; if(max_p.x > m_region.border_maxx()) res |= Range_status::Range_right; // LOG_IF(WARNING, m_region.region_id() == 4)<< "border minmax x: "<< min_p.x<<", "< m_region.border_minx()+0.010) { int t = Range_status::Range_left; m_range_status_last &= (~t); } //else m_range_status_last no change if(max_p.x > m_region.border_maxx()+0.000) { m_range_status_last |= Range_status::Range_right; } else if(max_p.x < m_region.border_maxx()-0.010) { int t = Range_status::Range_right; m_range_status_last &= (~t); } //else m_range_status_last no change */ //huli 不再使用汽车左右边界来判断,直接使用中心点x值判断左右超界。 //plc坐标,1 3 5入口x为-1.8左右, 2 4 6入口为1.8左右 //要求x取值范围,1650~2150左右 float t_center_x = cx + m_region.plc_offsetx(); if(t_center_x < m_region.plc_out_of_region().border_minx()-0.000) { m_range_status_last |= Range_status::Range_left; } else if(t_center_x > m_region.plc_out_of_region().border_minx()+0.010) { int t = Range_status::Range_left; m_range_status_last &= (~t); } //else m_range_status_last no change if(t_center_x > m_region.plc_out_of_region().border_maxx()+0.000) { m_range_status_last |= Range_status::Range_right; } else if(t_center_x < m_region.plc_out_of_region().border_maxx()-0.010) { int t = Range_status::Range_right; m_range_status_last &= (~t); } //else m_range_status_last no change // LOG(WARNING) << "-------------------------------------------------------" <::Ptr cloud, Common_data::Car_wheel_information measure_result, double plate_cx, double plate_cy, double theta) { // int res = Range_status::Range_correct; if(cloud->size() <= 0) return Range_status::Range_correct; // 计算转盘旋转后车辆中心点坐标 Eigen::Vector2d car_center(measure_result.car_center_x, measure_result.car_center_y); Eigen::Vector2d car_uniform_center; double theta_rad = theta * M_PI / 180.0; car_uniform_center.x() = car_center.x() * cos(theta_rad) - car_center.y() * sin(theta_rad); car_uniform_center.y() = car_center.x() * sin(theta_rad) + car_center.y() * cos(theta_rad); #ifndef ANTI_SHAKE // 车位位于y负方向,判断后轮超界情况 //yct old program 20221101 // double rear_wheel_y = car_uniform_center.y() + m_region.plc_offsety() - 0.5 * measure_result.car_wheel_base; // if(rear_wheel_y < m_region.plc_border_miny()) // { // res |= Range_status::Range_back; // } //huli new program 20221101 double t_center_y = 0.0 - (car_uniform_center.y() + m_region.plc_offsety()); // 汽车中心y, 参照plc坐标,并且进行偏移, 绝对值。结果大约为 5~6左右 if( t_center_y - 4.88 + 0.5*(measure_result.car_wheel_base - 2.7) < m_region.plc_border_maxy() && t_center_y - 4.88 - 0.5*(measure_result.car_wheel_base - 2.7) < m_region.plc_border_miny() ) { //normal } else { res |= Range_status::Range_back; } // 判断车辆宽度超界情况 if (measure_result.car_wheel_width < m_region.car_min_width() || measure_result.car_wheel_width > m_region.car_max_width()) { res |= Range_status::Range_car_width; } // 判断车辆轴距超界情况 if (measure_result.car_wheel_base < m_region.car_min_wheelbase() || measure_result.car_wheel_base > m_region.car_max_wheelbase()) { res |= Range_status::Range_car_wheelbase; } //yct old program 20221103 , 使用 velodyne_manager.prototxt 里面的 turnplate_angle_limit 来做边界判断 // 判断车辆旋转角超界情况 // double dtheta = 90-measure_result.car_angle; // if (dtheta > m_region.turnplate_angle_limit_anti_clockwise()) // { // res |= Range_status::Range_angle_clock; // } // if (dtheta < -m_region.turnplate_angle_limit_clockwise()) // { // res |= Range_status::Range_angle_anti_clock; // } //huli new program 20221103, 使用 plc dispatch_1_statu_port 里面的 来做边界判断 // double t_dtheta = 90-measure_result.car_angle; // 车身的旋转角, 修正到正负5度, double t_dtheta = measure_result.car_angle + m_region.plc_offset_degree(); // 车身的旋转角, 修正到正5度, (m_region.plc_offset_degree():-89) int t_terminal_id = m_region.region_id() +1; //region_id:0~5, terminal_id:1~6 //turnplate_angle_min = -5, turnplate_angle_max = 5, if (t_dtheta > System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_max()) { res |= Range_status::Range_angle_anti_clock; } if (t_dtheta < System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_min()) { res |= Range_status::Range_angle_clock; } // // 判断车辆前轮角回正情况 // if (fabs(measure_result.car_front_theta) > 8.0) // { // res |= Range_status::Range_steering_wheel_nozero; // } res |= outOfRangeDetection(cloud, plate_cx, plate_cy, theta); return res; #else // LOG(WARNING) << m_range_status_last; // 车位位于y负方向,判断后轮超界情况 double t_center_y = 0.0 - (car_uniform_center.y() + m_region.plc_offsety()); // 汽车中心y, 参照plc坐标,并且进行偏移, 绝对值。结果大约为 5~6左右 if( t_center_y - 4.88 + 0.5*(measure_result.car_wheel_base - 2.7) < m_region.plc_out_of_region().plc_border_maxy()-0.000 && t_center_y - 4.88 - 0.5*(measure_result.car_wheel_base - 2.7) < m_region.plc_out_of_region().plc_border_miny()-0.000 ) { int t = Range_status::Range_back; m_range_status_last &= (~t); } else if( t_center_y - 4.88 + 0.5*(measure_result.car_wheel_base - 2.7) > m_region.plc_out_of_region().plc_border_maxy()+0.010 || t_center_y - 4.88 - 0.5*(measure_result.car_wheel_base - 2.7) > m_region.plc_out_of_region().plc_border_miny()+0.010 ) { m_range_status_last |= Range_status::Range_back; } // 判断车辆宽度超界情况 if (measure_result.car_wheel_width < m_region.plc_out_of_region().car_min_width()-0.000 || measure_result.car_wheel_width > m_region.plc_out_of_region().car_max_width()+0.000) { m_range_status_last |= Range_status::Range_car_width; } else if (measure_result.car_wheel_width > m_region.plc_out_of_region().car_min_width()+0.010 && measure_result.car_wheel_width < m_region.plc_out_of_region().car_max_width()-0.010) { int t = Range_status::Range_car_width; m_range_status_last &= (~t); } // 判断车辆轴距超界情况 if (measure_result.car_wheel_base < m_region.plc_out_of_region().car_min_wheelbase()-0.000 || measure_result.car_wheel_base > m_region.plc_out_of_region().car_max_wheelbase()+0.000) { m_range_status_last |= Range_status::Range_car_wheelbase; } else if (measure_result.car_wheel_base > m_region.plc_out_of_region().car_min_wheelbase()+0.010 || measure_result.car_wheel_base < m_region.plc_out_of_region().car_max_wheelbase()-0.010) { int t = Range_status::Range_car_wheelbase; m_range_status_last &= (~t); } //else m_range_status_last no change // LOG_IF(INFO, m_region.region_id() == 4) << m_range_status_last; // 判断车辆旋转角超界情况 //huli new program 20221103, 使用 plc dispatch_1_statu_port 里面的 来做边界判断 // double t_dtheta = 90-measure_result.car_angle; // 车身的旋转角, 修正到正负5度, double t_dtheta = measure_result.car_angle + m_region.plc_offset_degree(); // 车身的旋转角, 修正到正5度, (m_region.plc_offset_degree():-89) int t_terminal_id = m_region.region_id() +1; //region_id:0~5, terminal_id:1~6 //turnplate_angle_min = -5, turnplate_angle_max = 5, if (t_dtheta > System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_max()+0.00) { m_range_status_last |= Range_status::Range_angle_anti_clock; } else if (t_dtheta < System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_max()-0.10) { int t = Range_status::Range_angle_anti_clock; m_range_status_last &= (~t); } // LOG_IF(INFO, m_region.region_id() == 4) << m_range_status_last; //else m_range_status_last no change if (t_dtheta < System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_min()-0.00) { m_range_status_last |= Range_status::Range_angle_clock; } else if (t_dtheta > System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_min()+0.10) { int t = Range_status::Range_angle_clock; m_range_status_last &= (~t); } // LOG_IF(INFO, m_region.region_id() == 4) << m_range_status_last <<", t_dtheta "< outOfRegion.plc_border_maxy()+0.010 || t_center_y - 4.88 - 0.5*(measure_result.car_wheel_base - 2.7) > outOfRegion.plc_border_miny()+0.010 ) { t_range_status_last |= Range_status::Range_back; } // 判断车辆宽度超界情况 if (measure_result.car_wheel_width < outOfRegion.car_min_width()-0.000 || measure_result.car_wheel_width > outOfRegion.car_max_width()+0.000) { t_range_status_last |= Range_status::Range_car_width; } else if (measure_result.car_wheel_width > outOfRegion.car_min_width()+0.010 && measure_result.car_wheel_width < outOfRegion.car_max_width()-0.010) { int t = Range_status::Range_car_width; t_range_status_last &= (~t); } // 判断车辆轴距超界情况 if (measure_result.car_wheel_base < outOfRegion.car_min_wheelbase()-0.000 || measure_result.car_wheel_base > outOfRegion.car_max_wheelbase()+0.000) { t_range_status_last |= Range_status::Range_car_wheelbase; } else if (measure_result.car_wheel_base > outOfRegion.car_min_wheelbase()+0.010 || measure_result.car_wheel_base < outOfRegion.car_max_wheelbase()-0.010) { int t = Range_status::Range_car_wheelbase; t_range_status_last &= (~t); } // 判断车辆旋转角超界情况 double t_dtheta = measure_result.car_angle + m_region.plc_offset_degree(); // 车身的旋转角, 修正到正5度, (m_region.plc_offset_degree():-89) int t_terminal_id = m_region.region_id() +1; //region_id:0~5, terminal_id:1~6 if (t_dtheta > System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_max()+0.00) { t_range_status_last |= Range_status::Range_angle_anti_clock; } else if (t_dtheta < System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_max()-0.10) { int t = Range_status::Range_angle_anti_clock; t_range_status_last &= (~t); } if (t_dtheta < System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_min()-0.00) { t_range_status_last |= Range_status::Range_angle_clock; } else if (t_dtheta > System_communication_mq::get_instance_references().dispatch_region_info_map[t_terminal_id].turnplate_angle_min()+0.10) { int t = Range_status::Range_angle_clock; t_range_status_last &= (~t); } //huli 不再使用汽车左右边界来判断,直接使用中心点x值判断左右超界。 //plc坐标,1 3 5入口x为-1.8左右, 2 4 6入口为1.8左右 //要求x取值范围,1650~2150左右 if(car_uniform_center.x() < outOfRegion.border_minx()-0.000) { t_range_status_last |= Range_status::Range_left; } else if(car_uniform_center.x() > outOfRegion.border_minx()+0.010) { int t = Range_status::Range_left; t_range_status_last &= (~t); } //else m_range_status_last no change if(car_uniform_center.x() > outOfRegion.border_maxx()+0.000) { t_range_status_last |= Range_status::Range_right; } else if(car_uniform_center.x() < outOfRegion.border_maxx()-0.010) { int t = Range_status::Range_right; t_range_status_last &= (~t); } return t_range_status_last; } //外部调用获取当前车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会报错, 不会等待 Error_manager Ground_region::get_current_wheel_information(Common_data::Car_wheel_information *p_car_wheel_information, std::chrono::system_clock::time_point command_time) { if ( p_car_wheel_information == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, " POINTER IS NULL "); } //获取指令时间之后的信息, 如果没有就会报错, 不会等待 if( m_detect_update_time > command_time ) { std::lock_guard lck(m_car_result_mutex); *p_car_wheel_information = m_car_wheel_information; if(m_car_wheel_information.correctness) return Error_code::SUCCESS; else return Error_manager(Error_code::VELODYNE_REGION_CERES_SOLVE_ERROR, Error_level::MINOR_ERROR, " Ground_region detect error"); } else { return Error_manager(Error_code::VELODYNE_REGION_EMPTY_NO_WHEEL_INFORMATION, Error_level::MINOR_ERROR, " Ground_region::get_current_wheel_information error "); } } //外部调用获取最新的车轮定位信息, 获取指令时间往前一个周期内的车轮定位信息, 如果没有就会报错, 不会等待 Error_manager Ground_region::get_last_wheel_information(Common_data::Car_wheel_information *p_car_wheel_information, std::chrono::system_clock::time_point command_time) { if ( p_car_wheel_information == NULL ) { return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR, " POINTER IS NULL "); } //获取指令时间之后的信息, 如果没有就会报错, 不会等待 // LOG(WARNING) << std::chrono::duration_cast(command_time-m_detect_update_time).count()/1000.0 <<", " // <(command_time-m_cloud_collection_time).count()/1000.0; if( m_detect_update_time > command_time - std::chrono::milliseconds(GROUND_REGION_DETECT_CYCLE_MS)) { std::lock_guard lck(m_car_result_mutex); *p_car_wheel_information = m_car_wheel_information; if(m_car_wheel_information.correctness) return Error_code::SUCCESS; else return Error_manager(Error_code::VELODYNE_REGION_CERES_SOLVE_ERROR, Error_level::MINOR_ERROR, " Ground_region detect error"); } else { return Error_manager(Error_code::VELODYNE_REGION_EMPTY_NO_WHEEL_INFORMATION, Error_level::MINOR_ERROR, " Ground_region::get_current_wheel_information error "); } } Error_manager Ground_region::update_cloud(pcl::PointCloud::Ptr cloud) { // // 点云z转90度,调试用 //Eigen::AngleAxisd rot_z = Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ()); //for (size_t i = 0; i < cloud->size(); i++) //{ // Eigen::Vector3d t_point(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z); // t_point = rot_z.toRotationMatrix() * t_point; // cloud->points[i].x = t_point.x(); // cloud->points[i].y = t_point.y(); // cloud->points[i].z = t_point.z(); //} // 限定锁区域,解决因条件变量在析构时无法获得内部mutex导致此互斥锁卡住,造成死锁无法结束程序的问题。 { std::lock_guard lck(m_cloud_collection_mutex); mp_cloud_collection = cloud; // LOG(WARNING) << "update region cloud size: " << mp_cloud_collection->size() << ",,, input size: " << cloud->size(); m_cloud_collection_time = std::chrono::system_clock::now(); } m_measure_condition.notify_one(false, true); // std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); // std::chrono::duration time_used_update = std::chrono::duration_cast>(t1 - t0); // std::cout << "update cloud time: " << time_used_update.count() << std::endl; return SUCCESS; } void Ground_region::thread_measure_func() { LOG(INFO) << " Ground_region::thread_measure_func() start " << this; while (m_measure_condition.is_alive()) { m_measure_condition.wait(); if (m_measure_condition.is_alive()) { m_region_status = E_BUSY; detect_wheel_ceres3d::Detect_result t_result; Error_manager ec = detect(mp_cloud_collection, t_result); std::lock_guard lck(m_car_result_mutex); m_detect_update_time = std::chrono::system_clock::now(); // 增加滤波轴距 Common_data::Car_wheel_information_stamped t_wheel_info_stamped; // 车辆移动检测 Common_data::Car_wheel_information_stamped t_wheel_info_stamped_for_car_move; if (ec == SUCCESS) { m_car_wheel_information.correctness = true; m_car_wheel_information.car_center_x = t_result.cx; m_car_wheel_information.car_center_y = t_result.cy; m_car_wheel_information.car_angle = t_result.theta; m_car_wheel_information.car_wheel_base = t_result.wheel_base; m_car_wheel_information.car_wheel_width = t_result.width; m_car_wheel_information.car_front_theta = t_result.front_theta; // 220110 added by yct, single wheel width filtering m_car_wheel_information.single_wheel_width = t_result.single_wheel_width; m_car_wheel_information.single_wheel_length = t_result.single_wheel_length; m_car_wheel_information.theta_uniform(m_region.turnplate_cx(), m_region.turnplate_cy()); // 超界校验 { std::lock_guard lck(m_filtered_cloud_mutex); m_car_wheel_information.range_status = outOfRangeDetection(mp_cloud_filtered, m_car_wheel_information, m_region.turnplate_cx(), m_region.turnplate_cy(), 90.0 - t_result.theta); // 新加对终端小框的边界判断 m_car_wheel_information.terminal_range_status = outOfTerminalRangeDetection(m_car_wheel_information, m_region.terminal_out_of_region()); } // 添加plc偏移 m_car_wheel_information.car_center_x += m_region.plc_offsetx(); m_car_wheel_information.car_center_y += m_region.plc_offsety(); m_car_wheel_information.uniform_car_x += m_region.plc_offsetx(); m_car_wheel_information.uniform_car_y += m_region.plc_offsety(); m_car_wheel_information.car_angle += m_region.plc_offset_degree(); t_wheel_info_stamped.wheel_data = m_car_wheel_information; t_wheel_info_stamped.measure_time = m_detect_update_time; Measure_filter::get_instance_references().update_data(m_region.region_id(), t_wheel_info_stamped); // 20211228 added by yct, car movement checking, human and door detection t_wheel_info_stamped_for_car_move = t_wheel_info_stamped; t_wheel_info_stamped_for_car_move.wheel_data.car_center_x -= m_region.plc_offsetx(); t_wheel_info_stamped_for_car_move.wheel_data.car_center_y -= m_region.plc_offsety(); t_wheel_info_stamped_for_car_move.wheel_data.uniform_car_x -= m_region.plc_offsetx(); t_wheel_info_stamped_for_car_move.wheel_data.uniform_car_y -= m_region.plc_offsety(); t_wheel_info_stamped_for_car_move.wheel_data.car_angle -= m_region.plc_offset_degree(); Error_manager car_status_res = Region_status_checker::get_instance_references().get_region_parking_status(m_region.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_filtered); // success means car stable if(car_status_res == SUCCESS) { m_car_wheel_information.range_status &= ~((int)Range_status::Range_car_moving); }else { m_car_wheel_information.range_status |= Range_status::Range_car_moving; } Region_status_checker::get_instance_references().add_measure_data(m_region.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_filtered); ec = Measure_filter::get_instance_references().get_filtered_wheel_information(m_region.region_id(), t_wheel_info_stamped.wheel_data); if (ec == SUCCESS) { m_car_wheel_information.car_wheel_base = t_wheel_info_stamped.wheel_data.car_wheel_base; m_car_wheel_information.car_front_theta = t_wheel_info_stamped.wheel_data.car_front_theta; // 临时添加,滤波后前轮超界 if(fabs(m_car_wheel_information.car_front_theta)>16.0) { m_car_wheel_information.range_status |= Range_status::Range_steering_wheel_nozero; } } // 超界判断,这个必须最后判断 } // rough angle too large, display for pre-movement else if(ec.get_error_code() == VELODYNE_REGION_ANGLE_PRECHECK_FAILED) { m_car_wheel_information.correctness = false; m_car_wheel_information.car_center_x = t_result.cx; m_car_wheel_information.car_center_y = t_result.cy; m_car_wheel_information.car_angle = t_result.theta; m_car_wheel_information.car_wheel_base = 0.0; m_car_wheel_information.car_wheel_width = 0.0; m_car_wheel_information.car_front_theta = 0.0; // 220110 added by yct, single wheel width filtering m_car_wheel_information.single_wheel_width = 0.0; m_car_wheel_information.single_wheel_length = 0.0; // 添加plc偏移 m_car_wheel_information.car_center_x += m_region.plc_offsetx(); m_car_wheel_information.car_center_y += m_region.plc_offsety(); m_car_wheel_information.uniform_car_x += m_region.plc_offsetx(); m_car_wheel_information.uniform_car_y += m_region.plc_offsety(); m_car_wheel_information.theta_uniform(m_region.turnplate_cx(), m_region.turnplate_cy()); m_car_wheel_information.range_status |= m_car_wheel_information.car_angle<0?Range_status::Range_angle_clock:Range_status::Range_angle_anti_clock; } else { m_car_wheel_information.correctness = false; // LOG_IF(ERROR, m_region.region_id() == 1) << ec.to_string(); // 20211228 added by yct, car movement checking, human and door detection Error_manager car_status_res = Region_status_checker::get_instance_references().get_region_parking_status(m_region.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_filtered); // success means car stable if(car_status_res == SUCCESS) { m_car_wheel_information.range_status &= ~((int)Range_status::Range_car_moving); }else { m_car_wheel_information.range_status |= Range_status::Range_car_moving; // if(m_region.region_id()==1){ // std::cout<<"failed: "<