|
|
@@ -139,8 +139,7 @@ CSeedlingStatus::CSeedlingStatus(
|
|
|
{
|
|
|
int x0 = int(x_min);
|
|
|
int x1 = int(x_max);
|
|
|
- m_hist_length = int((x1 - x0) / step);
|
|
|
- m_history_histogram = cv::Mat::zeros(m_max_size, m_hist_length, CV_32F);
|
|
|
+ m_hist_length = int((x1 - x0) / step);
|
|
|
m_history_point_size = cv::Mat::zeros(m_max_size, 1, CV_32F);
|
|
|
}
|
|
|
|
|
|
@@ -165,25 +164,30 @@ void CSeedlingStatus::set_x_centers(std::vector<double>&cx)
|
|
|
m_idx_up.push_back(idx_up);
|
|
|
}
|
|
|
m_history_status = cv::Mat::zeros(m_max_size, m_center_x.size(), CV_8U);
|
|
|
+ m_history_histogram = cv::Mat::zeros(m_max_size, m_center_x.size(), CV_32F);
|
|
|
|
|
|
}
|
|
|
void CSeedlingStatus::append_hist(
|
|
|
- std::vector<int>&xhist, //input
|
|
|
- std::vector<bool>&xstatus //output
|
|
|
+ std::vector<int>&xhist //input
|
|
|
+ //std::vector<bool>&xstatus //output
|
|
|
)
|
|
|
{
|
|
|
assert(xhist.size() == m_hist_length);
|
|
|
+
|
|
|
+ std::vector<float>center_count;
|
|
|
+ calculate_center_size(xhist, center_count);
|
|
|
+ assert(center_count.size() == m_center_x.size());
|
|
|
+
|
|
|
m_global_cursor++;
|
|
|
if (m_record_cursor < m_max_size-1) {
|
|
|
m_record_cursor++;
|
|
|
float pc_size = 0.0;
|
|
|
- for (int i = 0; i < xhist.size(); ++i) {
|
|
|
- if (i >= m_hist_length) { continue; }
|
|
|
- m_history_histogram.at<float>(m_record_cursor,i) = xhist.at(i);
|
|
|
- pc_size += xhist.at(i);
|
|
|
+ for (int i = 0; i < center_count.size(); ++i) {
|
|
|
+ m_history_histogram.at<float>(m_record_cursor,i) = center_count.at(i);
|
|
|
+ pc_size += center_count.at(i);
|
|
|
}
|
|
|
m_history_point_size.at<float>(m_record_cursor, 0) = pc_size;
|
|
|
- get_status(xstatus);
|
|
|
+ //get_status(xstatus);
|
|
|
}
|
|
|
else {
|
|
|
//数据上移一行，数据放在最后一行
|
|
|
@@ -194,238 +198,424 @@ void CSeedlingStatus::append_hist(
|
|
|
}
|
|
|
for (int row = 0; row < m_history_point_size.rows - 1; ++row) {
|
|
|
m_history_point_size.at<float>(row, 0) = m_history_point_size.at<float>(row+1, 0);
|
|
|
- }
|
|
|
- /*memcpy_s(m_history_histogram.data,
|
|
|
- m_history_histogram.step[0] * (m_max_size - 1),
|
|
|
- m_history_histogram.data + m_history_histogram.step[0],
|
|
|
- m_history_histogram.step[0] * (m_max_size - 1));*/
|
|
|
-
|
|
|
- /*memcpy_s(m_history_point_size.data,
|
|
|
- m_history_point_size.step[0] * (m_max_size - 1),
|
|
|
- m_history_point_size.data + m_history_point_size.step[0],
|
|
|
- m_history_point_size.step[0] * (m_max_size - 1));*/
|
|
|
+ }
|
|
|
|
|
|
float pc_size = 0.0;
|
|
|
- for (int i = 0; i < xhist.size(); ++i) {
|
|
|
- m_history_histogram.at<float>(m_history_histogram.rows - 1, i) = float(xhist.at(i));
|
|
|
- pc_size += xhist.at(i);
|
|
|
+ for (int i = 0; i < center_count.size(); ++i) {
|
|
|
+ m_history_histogram.at<float>(m_history_histogram.rows - 1, i) = center_count.at(i);
|
|
|
+ pc_size += center_count.at(i);
|
|
|
}
|
|
|
m_history_point_size.at<float>(m_history_point_size.rows - 1, 0) = pc_size;
|
|
|
- get_status(xstatus);
|
|
|
+ //get_status(xstatus);
|
|
|
}
|
|
|
|
|
|
|
|
|
}
|
|
|
-void CSeedlingStatus::get_status(std::vector<bool>&xstatus)
|
|
|
+void CSeedlingStatus::calculate_center_size(std::vector<int>&xhist, std::vector<float>&x_count)
|
|
|
{
|
|
|
- xstatus.clear();
|
|
|
- xstatus.assign(m_center_x.size(), false);
|
|
|
-
|
|
|
- //1 如果没有记录输入，返回没有苗
|
|
|
- if (m_record_cursor < 0) { return; }
|
|
|
-
|
|
|
- //2 如果是第一次，没有参考，用自身的分布阈值判断是否有苗（可能不准确，但没有其他办法）
|
|
|
- //点云分布情况分析
|
|
|
- // 每个bin的数量阈值设定m_bin_step宽度，至少有10毫米的点云，才认为有苗
|
|
|
- float th_hist = m_bin_step * 10 / m_pc_mean_dist / m_pc_mean_dist;
|
|
|
- if (m_record_cursor == 0) {
|
|
|
- std::vector<bool> hist_status;
|
|
|
- hist_status.assign(m_hist_length, false);
|
|
|
- for (int i = 0; i < m_hist_length; ++i) {
|
|
|
- if (m_history_histogram.at<float>(m_record_cursor, i) > th_hist) {
|
|
|
- hist_status.at(i) = true;
|
|
|
- }
|
|
|
- }
|
|
|
- for (int i = 0; i < m_center_x.size(); ++i) {
|
|
|
- int idx_low = m_idx_low.at(i);
|
|
|
- int idx_up = m_idx_up.at(i);
|
|
|
- int valid_bin_cnt = 0;
|
|
|
- for (int k = idx_low; k < idx_up; ++k) {
|
|
|
- if (hist_status.at(k)) { valid_bin_cnt++; }
|
|
|
- }
|
|
|
- double valid_ratio = (double)valid_bin_cnt / (double)(idx_up - idx_low);
|
|
|
- xstatus.at(i) = valid_ratio > 0.5;
|
|
|
- }
|
|
|
- //update m_history_status
|
|
|
- for (int i = 0; i < m_history_status.cols; ++i) {
|
|
|
- m_history_status.at<unsigned char>(m_record_cursor, i) = xstatus.at(i);
|
|
|
+ x_count.clear();
|
|
|
+ for (int i = 0; i < m_center_x.size(); ++i) {
|
|
|
+ int idx_low = m_idx_low.at(i);
|
|
|
+ int idx_up = m_idx_up.at(i);
|
|
|
+ float valid_pc_cnt = 0;
|
|
|
+ for (int k = idx_low; k <= idx_up; ++k) {
|
|
|
+ if (k < 0 || k >= xhist.size()) { continue; }
|
|
|
+ valid_pc_cnt += xhist.at(k);
|
|
|
}
|
|
|
- return;
|
|
|
+ x_count.push_back(valid_pc_cnt);
|
|
|
}
|
|
|
+}
|
|
|
+//void CSeedlingStatus::calculate_center_size(int cursor, std::vector<float>&x_count)
|
|
|
+//{
|
|
|
+// x_count.clear();
|
|
|
+// for (int i = 0; i < m_center_x.size(); ++i) {
|
|
|
+// int idx_low = m_idx_low.at(i);
|
|
|
+// int idx_up = m_idx_up.at(i);
|
|
|
+// float valid_pc_cnt = 0;
|
|
|
+// for (int k = idx_low; k <= idx_up; ++k) {
|
|
|
+// valid_pc_cnt += m_history_histogram.at<float>(cursor, i);
|
|
|
+// }
|
|
|
+// x_count.push_back(valid_pc_cnt);
|
|
|
+// }
|
|
|
+//}
|
|
|
+//void CSeedlingStatus::get_status(std::vector<bool>&xstatus)
|
|
|
+//{
|
|
|
+// //根据历史信息评估：
|
|
|
+// // 1）位置上是否有苗
|
|
|
+// // 2）历史上是否已经取走苗
|
|
|
+// // 3）是否有新进的苗
|
|
|
+//
|
|
|
+// xstatus.clear();
|
|
|
+// xstatus.assign(m_center_x.size(), false);
|
|
|
+//
|
|
|
+// //1 如果没有记录输入，返回没有苗
|
|
|
+// if (m_record_cursor < 0) { return; }
|
|
|
+//
|
|
|
+// //2 如果是第一次，没有参考，用自身的分布阈值判断是否有苗（可能不准确，但没有其他办法）
|
|
|
+// //点云分布情况分析
|
|
|
+// // 每个bin的数量阈值设定m_bin_step宽度，至少有10毫米的点云，才认为有苗
|
|
|
+// float th_hist = m_bin_step * 10 / m_pc_mean_dist / m_pc_mean_dist;
|
|
|
+// if (m_record_cursor == 0) {
|
|
|
+// //std::vector<bool> hist_status;
|
|
|
+// //hist_status.assign(m_hist_length, false);
|
|
|
+// //for (int i = 0; i < m_hist_length; ++i) {
|
|
|
+// // if (m_history_histogram.at<float>(m_record_cursor, i) > th_hist) {
|
|
|
+// // hist_status.at(i) = true;
|
|
|
+// // }
|
|
|
+// //}
|
|
|
+// //for (int i = 0; i < m_center_x.size(); ++i) {
|
|
|
+// // int idx_low = m_idx_low.at(i);
|
|
|
+// // int idx_up = m_idx_up.at(i);
|
|
|
+// // int valid_bin_cnt = 0;
|
|
|
+// // for (int k = idx_low; k < idx_up; ++k) {
|
|
|
+// // if (hist_status.at(k)) { valid_bin_cnt++; }
|
|
|
+// // }
|
|
|
+// // double valid_ratio = (double)valid_bin_cnt / (double)(idx_up - idx_low);
|
|
|
+// // xstatus.at(i) = valid_ratio > 0.5;
|
|
|
+// //}
|
|
|
+// ////update m_history_status
|
|
|
+// //for (int i = 0; i < m_history_status.cols; ++i) {
|
|
|
+// // m_history_status.at<unsigned char>(m_record_cursor, i) = xstatus.at(i);
|
|
|
+// //}
|
|
|
+// //return;
|
|
|
+//
|
|
|
+// std::vector<float> x_count;
|
|
|
+// calculate_center_size(m_record_cursor, x_count);
|
|
|
+// for (int i = 0; i < x_count.size(); ++i) {
|
|
|
+// int idx_low = m_idx_low.at(i);
|
|
|
+// int idx_up = m_idx_up.at(i);
|
|
|
+// xstatus.at(i) = x_count.at(i) > (idx_up - idx_low + 1) * th_hist;
|
|
|
+// }
|
|
|
+// //update m_history_status
|
|
|
+// for (int i = 0; i < m_history_status.cols; ++i) {
|
|
|
+// m_history_status.at<unsigned char>(m_record_cursor, i) = xstatus.at(i);
|
|
|
+// }
|
|
|
+// return;
|
|
|
+// }
|
|
|
+//
|
|
|
+// //3 2次或更多，通过前后2次差分析苗取走的情况
|
|
|
+// //3.1 计算被取走的点云位置分布
|
|
|
+// std::vector<float>hist_diff;
|
|
|
+// hist_diff.assign(m_hist_length, 0.0);
|
|
|
+// float sum_dn = 0.0;
|
|
|
+// float sum_n = 0.0;
|
|
|
+// float diff_cnt = 0.0;
|
|
|
+// for (int i = 0; i < m_hist_length; ++i) {
|
|
|
+// diff_cnt = m_history_histogram.at<float>(m_record_cursor - 1, i) -
|
|
|
+// m_history_histogram.at<float>(m_record_cursor, i);
|
|
|
+// hist_diff.at(i) = diff_cnt;
|
|
|
+// sum_n += diff_cnt;
|
|
|
+// sum_dn += diff_cnt * i;
|
|
|
+// }
|
|
|
+// /*if (m_record_cursor < m_max_size) {
|
|
|
+// for (int i = 0; i < m_hist_length; ++i) {
|
|
|
+// diff_cnt = m_history_histogram.at<float>(m_record_cursor - 1, i) -
|
|
|
+// m_history_histogram.at<float>(m_record_cursor, i);
|
|
|
+// hist_diff.at(i) = diff_cnt;
|
|
|
+// sum_n += diff_cnt;
|
|
|
+// sum_dn += diff_cnt * i;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// else {
|
|
|
+// for (int i = 0; i < m_hist_length; ++i) {
|
|
|
+// diff_cnt = m_history_histogram.at<float>(m_max_size - 2, i) -
|
|
|
+// m_history_histogram.at<float>(m_max_size - 1, i);
|
|
|
+// hist_diff.at(i) = diff_cnt;
|
|
|
+// sum_n += diff_cnt;
|
|
|
+// sum_dn += diff_cnt * i;
|
|
|
+// }
|
|
|
+//
|
|
|
+// }*/
|
|
|
+//
|
|
|
+// //3.2 统计增减点云的状态，区分点云增加，点云减小，点云没变化的部分
|
|
|
+// std::vector<int> hist_status_2d; //3种状态记录： -1取走，0没变化，1上苗
|
|
|
+// hist_status_2d.assign(m_hist_length, 0);
|
|
|
+// int add_cnt = 0;
|
|
|
+// int sub_cnt = 0;
|
|
|
+// for (int i = 0; i < m_hist_length; ++i) {
|
|
|
+// if (hist_diff.at(i) > th_hist) {
|
|
|
+// hist_status_2d.at(i) = -1;
|
|
|
+// sub_cnt += 1;
|
|
|
+// }
|
|
|
+// if (hist_diff.at(i) < -th_hist) {
|
|
|
+// hist_status_2d.at(i) = 1;
|
|
|
+// add_cnt += 1;
|
|
|
+// }
|
|
|
+// }
|
|
|
+//
|
|
|
+// //3.3 判断苗的整体情况
|
|
|
+// double seedling_distance = m_center_x.at(1) - m_center_x.at(0); //株间距离
|
|
|
+// double grid_one_seedling = seedling_distance / m_bin_step; //每穴位占histogram的桶数
|
|
|
+// //3.3.1进一排苗
|
|
|
+// if (add_cnt > grid_one_seedling*3.0) {
|
|
|
+// xstatus.assign(m_center_x.size(), true);
|
|
|
+// //update m_history_status
|
|
|
+// if (m_record_cursor == m_global_cursor) {
|
|
|
+// if (m_record_cursor < m_max_size) {
|
|
|
+// for (int i = 0; i < m_history_status.cols; ++i) {
|
|
|
+// m_history_status.at<unsigned char>(m_record_cursor, i) = xstatus.at(i);
|
|
|
+// }
|
|
|
+// }
|
|
|
+// else {
|
|
|
+//
|
|
|
+// }
|
|
|
+// }
|
|
|
+// else {
|
|
|
+// //数据上移一行，数据放在最后一行
|
|
|
+// for (int row = 0; row < m_history_status.rows - 1; ++row) {
|
|
|
+// for (int col = 0; col < m_history_status.cols; ++col) {
|
|
|
+// m_history_status.at<float>(row, col) = m_history_status.at<float>(row + 1, col);
|
|
|
+// }
|
|
|
+// }
|
|
|
+// /*memcpy_s(m_history_status.data,
|
|
|
+// m_history_status.step[0] * (m_max_size - 1),
|
|
|
+// m_history_status.data + m_history_status.step[0],
|
|
|
+// m_history_status.step[0] * (m_max_size - 1));
|
|
|
+// */
|
|
|
+// for (int i = 0; i < m_history_status.cols; ++i) {
|
|
|
+// m_history_status.at<unsigned char>(m_max_size-1, i) = xstatus.at(i);
|
|
|
+// }
|
|
|
+// }
|
|
|
+// return;
|
|
|
+// }
|
|
|
+//
|
|
|
+// std::vector<size_t>sorted_idx;
|
|
|
+// std::vector<float>sub_seedling_score; //移出植株得分，记录每个穴位上点云变化得分
|
|
|
+// //3.3.2 变化很小，说明没有改变（没能成功抓走）
|
|
|
+// if (add_cnt + sub_cnt < 0.5 * grid_one_seedling) {
|
|
|
+// goto no_change;
|
|
|
+// }
|
|
|
+// //3.3.3 否则的话，就是抓走过一个苗
|
|
|
+// //找到被取走的苗的中心，然后根据dtype确定有苗的位置
|
|
|
+// //找覆盖范围最大的区域
|
|
|
+// double sub_cent_indx = sum_dn / sum_n; //计算改变范围的中心，目前没用到
|
|
|
+// sub_seedling_score.assign(m_center_x.size(), 0.0);
|
|
|
+// for (int idx = 0; idx < hist_status_2d.size(); ++idx) {
|
|
|
+// if (hist_status_2d.at(idx) >= 0) {
|
|
|
+// //这个histogram上没有移出，不统计， hist_status_2d的值域：-1取走，0没变化，1上苗
|
|
|
+// continue;
|
|
|
+// }
|
|
|
+// for (int i = 0; i < m_center_x.size(); ++i) {
|
|
|
+// int idx_low = m_idx_low.at(i);
|
|
|
+// int idx_up = m_idx_up.at(i);
|
|
|
+// if (idx >= idx_low && idx < idx_up) {
|
|
|
+// sub_seedling_score.at(i) += 1.0;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// }
|
|
|
+// int sub_pos = -1;
|
|
|
+// sorted_idx = sort_indexes_e(sub_seedling_score, false);
|
|
|
+// for (auto& idx : sorted_idx) {
|
|
|
+// if (sub_seedling_score.at(idx) < 0.25 * grid_one_seedling) {
|
|
|
+// //如果改变量，不到穴位范围的一半，不认为是移走的
|
|
|
+// continue;
|
|
|
+// }
|
|
|
+// if (m_history_status.at<unsigned char>(m_record_cursor - 1, idx) == 0) {
|
|
|
+// //如果这个位置上一帧就没有苗，那判别也是错误的
|
|
|
+// continue;
|
|
|
+// }
|
|
|
+// sub_pos = idx;
|
|
|
+// break;//找到得分最高，并且满足条件的位置，就是被抓走的位置，跳出
|
|
|
+// }
|
|
|
+// if (sub_pos >= 0) {
|
|
|
+// xstatus.assign(m_center_x.size(), false);
|
|
|
+// int cursor = m_record_cursor-1;
|
|
|
+// for (int i = 0; i < m_history_status.cols; ++i) {
|
|
|
+// if (m_history_status.at<unsigned char>(cursor, i) == 1) {
|
|
|
+// xstatus.at(i) = true;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// xstatus.at(sub_pos) = false;
|
|
|
+//
|
|
|
+// //update m_history_status
|
|
|
+// if (m_record_cursor == m_global_cursor) {
|
|
|
+// for (int i = 0; i < m_history_status.cols; ++i) {
|
|
|
+// m_history_status.at<unsigned char>(m_record_cursor, i) = xstatus.at(i);
|
|
|
+// }
|
|
|
+// }
|
|
|
+// else{
|
|
|
+// //数据上移一行，数据放在最后一行
|
|
|
+// for (int row = 0; row < m_history_status.rows - 1; ++row) {
|
|
|
+// for (int col = 0; col < m_history_status.cols; ++col) {
|
|
|
+// m_history_status.at<float>(row, col) = m_history_status.at<float>(row + 1, col);
|
|
|
+// }
|
|
|
+// }
|
|
|
+// /*memcpy_s(m_history_status.data,
|
|
|
+// m_history_status.step[0] * (m_max_size - 1),
|
|
|
+// m_history_status.data + m_history_status.step[0],
|
|
|
+// m_history_status.step[0] * (m_max_size - 1));*/
|
|
|
+// for (int i = 0; i < m_history_status.cols; ++i) {
|
|
|
+// m_history_status.at<unsigned char>(m_max_size - 1, i) = xstatus.at(i);
|
|
|
+// }
|
|
|
+// }
|
|
|
+// return;
|
|
|
+// }
|
|
|
+// else {
|
|
|
+// //如果没有找到有效位置，按没有变化处理
|
|
|
+// goto no_change;
|
|
|
+// }
|
|
|
+//
|
|
|
+//no_change:
|
|
|
+// //没有改变,用上一次的结果
|
|
|
+// xstatus.assign(m_center_x.size(), true);
|
|
|
+// //update m_history_status
|
|
|
+// if (m_record_cursor == m_global_cursor) {
|
|
|
+// for (int i = 0; i < m_history_status.cols; ++i) {
|
|
|
+// m_history_status.at<unsigned char>(m_record_cursor, i) =
|
|
|
+// m_history_status.at<unsigned char>(m_record_cursor - 1, i);
|
|
|
+// if (m_history_status.at<unsigned char>(m_record_cursor - 1, i) == 0) {
|
|
|
+// xstatus.at(i) = false;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// }
|
|
|
+// else{
|
|
|
+// //数据上移一行，数据放在最后一行
|
|
|
+// for (int row = 0; row < m_history_status.rows - 1; ++row) {
|
|
|
+// for (int col = 0; col < m_history_status.cols; ++col) {
|
|
|
+// m_history_status.at<float>(row, col) = m_history_status.at<float>(row + 1, col);
|
|
|
+// }
|
|
|
+// }
|
|
|
+// /*memcpy_s(m_history_status.data,
|
|
|
+// m_history_status.step[0] * (m_max_size - 1),
|
|
|
+// m_history_status.data + m_history_status.step[0],
|
|
|
+// m_history_status.step[0] * (m_max_size - 1));*/
|
|
|
+// for (int i = 0; i < m_history_status.cols; ++i) {
|
|
|
+// m_history_status.at<unsigned char>(m_max_size - 1, i) =
|
|
|
+// m_history_status.at<unsigned char>(m_max_size - 2, i);
|
|
|
+// if (m_history_status.at<unsigned char>(m_max_size - 2, i) == 0) {
|
|
|
+// xstatus.at(i) = false;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// }
|
|
|
+//}
|
|
|
|
|
|
- //3 2次或更多，通过前后2次差分析苗取走的情况
|
|
|
- //3.1 计算被取走的点云位置分布
|
|
|
- std::vector<float>hist_diff;
|
|
|
- hist_diff.assign(m_hist_length, 0.0);
|
|
|
- float sum_dn = 0.0;
|
|
|
- float sum_n = 0.0;
|
|
|
- float diff_cnt = 0.0;
|
|
|
- for (int i = 0; i < m_hist_length; ++i) {
|
|
|
- diff_cnt = m_history_histogram.at<float>(m_record_cursor - 1, i) -
|
|
|
- m_history_histogram.at<float>(m_record_cursor, i);
|
|
|
- hist_diff.at(i) = diff_cnt;
|
|
|
- sum_n += diff_cnt;
|
|
|
- sum_dn += diff_cnt * i;
|
|
|
+int CSeedlingStatus::get_status(int& max_idx, float&max_change)
|
|
|
+{
|
|
|
+ //根据历史信息评估：
|
|
|
+ // 1）评估是否新上苗
|
|
|
+ // 2）判断被取走的那个苗
|
|
|
+
|
|
|
+ //状态 1 ---此次为新上苗， 0 无显著变化， -1 上一次取苗成功
|
|
|
+ int status = 1;
|
|
|
+
|
|
|
+ //1 如果没有记录输入，返回没有苗
|
|
|
+ if (m_record_cursor <= 0) {
|
|
|
+ return status;
|
|
|
}
|
|
|
- /*if (m_record_cursor < m_max_size) {
|
|
|
- for (int i = 0; i < m_hist_length; ++i) {
|
|
|
- diff_cnt = m_history_histogram.at<float>(m_record_cursor - 1, i) -
|
|
|
- m_history_histogram.at<float>(m_record_cursor, i);
|
|
|
- hist_diff.at(i) = diff_cnt;
|
|
|
- sum_n += diff_cnt;
|
|
|
- sum_dn += diff_cnt * i;
|
|
|
- }
|
|
|
+
|
|
|
+ float change_threshold = 500.0;//点云变化，是否有苗新加或取走的阈值
|
|
|
+ float change_count = m_history_point_size.at<float>(m_record_cursor, 0) - m_history_point_size.at<float>(m_record_cursor - 1, 0);
|
|
|
+ if (change_count > 2.0 * change_threshold) {
|
|
|
+ status = 1;
|
|
|
}
|
|
|
else {
|
|
|
- for (int i = 0; i < m_hist_length; ++i) {
|
|
|
- diff_cnt = m_history_histogram.at<float>(m_max_size - 2, i) -
|
|
|
- m_history_histogram.at<float>(m_max_size - 1, i);
|
|
|
- hist_diff.at(i) = diff_cnt;
|
|
|
- sum_n += diff_cnt;
|
|
|
- sum_dn += diff_cnt * i;
|
|
|
+ if (change_count < -change_threshold) {
|
|
|
+ status = -1;
|
|
|
}
|
|
|
-
|
|
|
- }*/
|
|
|
-
|
|
|
- //3.2 统计增减点云的状态，区分点云增加，点云减小，点云没变化的部分
|
|
|
- std::vector<int> hist_status_2d; //3种状态记录： -1取走，0没变化，1上苗
|
|
|
- hist_status_2d.assign(m_hist_length, 0);
|
|
|
- int add_cnt = 0;
|
|
|
- int sub_cnt = 0;
|
|
|
- for (int i = 0; i < m_hist_length; ++i) {
|
|
|
- if (hist_diff.at(i) > th_hist) {
|
|
|
- hist_status_2d.at(i) = -1;
|
|
|
- sub_cnt += 1;
|
|
|
+ else {
|
|
|
+ status = 0;
|
|
|
}
|
|
|
- if (hist_diff.at(i) < -th_hist) {
|
|
|
- hist_status_2d.at(i) = 1;
|
|
|
- add_cnt += 1;
|
|
|
+ }
|
|
|
+ max_change = 0;
|
|
|
+ max_idx = 0;
|
|
|
+ for (int i = 0; i < m_history_histogram.cols; ++i) {
|
|
|
+ float d = m_history_histogram.at<float>(m_record_cursor, i) - m_history_histogram.at<float>(m_record_cursor - 1, i);
|
|
|
+ if (fabs(d) > fabs(max_change)) {
|
|
|
+ max_change = d;
|
|
|
+ max_idx = i;
|
|
|
}
|
|
|
}
|
|
|
+ if (max_change < -change_threshold) {
|
|
|
+ status = -1;
|
|
|
+ }
|
|
|
+ return status;
|
|
|
+}
|
|
|
|
|
|
- //3.3 判断苗的整体情况
|
|
|
- double seedling_distance = m_center_x.at(1) - m_center_x.at(0); //株间距离
|
|
|
- double grid_one_seedling = seedling_distance / m_bin_step; //每穴位占histogram的桶数
|
|
|
- //3.3.1进一排苗
|
|
|
- if (add_cnt > grid_one_seedling*3.0) {
|
|
|
- xstatus.assign(m_center_x.size(), true);
|
|
|
- //update m_history_status
|
|
|
- if (m_record_cursor == m_global_cursor) {
|
|
|
- if (m_record_cursor < m_max_size) {
|
|
|
- for (int i = 0; i < m_history_status.cols; ++i) {
|
|
|
- m_history_status.at<unsigned char>(m_record_cursor, i) = xstatus.at(i);
|
|
|
- }
|
|
|
- }
|
|
|
- else {
|
|
|
-
|
|
|
+
|
|
|
+void CSeedlingStatus::real_result_update(
|
|
|
+ std::vector<pcl::PointXYZ>& root
|
|
|
+)
|
|
|
+{
|
|
|
+ m_stem_status.clear();
|
|
|
+ m_stem_status.assign(m_center_x.size(), 0);
|
|
|
+
|
|
|
+ for (auto& p : root) {
|
|
|
+ int mini = -1;
|
|
|
+ float mind = 1000.0;
|
|
|
+ for (int i = 0; i < m_center_x.size();++i) {
|
|
|
+ float d = fabs(m_center_x.at(i) - p.x);
|
|
|
+ if (d < mind) {
|
|
|
+ mind = d;
|
|
|
+ mini = i;
|
|
|
}
|
|
|
}
|
|
|
- else {
|
|
|
- //数据上移一行，数据放在最后一行
|
|
|
- for (int row = 0; row < m_history_status.rows - 1; ++row) {
|
|
|
- for (int col = 0; col < m_history_status.cols; ++col) {
|
|
|
- m_history_status.at<float>(row, col) = m_history_status.at<float>(row + 1, col);
|
|
|
- }
|
|
|
- }
|
|
|
- /*memcpy_s(m_history_status.data,
|
|
|
- m_history_status.step[0] * (m_max_size - 1),
|
|
|
- m_history_status.data + m_history_status.step[0],
|
|
|
- m_history_status.step[0] * (m_max_size - 1));
|
|
|
- */
|
|
|
- for (int i = 0; i < m_history_status.cols; ++i) {
|
|
|
- m_history_status.at<unsigned char>(m_max_size-1, i) = xstatus.at(i);
|
|
|
- }
|
|
|
- }
|
|
|
- return;
|
|
|
+ if (mini >= 0) {
|
|
|
+ m_stem_status.at(mini) = 1;
|
|
|
+ }
|
|
|
}
|
|
|
+}
|
|
|
|
|
|
- std::vector<size_t>sorted_idx;
|
|
|
- std::vector<float>sub_seedling_score; //移出植株得分，记录每个穴位上点云变化得分
|
|
|
- //3.3.2 变化很小，说明没有改变（没能成功抓走）
|
|
|
- if (add_cnt + sub_cnt < 0.5 * grid_one_seedling) {
|
|
|
- goto no_change;
|
|
|
- }
|
|
|
- //3.3.3 否则的话，就是抓走过一个苗
|
|
|
- //找到被取走的苗的中心，然后根据dtype确定有苗的位置
|
|
|
- //找覆盖范围最大的区域
|
|
|
- double sub_cent_indx = sum_dn / sum_n; //计算改变范围的中心，目前没用到
|
|
|
- sub_seedling_score.assign(m_center_x.size(), 0.0);
|
|
|
- for (int idx = 0; idx < hist_status_2d.size(); ++idx) {
|
|
|
- if (hist_status_2d.at(idx) >= 0) {
|
|
|
- //这个histogram上没有移出，不统计， hist_status_2d的值域：-1取走，0没变化，1上苗
|
|
|
- continue;
|
|
|
- }
|
|
|
- for (int i = 0; i < m_center_x.size(); ++i) {
|
|
|
- int idx_low = m_idx_low.at(i);
|
|
|
- int idx_up = m_idx_up.at(i);
|
|
|
- if (idx >= idx_low && idx < idx_up) {
|
|
|
- sub_seedling_score.at(i) += 1.0;
|
|
|
- }
|
|
|
+void CSeedlingStatus::occlusion_result_update(
|
|
|
+ std::vector<int>& leaf_occlusion
|
|
|
+)
|
|
|
+{
|
|
|
+ assert(m_center_x.size() == leaf_occlusion.size());
|
|
|
+ assert(m_center_x.size() == m_stem_status.size());
|
|
|
+ for (int i = 0; i < m_center_x.size(); ++i) {
|
|
|
+ if (m_stem_status.at(i)==0 && leaf_occlusion.at(i)>0) {
|
|
|
+ m_stem_status.at(i) = 2;
|
|
|
}
|
|
|
}
|
|
|
- int sub_pos = -1;
|
|
|
- sorted_idx = sort_indexes_e(sub_seedling_score, false);
|
|
|
- for (auto& idx : sorted_idx) {
|
|
|
- if (sub_seedling_score.at(idx) < 0.25 * grid_one_seedling) {
|
|
|
- //如果改变量，不到穴位范围的一半，不认为是移走的
|
|
|
- continue;
|
|
|
- }
|
|
|
- if (m_history_status.at<unsigned char>(m_record_cursor - 1, idx) == 0) {
|
|
|
- //如果这个位置上一帧就没有苗，那判别也是错误的
|
|
|
- continue;
|
|
|
- }
|
|
|
- sub_pos = idx;
|
|
|
- break;//找到得分最高，并且满足条件的位置，就是被抓走的位置，跳出
|
|
|
+
|
|
|
+}
|
|
|
+//获取植株的状态，是否有苗
|
|
|
+// 在real_result_update（）和occlusion_result_update（）调用后
|
|
|
+// 调用此函数，获取茎的状态
|
|
|
+void CSeedlingStatus::get_stem_status(
|
|
|
+ std::vector<int>&stem_status
|
|
|
+)
|
|
|
+{
|
|
|
+ //状态 1 ---此次为新上苗， 0 无显著变化， -1 上一次取苗成功
|
|
|
+ float max_change_points_count;
|
|
|
+ int max_change_center_idx;
|
|
|
+ int status = get_status(max_change_center_idx, max_change_points_count);
|
|
|
+ if (status > 0) {
|
|
|
+ m_center_grabed_record.clear();
|
|
|
+ m_center_grabed_record.assign(m_center_x.size(), -1);
|
|
|
}
|
|
|
- if (sub_pos >= 0) {
|
|
|
- xstatus.assign(m_center_x.size(), false);
|
|
|
- int cursor = m_record_cursor-1;
|
|
|
- for (int i = 0; i < m_history_status.cols; ++i) {
|
|
|
- if (m_history_status.at<unsigned char>(cursor, i) == 1) {
|
|
|
- xstatus.at(i) = true;
|
|
|
- }
|
|
|
- }
|
|
|
- xstatus.at(sub_pos) = false;
|
|
|
-
|
|
|
- //update m_history_status
|
|
|
- if (m_record_cursor == m_global_cursor) {
|
|
|
- for (int i = 0; i < m_history_status.cols; ++i) {
|
|
|
- m_history_status.at<unsigned char>(m_record_cursor, i) = xstatus.at(i);
|
|
|
- }
|
|
|
- }
|
|
|
- else{
|
|
|
- //数据上移一行，数据放在最后一行
|
|
|
- for (int row = 0; row < m_history_status.rows - 1; ++row) {
|
|
|
- for (int col = 0; col < m_history_status.cols; ++col) {
|
|
|
- m_history_status.at<float>(row, col) = m_history_status.at<float>(row + 1, col);
|
|
|
+
|
|
|
+ stem_status.clear();
|
|
|
+ if (status >= 0) {
|
|
|
+ //用当前识别的结果
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ m_center_grabed_record.at(max_change_center_idx) = 0;
|
|
|
+
|
|
|
+ //如果max_change_center_idx位置的苗上一次有苗，本次为2，那么久将2改成1
|
|
|
+ if (max_change_points_count < 0) {
|
|
|
+ if (m_history_status.at<unsigned char>(m_record_cursor - 1, max_change_center_idx) != 0) {
|
|
|
+ if (m_stem_status.at(max_change_center_idx) == 2) {
|
|
|
+ m_stem_status.at(max_change_center_idx) = 0;
|
|
|
}
|
|
|
}
|
|
|
- /*memcpy_s(m_history_status.data,
|
|
|
- m_history_status.step[0] * (m_max_size - 1),
|
|
|
- m_history_status.data + m_history_status.step[0],
|
|
|
- m_history_status.step[0] * (m_max_size - 1));*/
|
|
|
- for (int i = 0; i < m_history_status.cols; ++i) {
|
|
|
- m_history_status.at<unsigned char>(m_max_size - 1, i) = xstatus.at(i);
|
|
|
- }
|
|
|
- }
|
|
|
- return;
|
|
|
+ }
|
|
|
}
|
|
|
- else {
|
|
|
- //如果没有找到有效位置，按没有变化处理
|
|
|
- goto no_change;
|
|
|
+
|
|
|
+ //更新根据： m_center_grabed_record更新m_stem_status中的叶子遮挡
|
|
|
+ for (int i = 0; i < m_stem_status.size(); ++i) {
|
|
|
+ if (m_center_grabed_record.at(i) == 0 && m_stem_status.at(i) == 2) {
|
|
|
+ m_stem_status.at(i) = 0;
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
-no_change:
|
|
|
- //没有改变,用上一次的结果
|
|
|
- xstatus.assign(m_center_x.size(), true);
|
|
|
- //update m_history_status
|
|
|
+ for (int i = 0; i < m_stem_status.size(); ++i) {
|
|
|
+ stem_status.push_back(m_stem_status.at(i));
|
|
|
+ }
|
|
|
+
|
|
|
+ //更新m_history_status
|
|
|
if (m_record_cursor == m_global_cursor) {
|
|
|
for (int i = 0; i < m_history_status.cols; ++i) {
|
|
|
- m_history_status.at<unsigned char>(m_record_cursor, i) =
|
|
|
- m_history_status.at<unsigned char>(m_record_cursor - 1, i);
|
|
|
- if (m_history_status.at<unsigned char>(m_record_cursor - 1, i) == 0) {
|
|
|
- xstatus.at(i) = false;
|
|
|
- }
|
|
|
+ m_history_status.at<unsigned char>(m_record_cursor, i) = m_stem_status.at(i);
|
|
|
}
|
|
|
}
|
|
|
else{
|
|
|
@@ -434,19 +624,12 @@ no_change:
|
|
|
for (int col = 0; col < m_history_status.cols; ++col) {
|
|
|
m_history_status.at<float>(row, col) = m_history_status.at<float>(row + 1, col);
|
|
|
}
|
|
|
- }
|
|
|
- /*memcpy_s(m_history_status.data,
|
|
|
- m_history_status.step[0] * (m_max_size - 1),
|
|
|
- m_history_status.data + m_history_status.step[0],
|
|
|
- m_history_status.step[0] * (m_max_size - 1));*/
|
|
|
+ }
|
|
|
for (int i = 0; i < m_history_status.cols; ++i) {
|
|
|
- m_history_status.at<unsigned char>(m_max_size - 1, i) =
|
|
|
- m_history_status.at<unsigned char>(m_max_size - 2, i);
|
|
|
- if (m_history_status.at<unsigned char>(m_max_size - 2, i) == 0) {
|
|
|
- xstatus.at(i) = false;
|
|
|
- }
|
|
|
+ m_history_status.at<unsigned char>(m_max_size - 1, i) = m_stem_status.at(i);
|
|
|
}
|
|
|
- }
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
+
|
|
|
}
|
chenhongjiang