|
@@ -298,18 +298,18 @@ namespace graft_cv {
|
|
|
viewer_cloud_cluster(tttp, tmp, string("first"));
|
|
|
}
|
|
|
|
|
|
- pcl::PointXYZ selected_pt;
|
|
|
- int selected_pt_idx;
|
|
|
+ pcl::PointXYZ selected_pt;//抓取点坐标,根据茎节点的偏移
|
|
|
+ pcl::PointXYZ selected_pt_ref; //返回茎节点,作为抓取点偏的基点
|
|
|
std::vector<int> optimal_seeds_idx;
|
|
|
get_optimal_seed_compare(
|
|
|
cloud_dowm_sampled, //input
|
|
|
cloud_seedling_seed, //input
|
|
|
line_model, //input
|
|
|
selected_pt, //output
|
|
|
- selected_pt_idx, //output
|
|
|
+ selected_pt_ref, //output
|
|
|
optimal_seeds_idx); //output
|
|
|
|
|
|
- if (selected_pt_idx < 0) {
|
|
|
+ if (selected_pt.z < 0) {
|
|
|
if (m_pLogger) {
|
|
|
m_pLogger->ERRORINFO(m_pcdId + ": Not found valid fork point");
|
|
|
}
|
|
@@ -331,7 +331,7 @@ namespace graft_cv {
|
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
//7 保存处理结果到图片
|
|
|
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);
|
|
|
+ gen_result_img(cloud_ror, cloud_dowm_sampled, selected_pt, selected_pt_ref,rst_img);
|
|
|
if (m_ppImgSaver && *m_ppImgSaver) {
|
|
|
(*m_ppImgSaver)->saveImage(rst_img, m_pcdId + "_rst_0");
|
|
|
}
|
|
@@ -375,7 +375,7 @@ namespace graft_cv {
|
|
|
|
|
|
|
|
|
//viewer_cloud(cloud_cand_demo, std::string("cloud_cand_demo"));
|
|
|
- viewer_cloud_debug(cloud_cand_demo, selected_pt, xz_center, std::string("cloud_cand_demo"));
|
|
|
+ viewer_cloud_debug(cloud_cand_demo, selected_pt, selected_pt_ref, xz_center, std::string("cloud_cand_demo"));
|
|
|
imshow_wait("rst_img", rst_img);
|
|
|
}
|
|
|
return 0;
|
|
@@ -386,6 +386,7 @@ namespace graft_cv {
|
|
|
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,
|
|
|
+ pcl::PointXYZ& selected_pt_ref, //输入,selected_pt_ref,
|
|
|
cv::Mat& rst_img //输出,图片, 640*1280
|
|
|
)
|
|
|
{
|
|
@@ -434,6 +435,13 @@ namespace graft_cv {
|
|
|
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(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 fork_x = cx - int(selected_pt_ref.x / res + 0.5);
|
|
|
+ int fork_y = cy - int(selected_pt_ref.y / res + 0.5);
|
|
|
+ radius = 15;
|
|
|
+ cv::line(rst_img, cv::Point(fork_x - radius, fork_y), cv::Point(fork_x + radius, fork_y), cv::Scalar(153, 51, 255));
|
|
|
+
|
|
|
}
|
|
|
int CRootStockGrabPoint::read_ply_file(const char* fn)
|
|
|
{
|
|
@@ -2295,14 +2303,17 @@ void CRootStockGrabPoint::line_filter(
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud, //input 全部有效点云
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud, //input 茎上直线点云
|
|
|
pcl::ModelCoefficients::Ptr line_model, //input
|
|
|
- pcl::PointXYZ&pt, // 返回抓取的点坐标
|
|
|
- int &pt_idx, // 返回点index
|
|
|
+ pcl::PointXYZ&pt, // 返回抓取的点坐标,基于pt_ref的偏移点
|
|
|
+ pcl::PointXYZ &pt_ref, // 返回点茎节的参考点
|
|
|
std::vector<int>& valid_line_index // 返回有效直线点index
|
|
|
)
|
|
|
{
|
|
|
valid_line_index.clear();
|
|
|
float th_ratio = 2.5; //原始点云和直线点云邻域尺寸增加原来的20%,不能当做抓取点
|
|
|
- pt_idx = -1;
|
|
|
+ pt.x = -1000.0;
|
|
|
+ pt.y = -1000.0;
|
|
|
+ pt.z = -1000.0;
|
|
|
+
|
|
|
|
|
|
float ymin, ymax;
|
|
|
ymin = m_cparam.rs_grab_ymin;
|
|
@@ -2352,15 +2363,17 @@ void CRootStockGrabPoint::line_filter(
|
|
|
|
|
|
Eigen::Vector4f min_point;
|
|
|
Eigen::Vector4f max_point;
|
|
|
- pcl::getMinMax3D(*cloud_inbox, min_point, max_point);
|
|
|
- dz = max_point(2) - min_point(2);
|
|
|
+ pcl::getMinMax3D(*cloud_inbox, min_point, max_point);
|
|
|
dx = max_point(0) - min_point(0);
|
|
|
+ stem_width.push_back(dx);
|
|
|
+
|
|
|
+ /*dz = max_point(2) - min_point(2);
|
|
|
if (dx > dz) {
|
|
|
stem_width.push_back(dx);
|
|
|
}
|
|
|
else {
|
|
|
stem_width.push_back(dz);
|
|
|
- }
|
|
|
+ } */
|
|
|
}
|
|
|
else {
|
|
|
stem_width.push_back(0);
|
|
@@ -2380,6 +2393,8 @@ void CRootStockGrabPoint::line_filter(
|
|
|
float stdv = get_hist_std(valid_stem_width, mu);
|
|
|
|
|
|
int max_pos = std::max_element(stem_width.begin(), stem_width.end()) - stem_width.begin();
|
|
|
+ int max_pos_ref = max_pos;
|
|
|
+
|
|
|
float max_val = stem_width.at(max_pos);
|
|
|
float th = mu + th_ratio * stdv;
|
|
|
if(m_dtype == 0){
|
|
@@ -2404,6 +2419,7 @@ void CRootStockGrabPoint::line_filter(
|
|
|
}
|
|
|
max_pos = i;
|
|
|
}
|
|
|
+ max_pos_ref = max_pos;
|
|
|
max_pos += static_cast<int>(m_cparam.rs_grab_offset);
|
|
|
}
|
|
|
else{
|
|
@@ -2418,6 +2434,7 @@ void CRootStockGrabPoint::line_filter(
|
|
|
}
|
|
|
max_pos = i;
|
|
|
}
|
|
|
+ max_pos_ref = max_pos;
|
|
|
max_pos += static_cast<int>(m_cparam.rs_grab_offset);
|
|
|
}
|
|
|
if (max_pos < 0) { max_pos = 0; }
|
|
@@ -2446,7 +2463,9 @@ void CRootStockGrabPoint::line_filter(
|
|
|
pt.y = cy;
|
|
|
pt.z = z_mu;
|
|
|
|
|
|
- pt_idx = max_pos;
|
|
|
+ pt_ref.x = online_points.at(max_pos_ref).x;
|
|
|
+ pt_ref.y = online_points.at(max_pos_ref).y;
|
|
|
+ pt_ref.z = online_points.at(max_pos_ref).z;
|
|
|
|
|
|
//显示位置
|
|
|
if (m_cparam.image_show) {
|
|
@@ -2527,7 +2546,8 @@ void CRootStockGrabPoint::line_filter(
|
|
|
}
|
|
|
void CRootStockGrabPoint::viewer_cloud_debug(
|
|
|
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
|
|
|
- pcl::PointXYZ &p,
|
|
|
+ pcl::PointXYZ &p, //抓取点
|
|
|
+ pcl::PointXYZ &p_ref,//分叉点
|
|
|
pcl::PointXYZ &root_pt,
|
|
|
std::string&winname)
|
|
|
{
|
|
@@ -2564,11 +2584,27 @@ void CRootStockGrabPoint::line_filter(
|
|
|
root_px1.z = root_pt.z;
|
|
|
|
|
|
root_py0.x = root_pt.x;
|
|
|
- root_py0.y = root_pt.y - 5.0;
|
|
|
- root_py0.z = root_pt.z;
|
|
|
+ root_py0.y = root_pt.y;
|
|
|
+ root_py0.z = root_pt.z - 5.0;
|
|
|
root_py1.x = root_pt.x;
|
|
|
- root_py1.y = root_pt.y + 5.0;
|
|
|
- root_py1.z = root_pt.z;
|
|
|
+ root_py1.y = root_pt.y;
|
|
|
+ root_py1.z = root_pt.z + 5.0;
|
|
|
+
|
|
|
+ //茎节点
|
|
|
+ pcl::PointXYZ fork_px0, fork_px1, fork_py0, fork_py1;
|
|
|
+ fork_px0.x = p_ref.x - 5.0;
|
|
|
+ fork_px0.y = p_ref.y;
|
|
|
+ fork_px0.z = p_ref.z;
|
|
|
+ fork_px1.x = p_ref.x + 5.0;
|
|
|
+ fork_px1.y = p_ref.y;
|
|
|
+ fork_px1.z = p_ref.z;
|
|
|
+
|
|
|
+ fork_py0.x = p_ref.x;
|
|
|
+ fork_py0.y = p_ref.y;
|
|
|
+ fork_py0.z = p_ref.z - 5.0;
|
|
|
+ fork_py1.x = p_ref.x;
|
|
|
+ fork_py1.y = p_ref.y;
|
|
|
+ fork_py1.z = p_ref.z + 5.0;
|
|
|
|
|
|
|
|
|
viewer.addLine(p0, x1, 255, 0, 0, "x");
|
|
@@ -2580,6 +2616,9 @@ void CRootStockGrabPoint::line_filter(
|
|
|
viewer.addLine(root_px0, root_px1, 255, 0, 0, "rootx");
|
|
|
viewer.addLine(root_py0, root_py1, 0, 255, 0, "rooty");
|
|
|
|
|
|
+ viewer.addLine(fork_px0, fork_px1, 255, 0, 0, "forkx");
|
|
|
+ viewer.addLine(fork_py0, fork_py1, 0, 255, 0, "forky");
|
|
|
+
|
|
|
while (!viewer.wasStopped()) {
|
|
|
viewer.spinOnce(100);
|
|
|
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
|