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

Parallelize some parts of the code.

Browse Source
This commit is contained in:
Daniel
2024-09-16 10:25:00 +02:00
parent 3350e2c4af
commit 3bf6e6e639
3 changed files with 98 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

154
spt/triangulator.cpp
View File

@ -1,5 +1,6 @@
#include <numeric>
#include <omp.h>
#include <opencv2/opencv.hpp>
#include "camera.hpp"
@ -180,6 +181,7 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
}
// Undistort 2D poses
#pragma omp parallel for
for (size_t i = 0; i < cameras.size(); ++i)
{
undistort_poses(poses_2d_mats[i], internal_cameras[i]);
@ -206,11 +208,12 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
}
// Project
last_poses_2d.reserve(cameras.size());
last_poses_2d.resize(cameras.size());
#pragma omp parallel for
for (size_t i = 0; i < cameras.size(); ++i)
{
auto [poses2d, dists] = project_poses(last_core_poses, internal_cameras[i], true);
last_poses_2d.emplace_back(poses2d, dists);
last_poses_2d[i] = std::make_tuple(poses2d, dists);
}
}
@ -219,38 +222,51 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
std::map<size_t, std::map<size_t, std::vector<size_t>>> scored_pasts;
if (!last_poses_3d.empty())
{
// Calculate index pairs and initialize vectors
std::vector<std::array<size_t, 3>> indices;
for (size_t i = 0; i < cameras.size(); ++i)
{
size_t num_persons = std::get<0>(last_poses_2d[i]).size();
scored_pasts[i] = std::map<size_t, std::vector<size_t>>();
for (size_t j = 0; j < num_persons; ++j)
{
scored_pasts[i][j] = std::vector<size_t>();
for (size_t k = 0; k < poses_2d_mats[i].size(); ++k)
{
indices.push_back({i, j, k});
}
}
}
#pragma omp parallel for ordered schedule(dynamic)
for (size_t e = 0; e < indices.size(); ++e)
{
const auto &i = indices[e][0];
const auto &j = indices[e][1];
const auto &k = indices[e][2];
const std::vector<cv::Mat> &poses = poses_2d_mats[i];
const std::vector<cv::Mat> &last_poses = std::get<0>(last_poses_2d[i]);
const std::vector<cv::Mat> &dists = std::get<1>(last_poses_2d[i]);
for (size_t j = 0; j < last_poses.size(); ++j)
// Select core joints
const cv::Mat &last_pose = last_poses[j];
const cv::Mat &pose = poses[k];
std::vector<int> dims = {(int)core_joint_idx.size(), 3};
cv::Mat pose_core(dims, pose.type());
for (size_t l = 0; l < core_joint_idx.size(); ++l)
{
scored_pasts[i][j] = std::vector<size_t>();
const cv::Mat &last_pose = last_poses[j];
size_t idx = core_joint_idx[l];
pose.row(idx).copyTo(pose_core.row(l));
}
for (size_t k = 0; k < poses.size(); ++k)
{
// Select core joints
const cv::Mat &pose = poses[k];
std::vector<int> dims = {(int)core_joint_idx.size(), 3};
cv::Mat pose_core(dims, pose.type());
for (size_t l = 0; l < core_joint_idx.size(); ++l)
{
size_t idx = core_joint_idx[l];
pose.row(idx).copyTo(pose_core.row(l));
}
// Calculate score
double score = calc_pose_score(
pose_core, last_pose, dists[j], internal_cameras[i]);
if (score > threshold)
{
scored_pasts[i][j].push_back(k);
}
}
// Calculate score
double score = calc_pose_score(pose_core, last_pose, dists[j], internal_cameras[i]);
if (score > threshold)
{
#pragma omp ordered
scored_pasts[i][j].push_back(k);
}
}
}
@ -264,6 +280,7 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
num_persons.push_back(poses_2d[i].size());
}
std::vector<std::pair<std::tuple<int, int, int, int>, std::pair<int, int>>> all_pairs;
std::vector<std::array<size_t, 4>> indices;
for (size_t i = 0; i < cameras.size(); ++i)
{
for (size_t j = i + 1; j < cameras.size(); ++j)
@ -272,44 +289,54 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
{
for (size_t l = 0; l < poses_2d[j].size(); ++l)
{
int pid1 = std::accumulate(num_persons.begin(), num_persons.begin() + i, 0) + k;
int pid2 = std::accumulate(num_persons.begin(), num_persons.begin() + j, 0) + l;
bool match = false;
if (!last_poses_3d.empty())
{
for (size_t m = 0; m < last_poses_3d.size(); ++m)
{
auto &smi = scored_pasts[i][m];
auto &smj = scored_pasts[j][m];
if (std::find(smi.begin(), smi.end(), k) != smi.end() &&
std::find(smj.begin(), smj.end(), l) != smj.end())
{
match = true;
all_pairs.emplace_back(
std::make_tuple(i, j, k, l), std::make_pair(pid1, pid2));
}
else if (std::find(smi.begin(), smi.end(), k) != smi.end() ||
std::find(smj.begin(), smj.end(), l) != smj.end())
{
match = true;
}
}
}
if (!match)
{
all_pairs.emplace_back(
std::make_tuple(i, j, k, l), std::make_pair(pid1, pid2));
}
indices.push_back({i, j, k, l});
}
}
}
}
#pragma omp parallel for ordered schedule(dynamic)
for (size_t e = 0; e < indices.size(); ++e)
{
const auto [i, j, k, l] = indices[e];
int pid1 = std::accumulate(num_persons.begin(), num_persons.begin() + i, 0) + k;
int pid2 = std::accumulate(num_persons.begin(), num_persons.begin() + j, 0) + l;
bool match = false;
if (!last_poses_3d.empty())
{
for (size_t m = 0; m < last_poses_3d.size(); ++m)
{
auto &smi = scored_pasts[i][m];
auto &smj = scored_pasts[j][m];
if (std::find(smi.begin(), smi.end(), k) != smi.end() &&
std::find(smj.begin(), smj.end(), l) != smj.end())
{
match = true;
#pragma omp ordered
all_pairs.emplace_back(
std::make_tuple(i, j, k, l), std::make_pair(pid1, pid2));
}
else if (std::find(smi.begin(), smi.end(), k) != smi.end() ||
std::find(smj.begin(), smj.end(), l) != smj.end())
{
match = true;
}
}
}
if (!match)
{
#pragma omp ordered
all_pairs.emplace_back(
std::make_tuple(i, j, k, l), std::make_pair(pid1, pid2));
}
}
// Calculate pair scores
std::vector<std::pair<cv::Mat, double>> all_scored_poses;
all_scored_poses.reserve(all_pairs.size());
all_scored_poses.resize(all_pairs.size());
#pragma omp parallel for
for (size_t i = 0; i < all_pairs.size(); ++i)
{
const auto &pids = all_pairs[i].first;
@ -336,7 +363,7 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
// Triangulate and score
auto [pose3d, score] = triangulate_and_score(
pose1_core, pose2_core, cam1, cam2, roomparams, core_limbs_idx);
all_scored_poses.emplace_back(pose3d, score);
all_scored_poses[i] = std::make_pair(pose3d, score);
}
// Drop low scoring poses
@ -363,7 +390,8 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
// Calculate full 3D poses
std::vector<cv::Mat> all_full_poses;
all_full_poses.reserve(all_pairs.size());
all_full_poses.resize(all_pairs.size());
#pragma omp parallel for
for (size_t i = 0; i < all_pairs.size(); ++i)
{
const auto &pids = all_pairs[i].first;
@ -372,13 +400,15 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
const auto &pose1 = poses_2d_mats[std::get<0>(pids)][std::get<2>(pids)];
const auto &pose2 = poses_2d_mats[std::get<1>(pids)][std::get<3>(pids)];
auto [pose3d, score] = triangulate_and_score(pose1, pose2, cam1, cam2, roomparams, {});
all_full_poses.push_back(pose3d);
auto [pose3d, score] = triangulate_and_score(
pose1, pose2, cam1, cam2, roomparams, {});
all_full_poses[i] = (pose3d);
}
// Merge groups
std::vector<cv::Mat> all_merged_poses;
all_merged_poses.reserve(groups.size());
all_merged_poses.resize(groups.size());
#pragma omp parallel for
for (size_t i = 0; i < groups.size(); ++i)
{
const auto &group = groups[i];
@ -389,7 +419,7 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
}
auto merged_pose = merge_group(poses, min_score);
all_merged_poses.push_back(merged_pose);
all_merged_poses[i] = (merged_pose);
}
// Convert to output format