|
|
@@ -4,13 +4,27 @@
|
|
|
|
|
|
|
|
|
namespace graft_cv {
|
|
|
- CStemResultInfos::CStemResultInfos(double seedling_dist,std::string pcd_id, CGcvLogger*pLog)
|
|
|
+ CStemResultInfos::CStemResultInfos(double seedling_dist,
|
|
|
+ int holes_number,
|
|
|
+ double x_min,
|
|
|
+ double x_max,
|
|
|
+ double z_min,
|
|
|
+ double z_max,
|
|
|
+ std::string pcd_id,
|
|
|
+ CGcvLogger*pLog)
|
|
|
: m_pLogger(pLog)
|
|
|
, m_seedling_dist(seedling_dist)
|
|
|
- , m_append_counter(0)
|
|
|
+ , m_holes_number(holes_number)
|
|
|
+ , m_xmin(x_min)
|
|
|
+ , m_xmax(x_max)
|
|
|
+ , m_zmin(z_min)
|
|
|
+ , m_zmax(z_max)
|
|
|
+ //, m_append_counter(0)
|
|
|
, m_pcdId(pcd_id)
|
|
|
- , m_max_size(200)
|
|
|
- {}
|
|
|
+ , m_max_size(50)
|
|
|
+ {
|
|
|
+ gen_root_centers();
|
|
|
+ }
|
|
|
|
|
|
CStemResultInfos::~CStemResultInfos()
|
|
|
{}
|
|
|
@@ -19,17 +33,19 @@ namespace graft_cv {
|
|
|
CStemResult& sr
|
|
|
)
|
|
|
{
|
|
|
+
|
|
|
m_infos.insert(m_infos.begin(), sr);
|
|
|
if (m_infos.size() > m_max_size) {
|
|
|
m_infos.pop_back();
|
|
|
}
|
|
|
- m_append_counter += 1;
|
|
|
-
|
|
|
- if (m_append_counter % 10 == 0) {
|
|
|
- //定期写入数据
|
|
|
- _update_root_centers();
|
|
|
- write_root_centers(m_json_filename);
|
|
|
- }
|
|
|
+ //m_append_counter += 1;
|
|
|
+ //每次都更新
|
|
|
+ _update_root_centers(sr);
|
|
|
+ //if (m_append_counter % 10 == 0) {
|
|
|
+ // //定期写入数据
|
|
|
+ // _update_root_centers();
|
|
|
+ // write_root_centers(m_json_filename);
|
|
|
+ //}
|
|
|
}
|
|
|
void CStemResultInfos::clear()
|
|
|
{
|
|
|
@@ -38,362 +54,402 @@ namespace graft_cv {
|
|
|
void CStemResultInfos::get_root_centers(
|
|
|
std::vector<CStemResult>&rst
|
|
|
)
|
|
|
- {
|
|
|
- //如果存在m_root_centers，就返回m_root_centers
|
|
|
- //否则构建新的
|
|
|
-
|
|
|
- //方法： 通过m_seedling_dist进行分类, 当m_append_counter数量增加10后就进行更新
|
|
|
- if (m_root_centers.size() == 0) {
|
|
|
- //用当前m_infos中的数据生成root_centers数据
|
|
|
- _update_root_centers();
|
|
|
- rst.clear();
|
|
|
- for (auto& sr : m_root_centers) {
|
|
|
- rst.push_back(sr);
|
|
|
- }
|
|
|
- }
|
|
|
- else {
|
|
|
- rst.clear();
|
|
|
- for (auto& sr : m_root_centers) {
|
|
|
- rst.push_back(sr);
|
|
|
- }
|
|
|
-
|
|
|
- }
|
|
|
+ {
|
|
|
+ rst.clear();
|
|
|
+ for (auto& sr : m_root_centers) {
|
|
|
+ rst.push_back(sr);
|
|
|
+ }
|
|
|
}
|
|
|
-
|
|
|
-
|
|
|
- void CStemResultInfos::_update_root_centers() {
|
|
|
- //依据m_infos中的数据，苗间距m_seedling_dist，聚类生成茎的平均位置，茎的点云平均数量
|
|
|
- //更新m_root_centers
|
|
|
-
|
|
|
- //以root_center为基础进行聚类，如果root_center是空的，因当前数据进行聚类
|
|
|
- std::vector<CStemResult> root_centers;
|
|
|
- double d1 = m_seedling_dist / 2.0;
|
|
|
- double d2 = d1*1.75;
|
|
|
- pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2d_pos(new pcl::PointCloud < pcl::PointXYZ>);
|
|
|
- for (auto&sr : m_infos) {
|
|
|
- cloud2d_pos->points.push_back(pcl::PointXYZ((float)sr.root_x, 0.0, (float)sr.root_z));
|
|
|
- }
|
|
|
- cloud2d_pos->width = cloud2d_pos->points.size();
|
|
|
- std::vector<pcl::PointXYZ>cluster_center;
|
|
|
- std::vector<std::vector<int>> cluster_member;
|
|
|
- euclidean_clustering_ttsas(cloud2d_pos, d1, d2, cluster_center, cluster_member);
|
|
|
-
|
|
|
- for (size_t i = 0; i < cluster_center.size(); ++i) {
|
|
|
- double x = (double)cluster_center.at(i).x;
|
|
|
- double y = (double)cluster_center.at(i).y;
|
|
|
- double z = (double)cluster_center.at(i).z;
|
|
|
- int count = (int)cluster_member.at(i).size();
|
|
|
- int pc_size = 0.0;
|
|
|
- for (auto& idx : cluster_member.at(i)) {
|
|
|
- pc_size += m_infos.at(idx).stem_size;
|
|
|
+ void CStemResultInfos::_update_root_centers(CStemResult& sr) {
|
|
|
+ //直接在m_root_centers中找到最接近的中心，如果小于指定距离，更新m_root_centers
|
|
|
+ double d1 = m_seedling_dist / 4.0;
|
|
|
+
|
|
|
+ double d_min = 1.0e6;
|
|
|
+ int min_root_idx = -1;
|
|
|
+ for (int i = 0; i < m_root_centers.size(); ++i) {
|
|
|
+ double dist = m_root_centers.at(i).calcluate_distance(sr);
|
|
|
+ if (dist < d_min) {
|
|
|
+ d_min = dist;
|
|
|
+ min_root_idx = i;
|
|
|
}
|
|
|
- pc_size = (int)(pc_size / (float)count);
|
|
|
-
|
|
|
- CStemResult sr = CStemResult(x, y, z, pc_size, std::string(""), count);
|
|
|
- root_centers.push_back(sr);
|
|
|
- }
|
|
|
-
|
|
|
- if (m_root_centers.size() == 0) {
|
|
|
- //如果没有历史数据，直接赋值
|
|
|
- m_root_centers = root_centers;
|
|
|
}
|
|
|
- else {
|
|
|
- if (m_infos.size() > 50) {
|
|
|
- //如果当前数据量较大，用当前的结果
|
|
|
- m_root_centers = root_centers;
|
|
|
+ if (d_min < d1 && min_root_idx >= 0) {
|
|
|
+ //更新指定中心
|
|
|
+ double mu_x, mu_y, mu_z;
|
|
|
+ CStemResult& min_root = m_root_centers.at(min_root_idx);
|
|
|
+
|
|
|
+ mu_x = min_root.root_x;
|
|
|
+ if (min_root.root_count == 0) {
|
|
|
+ mu_y = sr.root_y;
|
|
|
+ mu_z = sr.root_z;
|
|
|
}
|
|
|
else {
|
|
|
- //否则将当前结果合并到m_root_centers
|
|
|
- //m_root_centers
|
|
|
- for (auto& cent : root_centers) {
|
|
|
- double dist_min = d2;
|
|
|
- int min_root_idx = 0;
|
|
|
-
|
|
|
- for (int j = 0; j < m_root_centers.size();++j) {
|
|
|
- CStemResult& root = m_root_centers.at(j);
|
|
|
- double dist = root.calcluate_distance(cent);
|
|
|
- if (dist < dist_min) {
|
|
|
- dist_min = dist;
|
|
|
- min_root_idx = j;
|
|
|
- }
|
|
|
- }
|
|
|
- if (dist_min < d1) {
|
|
|
- //merge
|
|
|
- double mu_x, mu_y,mu_z;
|
|
|
- CStemResult& min_root = m_root_centers.at(min_root_idx);
|
|
|
- mu_x = (min_root.root_x * min_root.root_count +
|
|
|
- cent.root_x * cent.root_count) / (double)(min_root.root_count + cent.root_count);
|
|
|
-
|
|
|
- mu_y = (min_root.root_y * min_root.root_count +
|
|
|
- cent.root_y * cent.root_count) / (double)(min_root.root_count + cent.root_count);
|
|
|
-
|
|
|
- mu_z = (min_root.root_z * min_root.root_count +
|
|
|
- cent.root_z * cent.root_count) / (double)(min_root.root_count + cent.root_count);
|
|
|
-
|
|
|
- min_root.root_x = mu_x;
|
|
|
- min_root.root_y = mu_y;
|
|
|
- min_root.root_z = mu_z;
|
|
|
- min_root.root_count += cent.root_count;
|
|
|
- }
|
|
|
- else {
|
|
|
- //add root
|
|
|
- m_root_centers.push_back(cent);
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
+ mu_y = (min_root.root_y * min_root.root_count +
|
|
|
+ sr.root_y * sr.root_count) / (double)(min_root.root_count + sr.root_count);
|
|
|
+ mu_z = (min_root.root_z * min_root.root_count +
|
|
|
+ sr.root_z * sr.root_count) / (double)(min_root.root_count + sr.root_count);
|
|
|
+
|
|
|
+ }
|
|
|
+ min_root.root_x = mu_x;
|
|
|
+ min_root.root_y = mu_y;
|
|
|
+ min_root.root_z = mu_z;
|
|
|
+ min_root.root_count += sr.root_count;
|
|
|
}
|
|
|
- //m_root_centers 排序、剔除
|
|
|
- std::sort(m_root_centers.begin(), m_root_centers.end(),
|
|
|
- [](const CStemResult& sr1, const CStemResult& sr2) {return sr1.root_x < sr2.root_x; });
|
|
|
- //nms
|
|
|
- _filter_root_centers(d1, d2);
|
|
|
}
|
|
|
- void CStemResultInfos::_filter_root_centers(double d1, double d2)
|
|
|
- {
|
|
|
- //对生成的根中心m_root_centers进行过滤，剔除异常
|
|
|
- //1 z值，通过中值，距离中值远的剔除
|
|
|
-
|
|
|
- if (m_root_centers.size() > 3) {
|
|
|
- std::vector<double>root_z;
|
|
|
- for (auto&rc : m_root_centers) {
|
|
|
- root_z.push_back(rc.root_z);
|
|
|
- }
|
|
|
- std::sort(root_z.begin(), root_z.end());
|
|
|
- double mid_z = 0.0;
|
|
|
- if (root_z.size() % 2 == 1) {
|
|
|
- int idx = (m_root_centers.size() - 1) / 2;
|
|
|
- mid_z = root_z.at(idx);
|
|
|
- }
|
|
|
- else {
|
|
|
- int idx = root_z.size() / 2;
|
|
|
- mid_z = 0.5 * (root_z.at(idx) + root_z.at(idx - 1));
|
|
|
- }
|
|
|
- std::vector<CStemResult> valid_root_centers;
|
|
|
- for (auto&rc : m_root_centers) {
|
|
|
- if (fabs(rc.root_z - mid_z) < d2) {
|
|
|
- valid_root_centers.push_back(rc);
|
|
|
- }
|
|
|
- }
|
|
|
- m_root_centers = valid_root_centers;
|
|
|
- }
|
|
|
|
|
|
- //2 按x做分组，排除权重小的
|
|
|
- if (m_root_centers.size() > 3) {
|
|
|
- std::vector<double> center_dist;
|
|
|
- for (int i = 0; i < m_root_centers.size(); ++i) {
|
|
|
- double x = m_root_centers.at(i).root_x;
|
|
|
- double mod_sum = 0.0;
|
|
|
- for (int j = 0; j < m_root_centers.size(); ++j) {
|
|
|
- double dist = fabs(m_root_centers.at(j).root_x - x);
|
|
|
- double mod = fmod(dist, m_seedling_dist);
|
|
|
- double mod_inv = m_seedling_dist - mod;
|
|
|
- mod = min(mod, mod_inv);
|
|
|
- mod_sum += mod;
|
|
|
- }
|
|
|
- mod_sum /= (m_root_centers.size() - 1);
|
|
|
- center_dist.push_back(mod_sum);
|
|
|
- }
|
|
|
-
|
|
|
- std::vector<CStemResult> valid_root_centers;
|
|
|
- for (int i = 0; i < center_dist.size(); ++i) {
|
|
|
- double dist = center_dist.at(i);
|
|
|
- if (dist < d1) {
|
|
|
- valid_root_centers.push_back(m_root_centers.at(i));
|
|
|
- }
|
|
|
- }
|
|
|
- m_root_centers = valid_root_centers;
|
|
|
- }
|
|
|
- //3 数量少的剔除
|
|
|
- int total_count = 0;
|
|
|
- for (auto&rc : m_root_centers) {
|
|
|
- total_count += rc.root_count;
|
|
|
- }
|
|
|
- if (total_count > 50) {
|
|
|
- bool need_del = false;
|
|
|
- for (auto& rc : m_root_centers) {
|
|
|
- if (rc.root_count <= 2) {
|
|
|
- need_del = true;
|
|
|
- break;
|
|
|
- }
|
|
|
- }
|
|
|
- if (need_del) {
|
|
|
- std::vector<CStemResult> valid_root_centers;
|
|
|
- for (int i = 0; i < m_root_centers.size(); ++i) {
|
|
|
- if (m_root_centers.at(i).root_count > 2) {
|
|
|
- valid_root_centers.push_back(m_root_centers.at(i));
|
|
|
- }
|
|
|
- }
|
|
|
- m_root_centers = valid_root_centers;
|
|
|
- }
|
|
|
-
|
|
|
- }
|
|
|
-
|
|
|
- }
|
|
|
- void CStemResultInfos::set_json_filename(std::string& filename) {
|
|
|
- m_json_filename = std::string(filename);
|
|
|
- }
|
|
|
- void CStemResultInfos::read_root_centers(
|
|
|
- std::string& filename
|
|
|
- )
|
|
|
+ //void CStemResultInfos::_update_root_centers() {
|
|
|
+ // //依据m_infos中的数据，苗间距m_seedling_dist，聚类生成茎的平均位置，茎的点云平均数量
|
|
|
+ // //更新m_root_centers
|
|
|
+
|
|
|
+ // //以root_center为基础进行聚类，如果root_center是空的，因当前数据进行聚类
|
|
|
+ // std::vector<CStemResult> root_centers;
|
|
|
+ // double d1 = m_seedling_dist / 2.0;
|
|
|
+ // double d2 = d1*1.75;
|
|
|
+ // pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2d_pos(new pcl::PointCloud < pcl::PointXYZ>);
|
|
|
+ // for (auto&sr : m_infos) {
|
|
|
+ // cloud2d_pos->points.push_back(pcl::PointXYZ((float)sr.root_x, 0.0, (float)sr.root_z));
|
|
|
+ // }
|
|
|
+ // cloud2d_pos->width = cloud2d_pos->points.size();
|
|
|
+ // std::vector<pcl::PointXYZ>cluster_center;
|
|
|
+ // std::vector<std::vector<int>> cluster_member;
|
|
|
+ // euclidean_clustering_ttsas(cloud2d_pos, d1, d2, cluster_center, cluster_member);
|
|
|
+
|
|
|
+ // for (size_t i = 0; i < cluster_center.size(); ++i) {
|
|
|
+ // double x = (double)cluster_center.at(i).x;
|
|
|
+ // double y = (double)cluster_center.at(i).y;
|
|
|
+ // double z = (double)cluster_center.at(i).z;
|
|
|
+ // int count = (int)cluster_member.at(i).size();
|
|
|
+ // int pc_size = 0.0;
|
|
|
+ // for (auto& idx : cluster_member.at(i)) {
|
|
|
+ // pc_size += m_infos.at(idx).stem_size;
|
|
|
+ // }
|
|
|
+ // pc_size = (int)(pc_size / (float)count);
|
|
|
+
|
|
|
+ // CStemResult sr = CStemResult(x, y, z, pc_size, std::string(""), count);
|
|
|
+ // root_centers.push_back(sr);
|
|
|
+ // }
|
|
|
+
|
|
|
+ // if (m_root_centers.size() == 0) {
|
|
|
+ // //如果没有历史数据，直接赋值
|
|
|
+ // m_root_centers = root_centers;
|
|
|
+ // }
|
|
|
+ // else {
|
|
|
+ // if (m_infos.size() > 50) {
|
|
|
+ // //如果当前数据量较大，用当前的结果
|
|
|
+ // m_root_centers = root_centers;
|
|
|
+ // }
|
|
|
+ // else {
|
|
|
+ // //否则将当前结果合并到m_root_centers
|
|
|
+ // //m_root_centers
|
|
|
+ // for (auto& cent : root_centers) {
|
|
|
+ // double dist_min = d2;
|
|
|
+ // int min_root_idx = 0;
|
|
|
+
|
|
|
+ // for (int j = 0; j < m_root_centers.size();++j) {
|
|
|
+ // CStemResult& root = m_root_centers.at(j);
|
|
|
+ // double dist = root.calcluate_distance(cent);
|
|
|
+ // if (dist < dist_min) {
|
|
|
+ // dist_min = dist;
|
|
|
+ // min_root_idx = j;
|
|
|
+ // }
|
|
|
+ // }
|
|
|
+ // if (dist_min < d1) {
|
|
|
+ // //merge
|
|
|
+ // double mu_x, mu_y,mu_z;
|
|
|
+ // CStemResult& min_root = m_root_centers.at(min_root_idx);
|
|
|
+ // mu_x = (min_root.root_x * min_root.root_count +
|
|
|
+ // cent.root_x * cent.root_count) / (double)(min_root.root_count + cent.root_count);
|
|
|
+
|
|
|
+ // mu_y = (min_root.root_y * min_root.root_count +
|
|
|
+ // cent.root_y * cent.root_count) / (double)(min_root.root_count + cent.root_count);
|
|
|
+
|
|
|
+ // mu_z = (min_root.root_z * min_root.root_count +
|
|
|
+ // cent.root_z * cent.root_count) / (double)(min_root.root_count + cent.root_count);
|
|
|
+
|
|
|
+ // min_root.root_x = mu_x;
|
|
|
+ // min_root.root_y = mu_y;
|
|
|
+ // min_root.root_z = mu_z;
|
|
|
+ // min_root.root_count += cent.root_count;
|
|
|
+ // }
|
|
|
+ // else {
|
|
|
+ // //add root
|
|
|
+ // m_root_centers.push_back(cent);
|
|
|
+ // }
|
|
|
+ // }
|
|
|
+ // }
|
|
|
+ // }
|
|
|
+ // //m_root_centers 排序、剔除
|
|
|
+ // std::sort(m_root_centers.begin(), m_root_centers.end(),
|
|
|
+ // [](const CStemResult& sr1, const CStemResult& sr2) {return sr1.root_x < sr2.root_x; });
|
|
|
+ // //nms
|
|
|
+ // _filter_root_centers(d1, d2);
|
|
|
+ //}
|
|
|
+ //void CStemResultInfos::_filter_root_centers(double d1, double d2)
|
|
|
+ //{
|
|
|
+ // //对生成的根中心m_root_centers进行过滤，剔除异常
|
|
|
+ // //1 z值，通过中值，距离中值远的剔除
|
|
|
+
|
|
|
+ // if (m_root_centers.size() > 3) {
|
|
|
+ // std::vector<double>root_z;
|
|
|
+ // for (auto&rc : m_root_centers) {
|
|
|
+ // root_z.push_back(rc.root_z);
|
|
|
+ // }
|
|
|
+ // std::sort(root_z.begin(), root_z.end());
|
|
|
+ // double mid_z = 0.0;
|
|
|
+ // if (root_z.size() % 2 == 1) {
|
|
|
+ // int idx = (m_root_centers.size() - 1) / 2;
|
|
|
+ // mid_z = root_z.at(idx);
|
|
|
+ // }
|
|
|
+ // else {
|
|
|
+ // int idx = root_z.size() / 2;
|
|
|
+ // mid_z = 0.5 * (root_z.at(idx) + root_z.at(idx - 1));
|
|
|
+ // }
|
|
|
+ // std::vector<CStemResult> valid_root_centers;
|
|
|
+ // for (auto&rc : m_root_centers) {
|
|
|
+ // if (fabs(rc.root_z - mid_z) < d2) {
|
|
|
+ // valid_root_centers.push_back(rc);
|
|
|
+ // }
|
|
|
+ // }
|
|
|
+ // m_root_centers = valid_root_centers;
|
|
|
+ // }
|
|
|
+
|
|
|
+ // //2 按x做分组，排除权重小的
|
|
|
+ // if (m_root_centers.size() > 3) {
|
|
|
+ // std::vector<double> center_dist;
|
|
|
+ // for (int i = 0; i < m_root_centers.size(); ++i) {
|
|
|
+ // double x = m_root_centers.at(i).root_x;
|
|
|
+ // double mod_sum = 0.0;
|
|
|
+ // for (int j = 0; j < m_root_centers.size(); ++j) {
|
|
|
+ // double dist = fabs(m_root_centers.at(j).root_x - x);
|
|
|
+ // double mod = fmod(dist, m_seedling_dist);
|
|
|
+ // double mod_inv = m_seedling_dist - mod;
|
|
|
+ // mod = min(mod, mod_inv);
|
|
|
+ // mod_sum += mod;
|
|
|
+ // }
|
|
|
+ // mod_sum /= (m_root_centers.size() - 1);
|
|
|
+ // center_dist.push_back(mod_sum);
|
|
|
+ // }
|
|
|
+
|
|
|
+ // std::vector<CStemResult> valid_root_centers;
|
|
|
+ // for (int i = 0; i < center_dist.size(); ++i) {
|
|
|
+ // double dist = center_dist.at(i);
|
|
|
+ // if (dist < d1) {
|
|
|
+ // valid_root_centers.push_back(m_root_centers.at(i));
|
|
|
+ // }
|
|
|
+ // }
|
|
|
+ // m_root_centers = valid_root_centers;
|
|
|
+ // }
|
|
|
+ // //3 数量少的剔除
|
|
|
+ // int total_count = 0;
|
|
|
+ // for (auto&rc : m_root_centers) {
|
|
|
+ // total_count += rc.root_count;
|
|
|
+ // }
|
|
|
+ // if (total_count > 50) {
|
|
|
+ // bool need_del = false;
|
|
|
+ // for (auto& rc : m_root_centers) {
|
|
|
+ // if (rc.root_count <= 2) {
|
|
|
+ // need_del = true;
|
|
|
+ // break;
|
|
|
+ // }
|
|
|
+ // }
|
|
|
+ // if (need_del) {
|
|
|
+ // std::vector<CStemResult> valid_root_centers;
|
|
|
+ // for (int i = 0; i < m_root_centers.size(); ++i) {
|
|
|
+ // if (m_root_centers.at(i).root_count > 2) {
|
|
|
+ // valid_root_centers.push_back(m_root_centers.at(i));
|
|
|
+ // }
|
|
|
+ // }
|
|
|
+ // m_root_centers = valid_root_centers;
|
|
|
+ // }
|
|
|
+ //
|
|
|
+ // }
|
|
|
+ //}
|
|
|
+ void CStemResultInfos::gen_root_centers()
|
|
|
{
|
|
|
- //读取历史数据，每次启动时，去取
|
|
|
- m_json_filename = std::string(filename);
|
|
|
- //1 文件是否存在
|
|
|
- ifstream f(filename.c_str());
|
|
|
- if (!f.good()) {
|
|
|
- //文件不存在
|
|
|
- if (m_pLogger) {
|
|
|
- stringstream buff;
|
|
|
- buff << m_pcdId << ": json file not exists:" << filename;
|
|
|
- m_pLogger->INFO(buff.str());
|
|
|
- }
|
|
|
- return;
|
|
|
- }
|
|
|
-
|
|
|
- cv::FileStorage fs(filename, cv::FileStorage::READ);
|
|
|
- cv::FileNode root_centers = fs["root_centers"];
|
|
|
- cv::FileNodeIterator it = root_centers.begin(), it_end = root_centers.end();
|
|
|
+ //根据 m_seedling_dist, m_holes_number， m_xmin， m_xmax生成初始的穴位中心
|
|
|
+ //以m_xmin， m_xmax的中间点为中心，分别找到间隔m_seedling_dist的m_holes_number个穴位中心
|
|
|
+ //初始的z设成-1，等待更新赋值
|
|
|
+ double x_mid = 0.5 * (m_xmin + m_xmax);
|
|
|
+ double holes_mid = 0.5 * (m_holes_number - 1) * m_seedling_dist;
|
|
|
+ double x0 = x_mid - holes_mid;
|
|
|
+ double z_mid = 0.5 * (m_zmin + m_zmax);
|
|
|
m_root_centers.clear();
|
|
|
- for (; it != it_end; ++it) {
|
|
|
- double x = (double)(*it)["x"];
|
|
|
- double y = (double)(*it)["y"];
|
|
|
- double z = (double)(*it)["z"];
|
|
|
- int size = (int)(*it)["size"];
|
|
|
- std::string bid = (std::string)(*it)["batch_id"];
|
|
|
- int count = (int)(*it)["count"];
|
|
|
- CStemResult sr = CStemResult(x,y,z,size, bid, count);
|
|
|
+ for (int i=0; i<m_holes_number; ++i) {
|
|
|
+ double x = x0 + i * m_seedling_dist;
|
|
|
+ double y = 0;
|
|
|
+ double z = z_mid;
|
|
|
+ int size = 0;
|
|
|
+ int count = 0;
|
|
|
+ CStemResult sr = CStemResult(x, y, z, size, std::string(""), count);
|
|
|
m_root_centers.push_back(sr);
|
|
|
}
|
|
|
- fs.release();
|
|
|
-
|
|
|
- }
|
|
|
- void CStemResultInfos::write_root_centers(
|
|
|
- std::string& filename
|
|
|
- )
|
|
|
- {
|
|
|
- cv::FileStorage fs(filename, cv::FileStorage::WRITE);
|
|
|
- fs << "root_centers" << "[";
|
|
|
- for (auto& sr : m_root_centers) {
|
|
|
- fs << "{" << "x" << sr.root_x
|
|
|
- << "y" << sr.root_y
|
|
|
- << "z" << sr.root_z
|
|
|
- << "size" << sr.stem_size
|
|
|
- << "batch_id" << sr.batch_id
|
|
|
- << "count" << sr.root_count
|
|
|
- << "}";
|
|
|
- }
|
|
|
- fs << "]";
|
|
|
- fs.release();
|
|
|
-
|
|
|
}
|
|
|
|
|
|
- void CStemResultInfos::euclidean_clustering_ttsas(
|
|
|
- pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
|
|
|
- double d1, double d2,
|
|
|
- std::vector<pcl::PointXYZ>&cluster_center,
|
|
|
- std::vector<std::vector<int>> &clustr_member
|
|
|
- )
|
|
|
- {
|
|
|
- if (m_pLogger) {
|
|
|
- stringstream buff;
|
|
|
- buff << m_pcdId << ": root center estimate: euclidean_clustering_ttsas() begin...";
|
|
|
- m_pLogger->INFO(buff.str());
|
|
|
- }
|
|
|
- std::vector<int> cluster_weight;
|
|
|
- std::vector<int> data_stat;
|
|
|
-
|
|
|
- std::vector<pcl::PointXYZ>cluster_center_raw;
|
|
|
- std::vector<std::vector<int>> clustr_member_raw;
|
|
|
-
|
|
|
- for (size_t i = 0; i < in_cloud->points.size(); ++i) { data_stat.push_back(0); }
|
|
|
- size_t data_len = in_cloud->points.size();
|
|
|
- int exists_change = 0;
|
|
|
- int prev_change = 0;
|
|
|
- int cur_change = 0;
|
|
|
- int m = 0;
|
|
|
- while (std::find(data_stat.begin(), data_stat.end(), 0) != data_stat.end()) {
|
|
|
- bool new_while_first = true;
|
|
|
- for (size_t i = 0; i < data_len; ++i) {
|
|
|
- if (data_stat.at(i) == 0 && new_while_first && exists_change == 0) {
|
|
|
- new_while_first = false;
|
|
|
- std::vector<int> idx;
|
|
|
- idx.push_back(i);
|
|
|
- clustr_member_raw.push_back(idx);
|
|
|
- pcl::PointXYZ center;
|
|
|
- center.x = in_cloud->points.at(i).x;
|
|
|
- center.y = in_cloud->points.at(i).y;
|
|
|
- center.z = in_cloud->points.at(i).z;
|
|
|
-
|
|
|
- cluster_center_raw.push_back(center);
|
|
|
- data_stat.at(i) = 1;
|
|
|
- cur_change += 1;
|
|
|
- cluster_weight.push_back(1);
|
|
|
- m += 1;
|
|
|
- }
|
|
|
- else if (data_stat[i] == 0) {
|
|
|
- std::vector<float> distances;
|
|
|
- for (size_t j = 0; j < clustr_member_raw.size(); ++j) {
|
|
|
- std::vector<float> distances_sub;
|
|
|
- for (size_t jj = 0; jj < clustr_member_raw.at(j).size(); ++jj) {
|
|
|
- size_t ele_idx = clustr_member_raw.at(j).at(jj);
|
|
|
- double d = sqrt(
|
|
|
- (in_cloud->points.at(i).x - in_cloud->points.at(ele_idx).x) * (in_cloud->points.at(i).x - in_cloud->points.at(ele_idx).x) +
|
|
|
- (in_cloud->points.at(i).y - in_cloud->points.at(ele_idx).y) * (in_cloud->points.at(i).y - in_cloud->points.at(ele_idx).y) +
|
|
|
- (in_cloud->points.at(i).z - in_cloud->points.at(ele_idx).z) * (in_cloud->points.at(i).z - in_cloud->points.at(ele_idx).z));
|
|
|
- distances_sub.push_back(d);
|
|
|
- }
|
|
|
- double min_dist = *std::min_element(distances_sub.begin(), distances_sub.end());
|
|
|
- distances.push_back(min_dist);
|
|
|
- }
|
|
|
- int min_idx = std::min_element(distances.begin(), distances.end()) - distances.begin();
|
|
|
- if (distances.at(min_idx) < d1) {
|
|
|
- data_stat.at(i) = 1;
|
|
|
- double w = cluster_weight.at(min_idx);
|
|
|
- cluster_weight.at(min_idx) += 1;
|
|
|
- clustr_member_raw.at(min_idx).push_back(i);
|
|
|
- cluster_center_raw.at(min_idx).x = (cluster_center_raw.at(min_idx).x * w + in_cloud->points.at(i).x) / (w + 1);
|
|
|
- cluster_center_raw.at(min_idx).y = (cluster_center_raw.at(min_idx).y * w + in_cloud->points.at(i).y) / (w + 1);
|
|
|
- cluster_center_raw.at(min_idx).z = (cluster_center_raw.at(min_idx).z * w + in_cloud->points.at(i).z) / (w + 1);
|
|
|
- cur_change += 1;
|
|
|
- }
|
|
|
- else if (distances.at(min_idx) > d2) {
|
|
|
- std::vector<int> idx;
|
|
|
- idx.push_back(i);
|
|
|
- clustr_member_raw.push_back(idx);
|
|
|
- pcl::PointXYZ center;
|
|
|
- center.x = in_cloud->points.at(i).x;
|
|
|
- center.y = in_cloud->points.at(i).y;
|
|
|
- center.z = in_cloud->points.at(i).z;
|
|
|
-
|
|
|
- cluster_center_raw.push_back(center);
|
|
|
- data_stat.at(i) = 1;
|
|
|
- cur_change += 1;
|
|
|
- cluster_weight.push_back(1);
|
|
|
- m += 1;
|
|
|
- }
|
|
|
-
|
|
|
- }
|
|
|
- else if (data_stat.at(i) == 1) {
|
|
|
- cur_change += 1;
|
|
|
- }
|
|
|
- }
|
|
|
- exists_change = fabs(cur_change - prev_change);
|
|
|
- prev_change = cur_change;
|
|
|
- cur_change = 0;
|
|
|
- }
|
|
|
-
|
|
|
- // copy result
|
|
|
- for (size_t i = 0; i < clustr_member_raw.size(); ++i) {
|
|
|
- //if (clustr_member_raw.at(i).size() < 20) { continue; }
|
|
|
- cluster_center.push_back(cluster_center_raw.at(i));
|
|
|
- clustr_member.push_back(clustr_member_raw.at(i));
|
|
|
- }
|
|
|
- if (m_pLogger) {
|
|
|
- stringstream buff;
|
|
|
- buff << m_pcdId << ": root center estimate: euclidean_clustering_ttsas() end";
|
|
|
- m_pLogger->INFO(buff.str());
|
|
|
- }
|
|
|
- }
|
|
|
+ //void CStemResultInfos::set_json_filename(std::string& filename) {
|
|
|
+ // m_json_filename = std::string(filename);
|
|
|
+ //}
|
|
|
+ //void CStemResultInfos::read_root_centers(
|
|
|
+ // std::string& filename
|
|
|
+ //)
|
|
|
+ //{
|
|
|
+ // //读取历史数据，每次启动时，去取
|
|
|
+ // m_json_filename = std::string(filename);
|
|
|
+ // //1 文件是否存在
|
|
|
+ // ifstream f(filename.c_str());
|
|
|
+ // if (!f.good()) {
|
|
|
+ // //文件不存在
|
|
|
+ // if (m_pLogger) {
|
|
|
+ // stringstream buff;
|
|
|
+ // buff << m_pcdId << ": json file not exists:" << filename;
|
|
|
+ // m_pLogger->INFO(buff.str());
|
|
|
+ // }
|
|
|
+ // return;
|
|
|
+ // }
|
|
|
+
|
|
|
+ // cv::FileStorage fs(filename, cv::FileStorage::READ);
|
|
|
+ // cv::FileNode root_centers = fs["root_centers"];
|
|
|
+ // cv::FileNodeIterator it = root_centers.begin(), it_end = root_centers.end();
|
|
|
+ // m_root_centers.clear();
|
|
|
+ // for (; it != it_end; ++it) {
|
|
|
+ // double x = (double)(*it)["x"];
|
|
|
+ // double y = (double)(*it)["y"];
|
|
|
+ // double z = (double)(*it)["z"];
|
|
|
+ // int size = (int)(*it)["size"];
|
|
|
+ // std::string bid = (std::string)(*it)["batch_id"];
|
|
|
+ // int count = (int)(*it)["count"];
|
|
|
+ // CStemResult sr = CStemResult(x,y,z,size, bid, count);
|
|
|
+ // m_root_centers.push_back(sr);
|
|
|
+ // }
|
|
|
+ // fs.release();
|
|
|
+
|
|
|
+ //}
|
|
|
+ //void CStemResultInfos::write_root_centers(
|
|
|
+ // std::string& filename
|
|
|
+ //)
|
|
|
+ //{
|
|
|
+ // cv::FileStorage fs(filename, cv::FileStorage::WRITE);
|
|
|
+ // fs << "root_centers" << "[";
|
|
|
+ // for (auto& sr : m_root_centers) {
|
|
|
+ // fs << "{" << "x" << sr.root_x
|
|
|
+ // << "y" << sr.root_y
|
|
|
+ // << "z" << sr.root_z
|
|
|
+ // << "size" << sr.stem_size
|
|
|
+ // << "batch_id" << sr.batch_id
|
|
|
+ // << "count" << sr.root_count
|
|
|
+ // << "}";
|
|
|
+ // }
|
|
|
+ // fs << "]";
|
|
|
+ // fs.release();
|
|
|
+
|
|
|
+ //}
|
|
|
+
|
|
|
+ //void CStemResultInfos::euclidean_clustering_ttsas(
|
|
|
+ // pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
|
|
|
+ // double d1, double d2,
|
|
|
+ // std::vector<pcl::PointXYZ>&cluster_center,
|
|
|
+ // std::vector<std::vector<int>> &clustr_member
|
|
|
+ //)
|
|
|
+ //{
|
|
|
+ // if (m_pLogger) {
|
|
|
+ // stringstream buff;
|
|
|
+ // buff << m_pcdId << ": root center estimate: euclidean_clustering_ttsas() begin...";
|
|
|
+ // m_pLogger->INFO(buff.str());
|
|
|
+ // }
|
|
|
+ // std::vector<int> cluster_weight;
|
|
|
+ // std::vector<int> data_stat;
|
|
|
+
|
|
|
+ // std::vector<pcl::PointXYZ>cluster_center_raw;
|
|
|
+ // std::vector<std::vector<int>> clustr_member_raw;
|
|
|
+
|
|
|
+ // for (size_t i = 0; i < in_cloud->points.size(); ++i) { data_stat.push_back(0); }
|
|
|
+ // size_t data_len = in_cloud->points.size();
|
|
|
+ // int exists_change = 0;
|
|
|
+ // int prev_change = 0;
|
|
|
+ // int cur_change = 0;
|
|
|
+ // int m = 0;
|
|
|
+ // while (std::find(data_stat.begin(), data_stat.end(), 0) != data_stat.end()) {
|
|
|
+ // bool new_while_first = true;
|
|
|
+ // for (size_t i = 0; i < data_len; ++i) {
|
|
|
+ // if (data_stat.at(i) == 0 && new_while_first && exists_change == 0) {
|
|
|
+ // new_while_first = false;
|
|
|
+ // std::vector<int> idx;
|
|
|
+ // idx.push_back(i);
|
|
|
+ // clustr_member_raw.push_back(idx);
|
|
|
+ // pcl::PointXYZ center;
|
|
|
+ // center.x = in_cloud->points.at(i).x;
|
|
|
+ // center.y = in_cloud->points.at(i).y;
|
|
|
+ // center.z = in_cloud->points.at(i).z;
|
|
|
+
|
|
|
+ // cluster_center_raw.push_back(center);
|
|
|
+ // data_stat.at(i) = 1;
|
|
|
+ // cur_change += 1;
|
|
|
+ // cluster_weight.push_back(1);
|
|
|
+ // m += 1;
|
|
|
+ // }
|
|
|
+ // else if (data_stat[i] == 0) {
|
|
|
+ // std::vector<float> distances;
|
|
|
+ // for (size_t j = 0; j < clustr_member_raw.size(); ++j) {
|
|
|
+ // std::vector<float> distances_sub;
|
|
|
+ // for (size_t jj = 0; jj < clustr_member_raw.at(j).size(); ++jj) {
|
|
|
+ // size_t ele_idx = clustr_member_raw.at(j).at(jj);
|
|
|
+ // double d = sqrt(
|
|
|
+ // (in_cloud->points.at(i).x - in_cloud->points.at(ele_idx).x) * (in_cloud->points.at(i).x - in_cloud->points.at(ele_idx).x) +
|
|
|
+ // (in_cloud->points.at(i).y - in_cloud->points.at(ele_idx).y) * (in_cloud->points.at(i).y - in_cloud->points.at(ele_idx).y) +
|
|
|
+ // (in_cloud->points.at(i).z - in_cloud->points.at(ele_idx).z) * (in_cloud->points.at(i).z - in_cloud->points.at(ele_idx).z));
|
|
|
+ // distances_sub.push_back(d);
|
|
|
+ // }
|
|
|
+ // double min_dist = *std::min_element(distances_sub.begin(), distances_sub.end());
|
|
|
+ // distances.push_back(min_dist);
|
|
|
+ // }
|
|
|
+ // int min_idx = std::min_element(distances.begin(), distances.end()) - distances.begin();
|
|
|
+ // if (distances.at(min_idx) < d1) {
|
|
|
+ // data_stat.at(i) = 1;
|
|
|
+ // double w = cluster_weight.at(min_idx);
|
|
|
+ // cluster_weight.at(min_idx) += 1;
|
|
|
+ // clustr_member_raw.at(min_idx).push_back(i);
|
|
|
+ // cluster_center_raw.at(min_idx).x = (cluster_center_raw.at(min_idx).x * w + in_cloud->points.at(i).x) / (w + 1);
|
|
|
+ // cluster_center_raw.at(min_idx).y = (cluster_center_raw.at(min_idx).y * w + in_cloud->points.at(i).y) / (w + 1);
|
|
|
+ // cluster_center_raw.at(min_idx).z = (cluster_center_raw.at(min_idx).z * w + in_cloud->points.at(i).z) / (w + 1);
|
|
|
+ // cur_change += 1;
|
|
|
+ // }
|
|
|
+ // else if (distances.at(min_idx) > d2) {
|
|
|
+ // std::vector<int> idx;
|
|
|
+ // idx.push_back(i);
|
|
|
+ // clustr_member_raw.push_back(idx);
|
|
|
+ // pcl::PointXYZ center;
|
|
|
+ // center.x = in_cloud->points.at(i).x;
|
|
|
+ // center.y = in_cloud->points.at(i).y;
|
|
|
+ // center.z = in_cloud->points.at(i).z;
|
|
|
+
|
|
|
+ // cluster_center_raw.push_back(center);
|
|
|
+ // data_stat.at(i) = 1;
|
|
|
+ // cur_change += 1;
|
|
|
+ // cluster_weight.push_back(1);
|
|
|
+ // m += 1;
|
|
|
+ // }
|
|
|
+
|
|
|
+ // }
|
|
|
+ // else if (data_stat.at(i) == 1) {
|
|
|
+ // cur_change += 1;
|
|
|
+ // }
|
|
|
+ // }
|
|
|
+ // exists_change = fabs(cur_change - prev_change);
|
|
|
+ // prev_change = cur_change;
|
|
|
+ // cur_change = 0;
|
|
|
+ // }
|
|
|
+
|
|
|
+ // // copy result
|
|
|
+ // for (size_t i = 0; i < clustr_member_raw.size(); ++i) {
|
|
|
+ // //if (clustr_member_raw.at(i).size() < 20) { continue; }
|
|
|
+ // cluster_center.push_back(cluster_center_raw.at(i));
|
|
|
+ // clustr_member.push_back(clustr_member_raw.at(i));
|
|
|
+ // }
|
|
|
+ // if (m_pLogger) {
|
|
|
+ // stringstream buff;
|
|
|
+ // buff << m_pcdId << ": root center estimate: euclidean_clustering_ttsas() end";
|
|
|
+ // m_pLogger->INFO(buff.str());
|
|
|
+ // }
|
|
|
+ //}
|
|
|
|
|
|
|
|
|
|
chenhongjiang