crosstyan/RapidPoseTriangulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Some fixes. Updated benchmarks.

Browse Source
This commit is contained in:
Daniel
2024-09-17 18:23:41 +02:00
parent 8eba0ffa65
commit ba910e11e6
5 changed files with 330 additions and 328 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
spt/triangulator.cpp
View File

@ -925,8 +925,8 @@ std::pair<cv::Mat, float> TriangulatorInternal::triangulate_and_score(
{
mean[j] *= inv_num_vis;
}
const std::array<float, 3> &center = roomparams[0];
const std::array<float, 3> &size = roomparams[1];
const std::array<float, 3> &size = roomparams[0];
const std::array<float, 3> &center = roomparams[1];
for (int j = 0; j < 3; ++j)
{
if (mean[j] > center[j] + size[j] / 2.0 ||
@ -1061,9 +1061,7 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
{
float max_center_dist_sq = 0.6 * 0.6;
float max_joint_avg_dist = 0.3;
size_t num_pairs = all_pairs.size();
size_t num_joints = all_scored_poses[0].first.rows;
// Calculate pose centers
std::vector<cv::Point3d> centers;
@ -1071,6 +1069,8 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
for (size_t i = 0; i < num_pairs; ++i)
{
const cv::Mat &pose_3d = all_scored_poses[i].first;
size_t num_joints = pose_3d.rows;
cv::Point3d center(0, 0, 0);
size_t num_valid = 0;
for (size_t j = 0; j < num_joints; ++j)
@ -1102,6 +1102,8 @@ std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> TriangulatorInte
for (size_t i = 0; i < num_pairs; ++i)
{
const cv::Mat &pose_3d = all_scored_poses[i].first;
size_t num_joints = pose_3d.rows;
const cv::Point3d &center = centers[i];
float best_dist = std::numeric_limits<float>::infinity();
int best_group = -1;
@ -1512,7 +1514,7 @@ void TriangulatorInternal::filter_poses(
{
// Check if the mean position is outside the room boundaries
if (mean[j] > room_half_size[j] + room_center[j] ||
mean[j] < -room_half_size[j] - room_center[j])
mean[j] < -room_half_size[j] + room_center[j])
{
outside = true;
break;
@ -1522,7 +1524,7 @@ void TriangulatorInternal::filter_poses(
{
// Check if any limb is too far outside the room
if (maxs[j] > room_half_size[j] + room_center[j] + wdist ||
mins[j] < -room_half_size[j] - room_center[j] - wdist)
mins[j] < -room_half_size[j] + room_center[j] - wdist)
{
outside = true;
break;
@ -1540,8 +1542,8 @@ void TriangulatorInternal::filter_poses(
size_t found_limbs = 0;
for (size_t j = 0; j < core_limbs_idx.size(); ++j)
{
size_t start_idx = core_limbs_idx[j][0];
size_t end_idx = core_limbs_idx[j][1];
size_t start_idx = core_joint_idx[core_limbs_idx[j][0]];
size_t end_idx = core_joint_idx[core_limbs_idx[j][1]];
float *joint_start_ptr = pose.ptr<float>(start_idx);
float *joint_end_ptr = pose.ptr<float>(end_idx);
@ -1565,8 +1567,8 @@ void TriangulatorInternal::filter_poses(
int total_limbs = 0;
for (size_t j = 0; j < core_limbs_idx.size(); ++j)
{
size_t start_idx = core_limbs_idx[j][0];
size_t end_idx = core_limbs_idx[j][1];
size_t start_idx = core_joint_idx[core_limbs_idx[j][0]];
size_t end_idx = core_joint_idx[core_limbs_idx[j][1]];
float *joint_start_ptr = pose.ptr<float>(start_idx);
float *joint_end_ptr = pose.ptr<float>(end_idx);
@ -1727,7 +1729,7 @@ void TriangulatorInternal::add_missing_joints(
if (adjacents.find(adname) != adjacents.end())
{
// Joint is missing; attempt to estimate its position based on adjacent joints
std::array<float, 3> adjacent_position;
std::array<float, 3> adjacent_position = {0.0f, 0.0f, 0.0f};
size_t adjacent_count = 0;
const std::vector<std::string> &adjacent_joint_names = adjacents[adname];