Improved group merging.
This commit is contained in:
Daniel
+118
-8
@@ -1220,8 +1220,7 @@ std::vector<std::tuple<cv::Point3f, cv::Mat, std::vector<int>>> TriangulatorInte
|
||||
cv::Mat &group_pose = std::get<1>(group);
|
||||
|
||||
// Calculate average joint distance
|
||||
float dist_sum = 0.0;
|
||||
size_t count = 0;
|
||||
std::vector<float> dists;
|
||||
for (size_t row = 0; row < num_joints; ++row)
|
||||
{
|
||||
const float *pose_3d_ptr = pose_3d.ptr<float>(row);
|
||||
@@ -1236,15 +1235,20 @@ std::vector<std::tuple<cv::Point3f, cv::Mat, std::vector<int>>> TriangulatorInte
|
||||
float dy = pose_3d_ptr[1] - group_pose_ptr[1];
|
||||
float dz = pose_3d_ptr[2] - group_pose_ptr[2];
|
||||
float dist_sq = dx * dx + dy * dy + dz * dz;
|
||||
dist_sum += std::sqrt(dist_sq);
|
||||
count++;
|
||||
dists.push_back(std::sqrt(dist_sq));
|
||||
}
|
||||
}
|
||||
|
||||
if (count > 0)
|
||||
if (dists.size() >= 5)
|
||||
{
|
||||
// Drop highest value to reduce influence of outliers
|
||||
auto max_it = std::max_element(dists.begin(), dists.end());
|
||||
dists.erase(max_it);
|
||||
}
|
||||
if (dists.size() > 0)
|
||||
{
|
||||
// Check if the average joint distance is close enough
|
||||
float avg_dist = dist_sum / count;
|
||||
float avg_dist = std::accumulate(dists.begin(), dists.end(), 0.0);
|
||||
avg_dist /= static_cast<float>(dists.size());
|
||||
if (avg_dist < max_joint_avg_dist && avg_dist < best_dist)
|
||||
{
|
||||
best_dist = avg_dist;
|
||||
@@ -1310,7 +1314,113 @@ std::vector<std::tuple<cv::Point3f, cv::Mat, std::vector<int>>> TriangulatorInte
|
||||
}
|
||||
}
|
||||
|
||||
return groups;
|
||||
// Merge close groups
|
||||
// Depending on the inital group creation, one or more groups can be created that in the end
|
||||
// share the same persons, even if they had a larger distance at the beginning
|
||||
// So merge them similar to the group assignment before
|
||||
std::vector<std::tuple<cv::Point3f, cv::Mat, std::vector<int>>> merged_groups;
|
||||
for (size_t i = 0; i < groups.size(); ++i)
|
||||
{
|
||||
size_t num_joints = std::get<1>(groups[i]).rows;
|
||||
auto &group = groups[i];
|
||||
auto &group_visible_counts = per_group_visible_counts[i];
|
||||
|
||||
float best_dist = std::numeric_limits<float>::infinity();
|
||||
int best_group = -1;
|
||||
|
||||
for (size_t j = 0; j < merged_groups.size(); ++j)
|
||||
{
|
||||
auto &merged_group = merged_groups[j];
|
||||
|
||||
// Calculate average joint distance
|
||||
std::vector<float> dists;
|
||||
for (size_t row = 0; row < num_joints; ++row)
|
||||
{
|
||||
const float *group_pose_ptr = std::get<1>(group).ptr<float>(row);
|
||||
const float *merged_pose_ptr = std::get<1>(merged_group).ptr<float>(row);
|
||||
|
||||
float score1 = group_pose_ptr[3];
|
||||
float score2 = merged_pose_ptr[3];
|
||||
|
||||
if (score1 > min_score && score2 > min_score)
|
||||
{
|
||||
float dx = group_pose_ptr[0] - merged_pose_ptr[0];
|
||||
float dy = group_pose_ptr[1] - merged_pose_ptr[1];
|
||||
float dz = group_pose_ptr[2] - merged_pose_ptr[2];
|
||||
float dist_sq = dx * dx + dy * dy + dz * dz;
|
||||
dists.push_back(std::sqrt(dist_sq));
|
||||
}
|
||||
}
|
||||
if (dists.size() >= 5)
|
||||
{
|
||||
// Drop highest value to reduce influence of outliers
|
||||
auto max_it = std::max_element(dists.begin(), dists.end());
|
||||
dists.erase(max_it);
|
||||
}
|
||||
if (dists.size() > 0)
|
||||
{
|
||||
// Check if the average joint distance is close enough
|
||||
float avg_dist = std::accumulate(dists.begin(), dists.end(), 0.0);
|
||||
avg_dist /= static_cast<float>(dists.size());
|
||||
if (avg_dist < max_joint_avg_dist && avg_dist < best_dist)
|
||||
{
|
||||
best_dist = avg_dist;
|
||||
best_group = static_cast<int>(j);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (best_group == -1)
|
||||
{
|
||||
// Create a new group
|
||||
merged_groups.push_back(group);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Update existing group
|
||||
auto &merged_group = merged_groups[best_group];
|
||||
cv::Point3f &merged_center = std::get<0>(merged_group);
|
||||
cv::Mat &merged_group_pose = std::get<1>(merged_group);
|
||||
std::vector<int> &merged_group_indices = std::get<2>(merged_group);
|
||||
|
||||
float n_elems1 = static_cast<float>(merged_group_indices.size());
|
||||
float n_elems2 = static_cast<float>(std::get<2>(group).size());
|
||||
float inv1 = n_elems1 / (n_elems1 + n_elems2);
|
||||
float inv2 = n_elems2 / (n_elems1 + n_elems2);
|
||||
|
||||
// Update group center
|
||||
merged_center.x = (merged_center.x * inv1 + std::get<0>(group).x * inv2);
|
||||
merged_center.y = (merged_center.y * inv1 + std::get<0>(group).y * inv2);
|
||||
merged_center.z = (merged_center.z * inv1 + std::get<0>(group).z * inv2);
|
||||
|
||||
// Update group pose
|
||||
for (size_t row = 0; row < num_joints; ++row)
|
||||
{
|
||||
const float *group_pose_ptr = std::get<1>(group).ptr<float>(row);
|
||||
float *merged_pose_ptr = merged_group_pose.ptr<float>(row);
|
||||
|
||||
if (group_pose_ptr[3] > min_score)
|
||||
{
|
||||
float j_elems1 = static_cast<float>(group_visible_counts[row]);
|
||||
float j_elems2 = static_cast<float>(per_group_visible_counts[best_group][row]);
|
||||
float inv1 = j_elems1 / (j_elems1 + j_elems2);
|
||||
float inv2 = j_elems2 / (j_elems1 + j_elems2);
|
||||
|
||||
merged_pose_ptr[0] = (merged_pose_ptr[0] * inv1 + group_pose_ptr[0] * inv2);
|
||||
merged_pose_ptr[1] = (merged_pose_ptr[1] * inv1 + group_pose_ptr[1] * inv2);
|
||||
merged_pose_ptr[2] = (merged_pose_ptr[2] * inv1 + group_pose_ptr[2] * inv2);
|
||||
merged_pose_ptr[3] = (merged_pose_ptr[3] * inv1 + group_pose_ptr[3] * inv2);
|
||||
group_visible_counts[row]++;
|
||||
}
|
||||
}
|
||||
|
||||
// Merge indices
|
||||
merged_group_indices.insert(
|
||||
merged_group_indices.end(), std::get<2>(group).begin(), std::get<2>(group).end());
|
||||
}
|
||||
}
|
||||
|
||||
return merged_groups;
|
||||
}
|
||||
|
||||
// =================================================================================================
|
||||
|
||||
Reference in New Issue
Block a user