|
|
@@ -183,10 +183,10 @@ namespace graft_cv {
|
|
|
buff << m_pcdId <<": RadiusOutlierRemoval filtered point cloud "
|
|
|
<< cloud_ror->width * cloud_ror->height
|
|
|
<< " data points. param stem_radius="<<
|
|
|
- stem_radius<<", nb_points="<< nb_points;
|
|
|
+ stem_radius<<", nb_points="<< nb_points<< "(if < 50, return)";
|
|
|
m_pLogger->INFO(buff.str());
|
|
|
}
|
|
|
- if (cloud_ror->width * cloud_ror->height < 1) {
|
|
|
+ if (cloud_ror->width * cloud_ror->height < 50) {
|
|
|
return 1;
|
|
|
}
|
|
|
|
|
|
@@ -198,43 +198,52 @@ namespace graft_cv {
|
|
|
}
|
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
//3 voxel grid down sampling， 降采样
|
|
|
- if (m_pLogger) {
|
|
|
- m_pLogger->INFO(m_pcdId + ": begin down");
|
|
|
- }
|
|
|
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_dowm_sampled_with_leaf(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
- pcl::VoxelGrid<pcl::PointXYZ> outrem;
|
|
|
- outrem.setInputCloud(cloud_ror);
|
|
|
- //outrem.setLeafSize(stem_radius, stem_radius, stem_radius);
|
|
|
- double voxel_size = m_cparam.rs_grab_voxel_size;
|
|
|
- if (m_dtype == 0) { voxel_size = m_cparam.sc_grab_voxel_size; }
|
|
|
- outrem.setLeafSize(voxel_size, voxel_size, voxel_size);
|
|
|
- outrem.filter(*cloud_dowm_sampled_with_leaf);
|
|
|
-
|
|
|
+ //if (m_pLogger) {
|
|
|
+ // m_pLogger->INFO(m_pcdId + ": begin down");
|
|
|
+ //}
|
|
|
+ //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_dowm_sampled_with_leaf(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
+ //pcl::VoxelGrid<pcl::PointXYZ> outrem;
|
|
|
+ //outrem.setInputCloud(cloud_ror);
|
|
|
+ ////outrem.setLeafSize(stem_radius, stem_radius, stem_radius);
|
|
|
+ //double voxel_size = m_cparam.rs_grab_voxel_size;
|
|
|
+ //if (m_dtype == 0) { voxel_size = m_cparam.sc_grab_voxel_size; }
|
|
|
+ //outrem.setLeafSize(voxel_size, voxel_size, voxel_size);
|
|
|
+ //outrem.filter(*cloud_dowm_sampled_with_leaf);
|
|
|
+
|
|
|
+ double mean_dist = 0.0;
|
|
|
+ cloud_mean_dist(cloud_ror, mean_dist);
|
|
|
if (m_pLogger) {
|
|
|
stringstream buff;
|
|
|
- buff << m_pcdId <<": VoxelGrid dowm_sampled point cloud "
|
|
|
- << cloud_dowm_sampled_with_leaf->width * cloud_dowm_sampled_with_leaf->height
|
|
|
- << " data points (if < 50, return)";
|
|
|
+ buff << m_pcdId <<": cloud_mean_dist = " << mean_dist;
|
|
|
m_pLogger->INFO(buff.str());
|
|
|
}
|
|
|
- if (cloud_dowm_sampled_with_leaf->width * cloud_dowm_sampled_with_leaf->height < 50) {
|
|
|
- return 1;
|
|
|
- }
|
|
|
+
|
|
|
|
|
|
- /*if (m_cparam.image_show) {
|
|
|
- viewer_cloud(cloud_dowm_sampled, std::string("cloud_dowm_sampled"));
|
|
|
- }*/
|
|
|
- if (m_pLogger) {
|
|
|
- m_pLogger->INFO(m_pcdId + ": end down");
|
|
|
- }
|
|
|
+ //if (m_pLogger) {
|
|
|
+ // stringstream buff;
|
|
|
+ // buff << m_pcdId <<": VoxelGrid dowm_sampled point cloud "
|
|
|
+ // << cloud_dowm_sampled_with_leaf->width * cloud_dowm_sampled_with_leaf->height
|
|
|
+ // << " data points (if < 50, return)";
|
|
|
+ // m_pLogger->INFO(buff.str());
|
|
|
+ //}
|
|
|
+ //if (cloud_dowm_sampled_with_leaf->width * cloud_dowm_sampled_with_leaf->height < 50) {
|
|
|
+ // return 1;
|
|
|
+ //}
|
|
|
+
|
|
|
+ ///*if (m_cparam.image_show) {
|
|
|
+ // viewer_cloud(cloud_dowm_sampled, std::string("cloud_dowm_sampled"));
|
|
|
+ //}*/
|
|
|
+ //if (m_pLogger) {
|
|
|
+ // m_pLogger->INFO(m_pcdId + ": end down");
|
|
|
+ //}
|
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
// 4 对截取的点云进行聚类分析，剔除叶片
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_dowm_sampled(new pcl::PointCloud<pcl::PointXYZ>);
|
|
|
std::vector<int> non_leaf_indices;
|
|
|
- //leaf_filter(cloud_dowm_sampled_with_leaf, non_leaf_indices);
|
|
|
- //leaf_filter_morph(cloud_dowm_sampled_with_leaf, non_leaf_indices);
|
|
|
- leaf_filter_ror(cloud_dowm_sampled_with_leaf, non_leaf_indices);
|
|
|
- pcl::copyPointCloud(*cloud_dowm_sampled_with_leaf, non_leaf_indices, *cloud_dowm_sampled);
|
|
|
+ //leaf_filter(cloud_ror, non_leaf_indices);
|
|
|
+ //leaf_filter_morph(cloud_ror, non_leaf_indices);
|
|
|
+ leaf_filter_ror(cloud_ror, non_leaf_indices);
|
|
|
+ pcl::copyPointCloud(*cloud_ror, non_leaf_indices, *cloud_dowm_sampled);
|
|
|
if (m_pLogger) {
|
|
|
stringstream buff;
|
|
|
buff << m_pcdId << ": after leaf_filter dowm_sampled point cloud "
|
|
|
@@ -320,8 +329,8 @@ namespace graft_cv {
|
|
|
}
|
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
//7 保存处理结果到图片
|
|
|
- cv::Mat rst_img = cv::Mat::zeros(cv::Size(1280,640),CV_8UC1);
|
|
|
- gen_result_img(cloud_dowm_sampled, selected_pt, rst_img);
|
|
|
+ cv::Mat rst_img = cv::Mat::zeros(cv::Size(1280,640),CV_8UC3);
|
|
|
+ gen_result_img(cloud_ror, cloud_dowm_sampled, selected_pt, rst_img);
|
|
|
if (m_ppImgSaver && *m_ppImgSaver) {
|
|
|
(*m_ppImgSaver)->saveImage(rst_img, m_pcdId + "_rst_0");
|
|
|
}
|
|
|
@@ -373,6 +382,7 @@ namespace graft_cv {
|
|
|
|
|
|
//生成结果图片
|
|
|
void CRootStockGrabPoint::gen_result_img(
|
|
|
+ pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_raw,//输入，未经过滤的整体点云in_cloud_raw,
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud, //输入，整体点云cloud_dowm_sampled,
|
|
|
pcl::PointXYZ& selected_pt, //输入，selected_pt,
|
|
|
cv::Mat& rst_img //输出，图片， 640*1280
|
|
|
@@ -385,32 +395,44 @@ namespace graft_cv {
|
|
|
float res = 0.33333; //分辨率，1个像素0.333mm
|
|
|
//绘制坐标轴
|
|
|
int arrow_len = 20;
|
|
|
- cv::line(rst_img, cv::Point(0, cy), cv::Point(cx, cy), cv::Scalar(128));
|
|
|
- cv::line(rst_img, cv::Point(0, cy), cv::Point(arrow_len, cy - int(arrow_len/2)), cv::Scalar(128));
|
|
|
- cv::line(rst_img, cv::Point(0, cy), cv::Point(arrow_len, cy + int(arrow_len/2)), cv::Scalar(128));
|
|
|
+ cv::line(rst_img, cv::Point(0, cy), cv::Point(cx, cy), cv::Scalar(128,128,128));
|
|
|
+ cv::line(rst_img, cv::Point(0, cy), cv::Point(arrow_len, cy - int(arrow_len/2)), cv::Scalar(128, 128, 128));
|
|
|
+ cv::line(rst_img, cv::Point(0, cy), cv::Point(arrow_len, cy + int(arrow_len/2)), cv::Scalar(128, 128, 128));
|
|
|
|
|
|
- cv::line(rst_img, cv::Point(cx, 0), cv::Point(cx, cy), cv::Scalar(128));
|
|
|
- cv::line(rst_img, cv::Point(cx, 0), cv::Point(cx - int(arrow_len/2), arrow_len), cv::Scalar(128));
|
|
|
- cv::line(rst_img, cv::Point(cx, 0), cv::Point(cx + int(arrow_len/2), arrow_len), cv::Scalar(128));
|
|
|
+ cv::line(rst_img, cv::Point(cx, 0), cv::Point(cx, cy), cv::Scalar(128, 128, 128));
|
|
|
+ cv::line(rst_img, cv::Point(cx, 0), cv::Point(cx - int(arrow_len/2), arrow_len), cv::Scalar(128, 128, 128));
|
|
|
+ cv::line(rst_img, cv::Point(cx, 0), cv::Point(cx + int(arrow_len/2), arrow_len), cv::Scalar(128, 128, 128));
|
|
|
|
|
|
- cv::putText(rst_img, std::string("x"), cv::Point(20, cy-10), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(128));
|
|
|
- cv::putText(rst_img, std::string("y"), cv::Point(cx+10, 20), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(128));
|
|
|
+ cv::putText(rst_img, std::string("x"), cv::Point(20, cy-10), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(128, 128, 128));
|
|
|
+ cv::putText(rst_img, std::string("y"), cv::Point(cx+10, 20), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(128, 128, 128));
|
|
|
|
|
|
//绘制所有点
|
|
|
int x, y;
|
|
|
+ for (auto&pt : in_cloud_raw->points) {
|
|
|
+ x = cx - int(pt.x / res + 0.5);
|
|
|
+ y = cy - int(pt.y / res + 0.5);
|
|
|
+ if (x < 0 || x >= rst_img.cols) { continue; }
|
|
|
+ if (y < 0 || y >= rst_img.rows) { continue; }
|
|
|
+ rst_img.at<cv::Vec3b>(y, x)[0] = 60;
|
|
|
+ rst_img.at<cv::Vec3b>(y, x)[1] = 20;
|
|
|
+ rst_img.at<cv::Vec3b>(y, x)[2] = 220;
|
|
|
+ }
|
|
|
+
|
|
|
for (auto&pt : in_cloud->points) {
|
|
|
x = cx - int(pt.x / res + 0.5);
|
|
|
y = cy - int(pt.y / res + 0.5);
|
|
|
if (x < 0 || x >= rst_img.cols) { continue; }
|
|
|
if (y < 0 || y >= rst_img.rows) { continue; }
|
|
|
- rst_img.at<unsigned char>(y, x) = 255;
|
|
|
+ rst_img.at<cv::Vec3b>(y, x)[0] = 0;
|
|
|
+ rst_img.at<cv::Vec3b>(y, x)[1] = 225;
|
|
|
+ rst_img.at<cv::Vec3b>(y, x)[2] = 0;
|
|
|
}
|
|
|
//绘制抓取点坐标
|
|
|
int grab_x = cx - int(selected_pt.x / res + 0.5);
|
|
|
int grab_y = cy - int(selected_pt.y / res + 0.5);
|
|
|
int radius = 30;
|
|
|
- cv::line(rst_img, cv::Point(grab_x - radius, grab_y - radius), cv::Point(grab_x + radius, grab_y + radius), cv::Scalar(128));
|
|
|
- cv::line(rst_img, cv::Point(grab_x - radius, grab_y + radius), cv::Point(grab_x + radius, grab_y - radius), cv::Scalar(128));
|
|
|
+ cv::line(rst_img, cv::Point(grab_x - radius, grab_y - radius), cv::Point(grab_x + radius, grab_y + radius), cv::Scalar(0, 215, 255));
|
|
|
+ cv::line(rst_img, cv::Point(grab_x - radius, grab_y + radius), cv::Point(grab_x + radius, grab_y - radius), cv::Scalar(0, 215, 255));
|
|
|
}
|
|
|
int CRootStockGrabPoint::read_ply_file(const char* fn)
|
|
|
{
|
|
|
@@ -870,9 +892,16 @@ void CRootStockGrabPoint::leaf_filter_ror(
|
|
|
//计算点云过滤半径和点数阈值
|
|
|
double stem_diameter = m_cparam.rs_grab_stem_diameter;
|
|
|
if (m_dtype == 0) { stem_diameter = m_cparam.sc_grab_stem_diameter; }
|
|
|
-
|
|
|
+
|
|
|
double remove_radius = stem_diameter;
|
|
|
- int nb_points = stem_diameter * stem_diameter * 2.57 / mean_dist / mean_dist; // (2d*d + pi *d*d) /2
|
|
|
+ double ror_ratio = m_cparam.rs_grab_ror_ratio;
|
|
|
+ if (m_dtype == 0){ror_ratio = m_cparam.sc_grab_ror_ratio;}
|
|
|
+ int nb_points = ror_ratio* stem_diameter * stem_diameter * 2.0 / mean_dist / mean_dist; // (2d*d + pi *d*d) /2
|
|
|
+ if (m_pLogger) {
|
|
|
+ stringstream buff;
|
|
|
+ buff << m_pcdId << ": ror_ratio=" << ror_ratio << ", ror filter nb_points=" << nb_points;
|
|
|
+ m_pLogger->INFO(buff.str());
|
|
|
+ }
|
|
|
|
|
|
//获取叶片的点云
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_leaf(new pcl::PointCloud<pcl::PointXYZ>);
|
chenhongjiang