|
|
@@ -40,9 +40,7 @@ namespace graft_cv {
|
|
|
m_ppImgSaver(0),
|
|
|
m_1st_row_zmean_rs(-1.0),
|
|
|
m_1st_row_zmean_sc(-1.0),
|
|
|
- m_cloud_mean_dist(0.0),
|
|
|
- m_stem_fork_ys_size(20),
|
|
|
- m_stem_fork_pos_mu(0)
|
|
|
+ m_cloud_mean_dist(0.0)
|
|
|
{
|
|
|
}
|
|
|
|
|
|
@@ -2170,63 +2168,63 @@ void CRootStockGrabPoint::line_filter(
|
|
|
// pt.z = in_cloud->points.at(nearest_idx).z;
|
|
|
// pt_idx = nearest_idx;
|
|
|
//}
|
|
|
- void CRootStockGrabPoint::get_optimal_seed(
|
|
|
- pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
|
|
|
- pcl::PointXYZ&pt,
|
|
|
- int &pt_idx)
|
|
|
- {
|
|
|
- /*double d1 = m_cparam.rs_grab_stem_diameter;
|
|
|
- double d2 = m_cparam.rs_grab_stem_diameter * 4.0;
|
|
|
- if (m_dtype != 0) {
|
|
|
- d1 = m_cparam.sc_grab_stem_diameter;
|
|
|
- d2 = m_cparam.sc_grab_stem_diameter * 4.0;
|
|
|
- }
|
|
|
- std::vector<pcl::PointXYZ>cluster_center;
|
|
|
- std::vector<std::vector<int>> cluster_member;
|
|
|
- euclidean_clustering_ttsas(in_cloud, d1, d2, cluster_center, cluster_member);*/
|
|
|
+ //void CRootStockGrabPoint::get_optimal_seed(
|
|
|
+ // pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
|
|
|
+ // pcl::PointXYZ&pt,
|
|
|
+ // int &pt_idx)
|
|
|
+ //{
|
|
|
+ // /*double d1 = m_cparam.rs_grab_stem_diameter;
|
|
|
+ // double d2 = m_cparam.rs_grab_stem_diameter * 4.0;
|
|
|
+ // if (m_dtype != 0) {
|
|
|
+ // d1 = m_cparam.sc_grab_stem_diameter;
|
|
|
+ // d2 = m_cparam.sc_grab_stem_diameter * 4.0;
|
|
|
+ // }
|
|
|
+ // std::vector<pcl::PointXYZ>cluster_center;
|
|
|
+ // std::vector<std::vector<int>> cluster_member;
|
|
|
+ // euclidean_clustering_ttsas(in_cloud, d1, d2, cluster_center, cluster_member);*/
|
|
|
|
|
|
- float min_y_dist = 1.0e6;
|
|
|
- int opt_idx = -1;
|
|
|
- int member_size = 5;
|
|
|
- float y_opt = m_cparam.rs_grab_y_opt;
|
|
|
- if (m_dtype == 0) {
|
|
|
- y_opt = m_cparam.sc_grab_y_opt;
|
|
|
- }
|
|
|
- for (int i = 0; i < in_cloud->points.size(); ++i) {
|
|
|
- /*if (cluster_member.at(i).size() < member_size) {
|
|
|
- continue;
|
|
|
- }*/
|
|
|
- if (fabs(in_cloud->points.at(i).y - y_opt) < min_y_dist) {
|
|
|
- min_y_dist = fabs(in_cloud->points.at(i).y - y_opt);
|
|
|
- opt_idx = i;
|
|
|
- }
|
|
|
- }
|
|
|
- if (opt_idx < 0) {
|
|
|
- if (m_pLogger) {
|
|
|
- stringstream buff;
|
|
|
- buff << m_pcdId <<": get_optimal_seed failed, get invalid optimal cluster id";
|
|
|
- m_pLogger->ERRORINFO(buff.str());
|
|
|
- }
|
|
|
- return;
|
|
|
- }
|
|
|
- //find nearest pt
|
|
|
- /*float nearest_dist = 1.0e6;
|
|
|
- int nearest_idx = -1;
|
|
|
- for (int i = 0; i < cluster_member.at(opt_idx).size(); ++i) {
|
|
|
- int idx = cluster_member.at(opt_idx).at(i);
|
|
|
- float dist = fabs(in_cloud->points.at(idx).x - cluster_center.at(opt_idx).x) +
|
|
|
- fabs(in_cloud->points.at(idx).y - cluster_center.at(opt_idx).y) +
|
|
|
- fabs(in_cloud->points.at(idx).z - cluster_center.at(opt_idx).z);
|
|
|
- if (dist < nearest_dist) {
|
|
|
- nearest_dist = dist;
|
|
|
- nearest_idx = idx;
|
|
|
- }
|
|
|
- }*/
|
|
|
- pt.x = in_cloud->points.at(opt_idx).x;
|
|
|
- pt.y = in_cloud->points.at(opt_idx).y;
|
|
|
- pt.z = in_cloud->points.at(opt_idx).z;
|
|
|
- pt_idx = opt_idx;
|
|
|
- }
|
|
|
+ // float min_y_dist = 1.0e6;
|
|
|
+ // int opt_idx = -1;
|
|
|
+ // int member_size = 5;
|
|
|
+ // float y_opt = m_cparam.rs_grab_y_opt;
|
|
|
+ // if (m_dtype == 0) {
|
|
|
+ // y_opt = m_cparam.sc_grab_y_opt;
|
|
|
+ // }
|
|
|
+ // for (int i = 0; i < in_cloud->points.size(); ++i) {
|
|
|
+ // /*if (cluster_member.at(i).size() < member_size) {
|
|
|
+ // continue;
|
|
|
+ // }*/
|
|
|
+ // if (fabs(in_cloud->points.at(i).y - y_opt) < min_y_dist) {
|
|
|
+ // min_y_dist = fabs(in_cloud->points.at(i).y - y_opt);
|
|
|
+ // opt_idx = i;
|
|
|
+ // }
|
|
|
+ // }
|
|
|
+ // if (opt_idx < 0) {
|
|
|
+ // if (m_pLogger) {
|
|
|
+ // stringstream buff;
|
|
|
+ // buff << m_pcdId <<": get_optimal_seed failed, get invalid optimal cluster id";
|
|
|
+ // m_pLogger->ERRORINFO(buff.str());
|
|
|
+ // }
|
|
|
+ // return;
|
|
|
+ // }
|
|
|
+ // //find nearest pt
|
|
|
+ // /*float nearest_dist = 1.0e6;
|
|
|
+ // int nearest_idx = -1;
|
|
|
+ // for (int i = 0; i < cluster_member.at(opt_idx).size(); ++i) {
|
|
|
+ // int idx = cluster_member.at(opt_idx).at(i);
|
|
|
+ // float dist = fabs(in_cloud->points.at(idx).x - cluster_center.at(opt_idx).x) +
|
|
|
+ // fabs(in_cloud->points.at(idx).y - cluster_center.at(opt_idx).y) +
|
|
|
+ // fabs(in_cloud->points.at(idx).z - cluster_center.at(opt_idx).z);
|
|
|
+ // if (dist < nearest_dist) {
|
|
|
+ // nearest_dist = dist;
|
|
|
+ // nearest_idx = idx;
|
|
|
+ // }
|
|
|
+ // }*/
|
|
|
+ // pt.x = in_cloud->points.at(opt_idx).x;
|
|
|
+ // pt.y = in_cloud->points.at(opt_idx).y;
|
|
|
+ // pt.z = in_cloud->points.at(opt_idx).z;
|
|
|
+ // pt_idx = opt_idx;
|
|
|
+ //}
|
|
|
//通过比较直线点云和原始点云相同位置邻域内xz的范围，确定此点是否是无干扰点的茎
|
|
|
//void CRootStockGrabPoint::get_optimal_seed_compare(
|
|
|
// pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud, // 全部有效点云
|
|
|
@@ -2310,7 +2308,7 @@ void CRootStockGrabPoint::line_filter(
|
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr in_line_cloud, //input 茎上直线点云
|
|
|
pcl::ModelCoefficients::Ptr line_model, //input
|
|
|
pcl::PointXYZ&pt, // 返回抓取的点坐标，基于pt_ref的偏移点
|
|
|
- pcl::PointXYZ &pt_ref, // 返回点茎节的参考点
|
|
|
+ pcl::PointXYZ &pt_ref, // 返回点茎节的参考点
|
|
|
std::vector<int>& valid_line_index // 返回有效直线点index
|
|
|
)
|
|
|
{
|
|
|
@@ -2343,7 +2341,7 @@ void CRootStockGrabPoint::line_filter(
|
|
|
box_filter_line.setInputCloud(in_line_cloud);
|
|
|
|
|
|
float radius = m_cparam.rs_grab_stem_diameter;
|
|
|
- float opt_y = m_cparam.rs_grab_y_opt;
|
|
|
+ /*float opt_y = m_cparam.rs_grab_y_opt;
|
|
|
bool opt_y_valid = false;
|
|
|
if (opt_y >= m_cparam.rs_grab_ymin && opt_y <= m_cparam.rs_grab_ymax) {
|
|
|
opt_y_valid = true;
|
|
|
@@ -2354,7 +2352,7 @@ void CRootStockGrabPoint::line_filter(
|
|
|
if (opt_y >= m_cparam.sc_grab_ymin && opt_y <= m_cparam.sc_grab_ymax) {
|
|
|
opt_y_valid = true;
|
|
|
}
|
|
|
- }
|
|
|
+ }*/
|
|
|
|
|
|
float cx, cy, cz, t;
|
|
|
float rx = 1.5;
|
|
|
@@ -2362,7 +2360,7 @@ void CRootStockGrabPoint::line_filter(
|
|
|
float rz = 1.5;
|
|
|
float dz, dx;
|
|
|
//用户指定抓取的y高度,按抓取高度找到对应的点， pt_ref参考位置为上顶点，pt为抓取点
|
|
|
- if (opt_y_valid) {
|
|
|
+ /*if (opt_y_valid) {
|
|
|
cy = opt_y;
|
|
|
t = (cy - line_model->values.at(1)) / line_model->values.at(4);
|
|
|
cx = line_model->values.at(3) * t + line_model->values.at(0);
|
|
|
@@ -2390,7 +2388,7 @@ void CRootStockGrabPoint::line_filter(
|
|
|
pt_ref.y = cy;
|
|
|
pt_ref.z = z_mu;
|
|
|
return;
|
|
|
- }
|
|
|
+ }*/
|
|
|
|
|
|
// 如果opt_y_valid==false，就是用户没有指定抓取的y高度
|
|
|
float max_dist_to_boundary = 0.0;
|
|
|
@@ -2469,17 +2467,17 @@ void CRootStockGrabPoint::line_filter(
|
|
|
|
|
|
//3 如果fork方法得到位置信息，进行更新，并记录历史
|
|
|
float max_power_th = m_cparam.rs_grab_fork_th;
|
|
|
- float max_dist_to_mean_fork = m_cparam.rs_grab_to_meanfork_max_dist;
|
|
|
+ //float max_dist_to_mean_fork = m_cparam.rs_grab_to_meanfork_max_dist;
|
|
|
if (m_dtype == 0) {
|
|
|
max_power_th = m_cparam.sc_grab_fork_th;
|
|
|
- max_dist_to_mean_fork = m_cparam.sc_grab_to_meanfork_max_dist;
|
|
|
+ //max_dist_to_mean_fork = m_cparam.sc_grab_to_meanfork_max_dist;
|
|
|
}
|
|
|
|
|
|
if (max_idx_to_boundary >= 0) {
|
|
|
max_pos = int(in_line_cloud->points.at(max_idx_to_boundary).y + 0.5 - ymin);
|
|
|
max_pos_ref = max_pos;
|
|
|
//记录历史信息
|
|
|
- if (2.0*max_dist_to_boundary > max_power_th * mu) {
|
|
|
+ /*if (2.0*max_dist_to_boundary > max_power_th * mu) {
|
|
|
m_stem_fork_ys.insert(m_stem_fork_ys.begin(), max_pos);
|
|
|
if (m_stem_fork_ys.size() > m_stem_fork_ys_size) { m_stem_fork_ys.pop_back(); }
|
|
|
m_stem_fork_pos_mu = int(get_hist_mean<int>(m_stem_fork_ys) + 0.5);
|
|
|
@@ -2494,34 +2492,63 @@ void CRootStockGrabPoint::line_filter(
|
|
|
", ratio is = "<< ratio;
|
|
|
m_pLogger->INFO(buff.str());
|
|
|
}
|
|
|
- }
|
|
|
+ } */
|
|
|
}
|
|
|
|
|
|
|
|
|
- //4 用历史均值进行约束，如果偏差大于max_dist_to_mean_fork mm，采用历史均值
|
|
|
+ //4 用茎节上下限高度值进行约束，如果超限，用最低限高度作为茎节高度
|
|
|
+ double grab_fork_yup = m_cparam.rs_grab_fork_yup;
|
|
|
+ double grab_fork_ybt = m_cparam.rs_grab_fork_ybt;
|
|
|
+ if (m_dtype == 0) {
|
|
|
+ grab_fork_yup = m_cparam.sc_grab_fork_yup;
|
|
|
+ grab_fork_ybt = m_cparam.sc_grab_fork_ybt;
|
|
|
+ }
|
|
|
bool out_of_range = false;
|
|
|
- if (m_stem_fork_pos_mu > 0) {
|
|
|
- if (fabs(max_pos - (float)m_stem_fork_pos_mu) > max_dist_to_mean_fork) {
|
|
|
- out_of_range = true;
|
|
|
- int original_max_pos = max_pos;
|
|
|
- max_pos = m_stem_fork_pos_mu;
|
|
|
- max_pos_ref = max_pos;
|
|
|
- if (m_pLogger) {
|
|
|
- stringstream buff;
|
|
|
- buff << m_pcdId << ": warning，self fork postiont = " << original_max_pos <<
|
|
|
- ", USE mean fork postiont " << m_stem_fork_pos_mu;
|
|
|
- m_pLogger->INFO(buff.str());
|
|
|
- }
|
|
|
+ if ((max_pos + ymin) > grab_fork_yup || (max_pos + ymin) < grab_fork_ybt) {
|
|
|
+ out_of_range = true;
|
|
|
+ int original_max_pos = max_pos;
|
|
|
+ max_pos = int(grab_fork_ybt - ymin + 0.5);
|
|
|
+ max_pos_ref = max_pos;
|
|
|
+ if (m_pLogger) {
|
|
|
+ stringstream buff;
|
|
|
+ buff << m_pcdId << ": warning，self fork postiont = " << original_max_pos <<
|
|
|
+ ", USE bottom limit fork postiont " << max_pos <<
|
|
|
+ ", valid fork postiont range:[" << int(grab_fork_ybt - ymin + 0.5) <<
|
|
|
+ ", " << int(grab_fork_yup - ymin + 0.5) << "]";
|
|
|
+ m_pLogger->INFO(buff.str());
|
|
|
}
|
|
|
- else {
|
|
|
- if (m_pLogger) {
|
|
|
- stringstream buff;
|
|
|
- buff << m_pcdId << ": self fork postiont = "<< max_pos<<
|
|
|
- ", reference mean fork postiont = " << m_stem_fork_pos_mu;
|
|
|
- m_pLogger->INFO(buff.str());
|
|
|
- }
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ if (m_pLogger) {
|
|
|
+ stringstream buff;
|
|
|
+ buff << m_pcdId << ": self fork postiont = " << max_pos <<
|
|
|
+ ", valid fork postiont range:[" << int(grab_fork_ybt - ymin + 0.5)<<
|
|
|
+ ", "<< int(grab_fork_yup - ymin + 0.5) << "]";
|
|
|
+ m_pLogger->INFO(buff.str());
|
|
|
}
|
|
|
}
|
|
|
+
|
|
|
+ /*if (fabs(max_pos - (float)m_stem_fork_pos_mu) > max_dist_to_mean_fork) {
|
|
|
+ out_of_range = true;
|
|
|
+ int original_max_pos = max_pos;
|
|
|
+ max_pos = m_stem_fork_pos_mu;
|
|
|
+ max_pos_ref = max_pos;
|
|
|
+ if (m_pLogger) {
|
|
|
+ stringstream buff;
|
|
|
+ buff << m_pcdId << ": warning，self fork postiont = " << original_max_pos <<
|
|
|
+ ", USE mean fork postiont " << m_stem_fork_pos_mu;
|
|
|
+ m_pLogger->INFO(buff.str());
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ if (m_pLogger) {
|
|
|
+ stringstream buff;
|
|
|
+ buff << m_pcdId << ": self fork postiont = "<< max_pos<<
|
|
|
+ ", reference mean fork postiont = " << m_stem_fork_pos_mu;
|
|
|
+ m_pLogger->INFO(buff.str());
|
|
|
+ }
|
|
|
+ }*/
|
|
|
+
|
|
|
|
|
|
//5 按指定量偏移
|
|
|
if(m_dtype == 0){
|
chenhongjiang