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

Added filtering with last poses.

Browse Source
This commit is contained in:
Daniel
2024-09-12 10:03:48 +02:00
parent 3e3a5194e2
commit b9949bfe4a
3 changed files with 146 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

129
spt/triangulator.cpp
View File

@ -178,6 +178,74 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
internal_cameras[i].update_projection_matrix();
}
// Project last 3D poses to 2D
std::vector<std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>>> last_poses_2d;
if (!last_poses_3d.empty())
{
// Select core joints
std::vector<cv::Mat> last_core_poses;
for (size_t i = 0; i < last_poses_3d.size(); ++i)
{
cv::Mat pose = last_poses_3d[i];
std::vector<int> dims = {(int)core_joint_idx.size(), 4};
cv::Mat last_poses_core(dims, pose.type());
for (size_t j = 0; j < core_joint_idx.size(); ++j)
{
pose.row(core_joint_idx[j]).copyTo(last_poses_core.row(j));
}
last_core_poses.push_back(last_poses_core);
}
// Project
last_poses_2d.reserve(cameras.size());
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);
}
}
// Check matches to old poses
double threshold = min_score - 0.2;
std::map<size_t, std::map<size_t, std::vector<size_t>>> scored_pasts;
if (!last_poses_3d.empty())
{
for (size_t i = 0; i < cameras.size(); ++i)
{
scored_pasts[i] = std::map<size_t, std::vector<size_t>>();
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)
{
scored_pasts[i][j] = std::vector<size_t>();
const cv::Mat &last_pose = last_poses[j];
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);
}
}
}
}
}
// Create pairs of persons
// Checks if the person was already matched to the last frame and if so only creates pairs
// with those, else it creates all possible pairs
@ -199,6 +267,27 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
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(
@ -316,6 +405,7 @@ std::vector<std::vector<std::array<double, 4>>> TriangulatorInternal::triangulat
poses_3d.push_back(std::move(pose));
}
last_poses_3d = all_merged_poses;
return poses_3d;
}
@ -489,6 +579,45 @@ std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> TriangulatorInternal::pro
// =================================================================================================
double TriangulatorInternal::calc_pose_score(
const cv::Mat &pose1,
const cv::Mat &pose2,
const cv::Mat &dist1,
const CameraInternal &icam)
{
// Create mask for valid points
double min_score = 0.1;
cv::Mat mask = (pose1.col(2) > min_score) & (pose2.col(2) > min_score);
int valid_count = cv::countNonZero(mask);
// Drop if not enough valid points
if (valid_count < 3)
{
return 0.0;
}
// Calculate scores
double iscale = (icam.cam.width + icam.cam.height) / 2;
cv::Mat scores = score_projection(
pose1, pose2, dist1, mask, iscale);
// Drop lowest scores
int drop_k = static_cast<int>(pose1.rows * 0.2);
std::sort(scores.begin<double>(), scores.end<double>());
// Calculate final score
double score = 0.0;
for (int i = drop_k; i < scores.rows; i++)
{
score += scores.at<double>(i);
}
score /= (pose1.rows - drop_k);
return score;
}
// =================================================================================================
cv::Mat TriangulatorInternal::score_projection(
const cv::Mat &pose1,
const cv::Mat &repro1,

6
spt/triangulator.hpp
View File

@ -66,6 +66,12 @@ private:
std::tuple<std::vector<cv::Mat>, std::vector<cv::Mat>> project_poses(
const std::vector<cv::Mat> &bodies3D, const CameraInternal &icam, bool calc_dists);
double calc_pose_score(
const cv::Mat &pose1,
const cv::Mat &pose2,
const cv::Mat &dist1,
const CameraInternal &icam);
cv::Mat score_projection(
const cv::Mat &pose1,
const cv::Mat &repro1,

11
tests/test_interface.py
View File

@ -67,6 +67,7 @@ def main():
)
print("3D time:", time.time() - stime)
print(np.array(poses_3d))
print("")
# Load input data
roomparams = [[0, -0.5, 1.2], [5.6, 6.4, 2.4]]
@ -88,6 +89,16 @@ def main():
)
print("3D time:", time.time() - stime)
print(np.array(poses_3d))
print("")
# Run again to test last pose cache
stime = time.time()
poses_3d = triangulator.triangulate_poses(
poses_2d, cameras, roomparams, joint_names
)
print("3D time:", time.time() - stime)
print(np.array(poses_3d))
print("")
# ==================================================================================================