diff --git a/.gitignore b/.gitignore index 82f9275..30ec818 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,40 @@ +spt_wrap.* +spt.py +*.bin + +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + # Byte-compiled / optimized / DLL files __pycache__/ *.py[cod] diff --git a/Dockerfile b/Dockerfile index 2412887..1007af5 100644 --- a/Dockerfile +++ b/Dockerfile @@ -37,6 +37,11 @@ RUN python3 -c "from utils_2d_pose import load_wb_model; load_wb_model();" # Fix an undefined symbol error with ompi RUN echo "ldconfig" >> ~/.bashrc +# Install swig and later dependencies +RUN apt-get update && apt-get install -y --no-install-recommends build-essential +RUN apt-get update && apt-get install -y --no-install-recommends swig +RUN apt-get update && apt-get install -y --no-install-recommends libopencv-dev + COPY ./skelda/ /skelda/ RUN pip3 install --no-cache-dir -e /skelda/ diff --git a/README.md b/README.md index f04c36b..4812eb3 100644 --- a/README.md +++ b/README.md @@ -34,3 +34,11 @@ Triangulation of multiple persons from multiple camera views. export CUDA_VISIBLE_DEVICES=0 python3 /SimplePoseTriangulation/scripts/test_skelda_dataset.py ``` + +
+ +## Debugging + +```bash +cd /SimplePoseTriangulation/swig/ && make all && cd ../tests/ && python3 test_interface.py +``` diff --git a/scripts/test_triangulate.py b/scripts/test_triangulate.py index f1189e1..7a38f5a 100644 --- a/scripts/test_triangulate.py +++ b/scripts/test_triangulate.py @@ -1,6 +1,7 @@ import copy import json import os +import time from typing import List import cv2 @@ -413,6 +414,7 @@ def main(): continue # Load sample infos + print("\n" + dirpath) with open(os.path.join(dirpath, "sample.json"), "r", encoding="utf-8") as file: sample = json.load(file) sample = update_sample(sample, dirpath) @@ -431,8 +433,11 @@ def main(): images_2d.append(img) # Get 2D poses + stime = time.time() poses_2d = utils_2d_pose.get_2d_pose(kpt_model, images_2d) poses_2d = update_keypoints(poses_2d, joint_names_2d) + print("2D time:", time.time() - stime) + # print(np.array(poses_2d).round(3).tolist()) fig1 = draw_utils.show_poses2d( poses_2d, np.array(images_2d), joint_names_2d, "2D detections" @@ -449,9 +454,11 @@ def main(): poses3D = np.zeros([1, len(joint_names_3d), 4]) poses2D = np.zeros([len(images_2d), 1, len(joint_names_3d), 3]) else: + stime = time.time() poses3D = triangulate_poses.get_3d_pose( poses_2d, camparams, roomparams, joint_names_2d ) + print("3D time:", time.time() - stime) poses2D = [] for cam in camparams: poses_2d, _ = utils_pose.project_poses(poses3D, cam) @@ -463,9 +470,9 @@ def main(): joint_names_3d, ) - print("\n" + dirpath) print(poses3D) # print(poses2D) + # print(poses3D.round(3).tolist()) fig2 = draw_utils.utils_view.show_poses3d( poses3D, joint_names_3d, roomparams, camparams diff --git a/spt/camera.cpp b/spt/camera.cpp new file mode 100644 index 0000000..0496b43 --- /dev/null +++ b/spt/camera.cpp @@ -0,0 +1,76 @@ +#include +#include + +#include "camera.hpp" + +// ================================================================================================= +// ================================================================================================= + +template +static const std::string print_matrix(const std::array, M> &matrix) +{ + std::ostringstream out; + out << "["; + for (size_t j = 0; j < M; ++j) + { + out << "["; + for (size_t i = 0; i < N; ++i) + { + out << matrix[j][i]; + if (i < N - 1) + { + out << ", "; + } + } + out << "]"; + if (j < M - 1) + { + out << ", "; + } + } + out << "]"; + return out.str(); +} + +// ================================================================================================= +// ================================================================================================= + +std::string Camera::to_string() const +{ + std::ostringstream out; + out << std::fixed << std::setprecision(6); + + out << "{"; + out << "'name': '" << name << "', "; + + out << "'K': " << print_matrix(K) << ", "; + + out << "'DC': ["; + for (size_t i = 0; i < DC.size(); ++i) + { + out << DC[i]; + if (i < DC.size() - 1) + out << ", "; + } + out << "], "; + + out << "'R': " << print_matrix(R) << ", "; + out << "'T': " << print_matrix(T) << ", "; + out << "'P': " << print_matrix(P) << ", "; + + out << "'width': " << width << ", "; + out << "'height': " << height << ", "; + out << "'type': " << type; + + out << "}"; + return out.str(); +} + +// ================================================================================================= + +std::ostream &operator<<(std::ostream &out, const Camera &cam) +{ + out << cam.to_string(); + return out; +} + diff --git a/spt/camera.hpp b/spt/camera.hpp new file mode 100644 index 0000000..f02b86f --- /dev/null +++ b/spt/camera.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include +#include +#include +#include + +// ================================================================================================= + +struct Camera +{ + std::string name; + std::array, 3> K; + std::vector DC; + std::array, 3> R; + std::array, 3> T; + std::array, 4> P; + int width; + int height; + std::string type; + + friend std::ostream &operator<<(std::ostream &out, Camera const &camera); + std::string to_string() const; +}; diff --git a/spt/interface.cpp b/spt/interface.cpp new file mode 100644 index 0000000..940d7af --- /dev/null +++ b/spt/interface.cpp @@ -0,0 +1,28 @@ +#include "triangulator.hpp" +#include "interface.hpp" + +// ================================================================================================= +// ================================================================================================= + +Triangulator::Triangulator(double min_score) +{ + this->triangulator = new TriangulatorInternal(min_score); +} + +// ================================================================================================= + +std::vector>> Triangulator::triangulate_poses( + const std::vector>>> &poses_2d, + const std::vector &cameras, + const std::array, 2> &roomparams, + const std::vector &joint_names) +{ + return this->triangulator->triangulate_poses(poses_2d, cameras, roomparams, joint_names); +} + +// ================================================================================================= + +void Triangulator::reset() +{ + this->triangulator->reset(); +} diff --git a/spt/interface.hpp b/spt/interface.hpp new file mode 100644 index 0000000..0092258 --- /dev/null +++ b/spt/interface.hpp @@ -0,0 +1,49 @@ +#pragma once + +#include +#include + +#include "camera.hpp" + +// ================================================================================================= + +// Forward declaration of the class, that swig does try to parse all its dependencies. +class TriangulatorInternal; + +// ================================================================================================= + +class Triangulator +{ +public: + /** + * Triangulator to predict poses from multiple views. + * + * + * @param min_score Minimum score to consider a triangulated joint as valid. + */ + Triangulator( + double min_score = 0.95); + + /** + * Calculate a triangulation. + * + * + * @param poses_2d List of shape [views, persons, joints, 3], containing the 2D poses. + * @param cameras List of cameras. + * @param roomparams Room parameters (room center, room size). + * @param joint_names List of 2D joint names. + * + * @return List of shape [persons, joints, 4], containing the 3D poses. + */ + std::vector>> triangulate_poses( + const std::vector>>> &poses_2d, + const std::vector &cameras, + const std::array, 2> &roomparams, + const std::vector &joint_names); + + /** Reset the triangulator. */ + void reset(); + +private: + TriangulatorInternal *triangulator; +}; diff --git a/spt/triangulator.cpp b/spt/triangulator.cpp new file mode 100644 index 0000000..433dd26 --- /dev/null +++ b/spt/triangulator.cpp @@ -0,0 +1,887 @@ +#include + +#include + +#include "camera.hpp" +#include "triangulator.hpp" + +// ================================================================================================= +// ================================================================================================= + +[[maybe_unused]] static void print_2d_mat(const cv::Mat &mat) +{ + // Ensure the matrix is 2D + if (mat.dims != 2) + { + std::cerr << "Error: The matrix is not 2D." << std::endl; + return; + } + + // Retrieve matrix dimensions + int rows = mat.rows; + int cols = mat.cols; + + // Print the matrix in a NumPy-like style + std::cout << "cv::Mat('shape': (" << rows << ", " << cols << ")"; + std::cout << ", 'data': [" << std::endl; + + for (int i = 0; i < rows; ++i) + { + std::cout << " ["; + for (int j = 0; j < cols; ++j) + { + std::cout << std::fixed << std::setprecision(3) << mat.at(i, j); + if (j < cols - 1) + { + std::cout << ", "; + } + } + std::cout << "]"; + if (i < rows - 1) + { + std::cout << "," << std::endl; + } + } + std::cout << "])" << std::endl; +} + +// ================================================================================================= + +[[maybe_unused]] static void print_allpairs( + const std::vector, std::pair>> &all_pairs) +{ + std::cout << "All pairs: [" << std::endl; + for (const auto &pair : all_pairs) + { + const auto &tuple_part = pair.first; + const auto &pair_part = pair.second; + + // Print the tuple part + std::cout << "(" + << std::get<0>(tuple_part) << ", " + << std::get<1>(tuple_part) << ", " + << std::get<2>(tuple_part) << ", " + << std::get<3>(tuple_part) << ")"; + + // Print the pair part + std::cout << ", (" + << pair_part.first << ", " + << pair_part.second << ")" + << std::endl; + } + std::cout << "]" << std::endl; +} + +// ================================================================================================= +// ================================================================================================= + +CameraInternal::CameraInternal(const Camera &cam) +{ + this->cam = cam; + + // Convert camera matrix to cv::Mat for OpenCV + K = cv::Mat(3, 3, CV_64FC1, const_cast(&cam.K[0][0])).clone(); + DC = cv::Mat(cam.DC.size(), 1, CV_64FC1, const_cast(cam.DC.data())).clone(); + R = cv::Mat(3, 3, CV_64FC1, const_cast(&cam.R[0][0])).clone(); + T = cv::Mat(3, 1, CV_64FC1, const_cast(&cam.T[0][0])).clone(); +} + +// ================================================================================================= + +void CameraInternal::update_projection_matrix() +{ + // Calculate opencv-style projection matrix + cv::Mat Tr, RT; + Tr = R * (T * -1); + cv::hconcat(R, Tr, RT); + P = K * RT; +} + +// ================================================================================================= +// ================================================================================================= + +TriangulatorInternal::TriangulatorInternal(double min_score) +{ + this->min_score = min_score; +} + +// ================================================================================================= + +std::vector>> TriangulatorInternal::triangulate_poses( + const std::vector>>> &poses_2d, + const std::vector &cameras, + const std::array, 2> &roomparams, + const std::vector &joint_names) +{ + // Check inputs + if (poses_2d.empty()) + { + throw std::invalid_argument("No 2D poses provided."); + } + if (poses_2d.size() != cameras.size()) + { + throw std::invalid_argument("Number of cameras and 2D poses must be the same."); + } + if (joint_names.size() != poses_2d[0][0].size()) + { + throw std::invalid_argument("Number of joint names and 2D poses must be the same."); + } + for (const auto &joint : core_joints) + { + if (std::find(joint_names.begin(), joint_names.end(), joint) == joint_names.end()) + { + throw std::invalid_argument("Core joint '" + joint + "' not found in joint names."); + } + } + + // Convert inputs to internal formats + std::vector> poses_2d_mats; + poses_2d_mats.reserve(cameras.size()); + for (size_t i = 0; i < cameras.size(); ++i) + { + size_t num_persons = poses_2d[i].size(); + size_t num_joints = poses_2d[i][0].size(); + std::vector camera_poses; + camera_poses.reserve(num_persons); + + for (size_t j = 0; j < num_persons; ++j) + { + std::vector dims = {(int)num_joints, 3}; + cv::Mat pose_mat(dims, CV_64F); + for (size_t k = 0; k < num_joints; ++k) + { + for (size_t l = 0; l < 3; ++l) + { + pose_mat.at(k, l) = poses_2d[i][j][k][l]; + } + } + camera_poses.push_back(pose_mat); + } + poses_2d_mats.push_back(camera_poses); + } + std::vector internal_cameras; + for (size_t i = 0; i < cameras.size(); ++i) + { + internal_cameras.emplace_back(cameras[i]); + } + std::vector core_joint_idx; + for (const auto &joint : core_joints) + { + auto it = std::find(joint_names.begin(), joint_names.end(), joint); + core_joint_idx.push_back(std::distance(joint_names.begin(), it)); + } + + // Undistort 2D poses + for (size_t i = 0; i < cameras.size(); ++i) + { + undistort_poses(poses_2d_mats[i], internal_cameras[i]); + internal_cameras[i].update_projection_matrix(); + } + + // 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 + std::vector num_persons; + for (size_t i = 0; i < cameras.size(); ++i) + { + num_persons.push_back(poses_2d[i].size()); + } + std::vector, std::pair>> all_pairs; + for (size_t i = 0; i < cameras.size(); ++i) + { + for (size_t j = i + 1; j < cameras.size(); ++j) + { + for (size_t k = 0; k < poses_2d[i].size(); ++k) + { + 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 (!match) + { + all_pairs.emplace_back( + std::make_tuple(i, j, k, l), std::make_pair(pid1, pid2)); + } + } + } + } + } + + // Calculate pair scores + std::vector> all_scored_poses; + all_scored_poses.reserve(all_pairs.size()); + for (size_t i = 0; i < all_pairs.size(); ++i) + { + const auto &pids = all_pairs[i].first; + + // Extract camera parameters + const auto &cam1 = internal_cameras[std::get<0>(pids)]; + const auto &cam2 = internal_cameras[std::get<1>(pids)]; + + // Extract 2D poses + const cv::Mat &pose1 = poses_2d_mats[std::get<0>(pids)][std::get<2>(pids)]; + const cv::Mat &pose2 = poses_2d_mats[std::get<1>(pids)][std::get<3>(pids)]; + + // Select core joints + std::vector dims = {(int)core_joint_idx.size(), 3}; + cv::Mat pose1_core(dims, pose1.type()); + cv::Mat pose2_core(dims, pose2.type()); + for (size_t j = 0; j < core_joint_idx.size(); ++j) + { + size_t idx = core_joint_idx[j]; + pose1.row(idx).copyTo(pose1_core.row(j)); + pose2.row(idx).copyTo(pose2_core.row(j)); + } + + // Triangulate and score + auto [pose3d, score] = triangulate_and_score(pose1_core, pose2_core, cam1, cam2, roomparams); + all_scored_poses.emplace_back(pose3d, score); + } + + // Drop low scoring poses + std::vector drop_indices; + for (size_t i = 0; i < all_scored_poses.size(); ++i) + { + if (all_scored_poses[i].second < min_score) + { + drop_indices.push_back(i); + } + } + if (!drop_indices.empty()) + { + for (size_t i = drop_indices.size() - 1; i >= 0; --i) + { + all_scored_poses.erase(all_scored_poses.begin() + drop_indices[i]); + all_pairs.erase(all_pairs.begin() + drop_indices[i]); + } + } + + // Group pairs that share a person + std::vector>> groups; + groups = calc_grouping(all_pairs, all_scored_poses, min_score); + + // Calculate full 3D poses + std::vector all_full_poses; + all_full_poses.reserve(all_pairs.size()); + for (size_t i = 0; i < all_pairs.size(); ++i) + { + const auto &pids = all_pairs[i].first; + const auto &cam1 = internal_cameras[std::get<0>(pids)]; + const auto &cam2 = internal_cameras[std::get<1>(pids)]; + 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); + } + + // Merge groups + std::vector all_merged_poses; + all_merged_poses.reserve(groups.size()); + for (size_t i = 0; i < groups.size(); ++i) + { + const auto &group = groups[i]; + std::vector poses; + for (const auto &idx : std::get<2>(group)) + { + poses.push_back(all_full_poses[idx]); + } + + auto merged_pose = merge_group(poses, min_score); + all_merged_poses.push_back(merged_pose); + } + + // Convert to output format + std::vector>> poses_3d; + poses_3d.reserve(all_merged_poses.size()); + for (size_t i = 0; i < all_merged_poses.size(); ++i) + { + const auto &mat = all_merged_poses[i]; + std::vector> pose; + size_t num_joints = mat.rows; + pose.reserve(num_joints); + + for (size_t j = 0; j < num_joints; ++j) + { + std::array point; + for (size_t k = 0; k < 4; ++k) + { + point[k] = mat.at(j, k); + } + pose.push_back(point); + } + + poses_3d.push_back(std::move(pose)); + } + + return poses_3d; +} + +// ================================================================================================= + +void TriangulatorInternal::reset() +{ + last_poses_3d.clear(); +} + +// ================================================================================================= + +void TriangulatorInternal::undistort_points(cv::Mat &points, CameraInternal &icam) +{ + int width = icam.cam.width; + int height = icam.cam.height; + + // Undistort camera matrix + cv::Mat newK = cv::getOptimalNewCameraMatrix( + icam.K, icam.DC, cv::Size(width, height), 1, cv::Size(width, height)); + + // Undistort points + cv::undistortPoints(points, points, icam.K, icam.DC, cv::noArray(), newK); + + // Update the camera parameters + icam.K = newK; + icam.DC = cv::Mat::zeros(5, 1, CV_64F); +} + +// ================================================================================================= + +void TriangulatorInternal::undistort_poses(std::vector &poses, CameraInternal &icam) +{ + for (size_t p = 0; p < poses.size(); ++p) + { + int num_joints = poses[p].rows; + + // Extract the (x, y) coordinates + std::vector dims = {num_joints, 2}; + cv::Mat points(dims, CV_64F); + for (int j = 0; j < num_joints; ++j) + { + points.at(j, 0) = poses[p].at(j, 0); + points.at(j, 1) = poses[p].at(j, 1); + } + + // Undistort the points + undistort_points(points, icam); + + // Update the original poses with the undistorted points + for (int j = 0; j < num_joints; ++j) + { + poses[p].at(j, 0) = points.at(j, 0); + poses[p].at(j, 1) = points.at(j, 1); + } + + // Mask out points that are far outside the image (points slightly outside are still valid) + double offset = (icam.cam.width + icam.cam.height) / 40.0; + for (int j = 0; j < num_joints; ++j) + { + double x = poses[p].at(j, 0); + double y = poses[p].at(j, 1); + bool in_x = x >= -offset && x < icam.cam.width + offset; + bool in_y = y >= -offset && y < icam.cam.height + offset; + if (!in_x || !in_y) + { + poses[p].at(j, 0) = 0; + poses[p].at(j, 1) = 0; + poses[p].at(j, 2) = 0; + } + } + } +} + +// ================================================================================================= + +std::tuple, std::vector> TriangulatorInternal::project_poses( + const std::vector &bodies3D, const CameraInternal &icam, bool calc_dists) +{ + size_t num_persons = bodies3D.size(); + std::vector bodies2D_list(num_persons); + std::vector dists_list(num_persons); + + for (size_t i = 0; i < num_persons; ++i) + { + const cv::Mat &body3D = bodies3D[i]; + size_t num_joints = body3D.size[0]; + + // Split up vector + std::vector dimsA = {(int)num_joints, 3}; + cv::Mat points3d(dimsA, CV_64F); + for (size_t i = 0; i < num_joints; ++i) + { + points3d.at(i, 0) = body3D.at(i, 0); + points3d.at(i, 1) = body3D.at(i, 1); + points3d.at(i, 2) = body3D.at(i, 2); + } + + // Project from world to camera coordinate system + cv::Mat T_repeated = cv::repeat(icam.T, 1, num_joints); + cv::Mat xyz = (icam.R * (points3d.t() - T_repeated)).t(); + + // Set points behind the camera to zero + for (size_t i = 0; i < num_joints; ++i) + { + if (xyz.at(i, 2) <= 0) + { + xyz.at(i, 0) = 0; + xyz.at(i, 1) = 0; + xyz.at(i, 2) = 0; + } + } + + // Calculate distance from camera center + cv::Mat dists; + if (calc_dists) + { + dists = xyz.mul(xyz); + cv::reduce(dists, dists, 1, cv::REDUCE_SUM, CV_64F); + cv::sqrt(dists, dists); + } + + // Project to camera system + cv::Mat uv; + if (icam.cam.type == "fisheye") + { + } + else + { + cv::Mat DCc = icam.DC.rowRange(0, 5).clone(); + cv::projectPoints( + xyz, cv::Mat::zeros(3, 1, CV_64F), cv::Mat::zeros(3, 1, CV_64F), icam.K, DCc, uv); + } + uv = uv.reshape(1, {xyz.rows, 2}); + + // Add scores again + std::vector dimsB = {(int)num_joints, 3}; + cv::Mat bodies2D(dimsB, CV_64F); + for (size_t i = 0; i < num_joints; ++i) + { + bodies2D.at(i, 0) = uv.at(i, 0); + bodies2D.at(i, 1) = uv.at(i, 1); + bodies2D.at(i, 2) = body3D.at(i, 3); + } + + // Filter invalid projections + cv::Mat valid_x = (bodies2D.col(0) >= 0) & (bodies2D.col(0) < icam.cam.width); + cv::Mat valid_y = (bodies2D.col(1) >= 0) & (bodies2D.col(1) < icam.cam.height); + cv::Mat vis = valid_x & valid_y; + for (size_t i = 0; i < num_joints; ++i) + { + if (!vis.at(i)) + { + bodies2D.at(i, 0) = 0; + bodies2D.at(i, 1) = 0; + bodies2D.at(i, 2) = 0; + } + } + + // Store results + bodies2D_list[i] = bodies2D; + dists_list[i] = dists; + } + + return std::make_tuple(bodies2D_list, dists_list); +} + +// ================================================================================================= + +cv::Mat TriangulatorInternal::score_projection( + const cv::Mat &pose1, + const cv::Mat &repro1, + const cv::Mat &dists1, + const cv::Mat &mask, + double iscale) +{ + double min_score = 0.1; + double penalty = iscale; + + // Calculate error + cv::Mat diff = pose1.colRange(0, 2) - repro1.colRange(0, 2); + cv::Mat error1; + error1 = diff.mul(diff); + cv::reduce(error1, error1, 1, cv::REDUCE_SUM, CV_64F); + cv::sqrt(error1, error1); + error1.setTo(0.0, ~mask); + + // Set errors of invisible reprojections to a high value + cv::Mat mask_invisible = (repro1.col(2) < min_score); + error1.setTo(penalty, mask_invisible); + + // Scale error by image size and distance to the camera + error1 = cv::min(error1, (iscale / 4.0)) / iscale; + + // Compute scaling factor + double dscale1 = std::sqrt(cv::mean(dists1).val[0] / 3.5); + + // Scale errors + error1 *= dscale1; + + // Convert errors to a score + cv::Mat score1 = 1.0 / (1.0 + error1 * 10); + + return score1; +} + +// ================================================================================================= + +std::pair TriangulatorInternal::triangulate_and_score( + const cv::Mat &pose1, + const cv::Mat &pose2, + const CameraInternal &cam1, + const CameraInternal &cam2, + const std::array, 2> &roomparams) +{ + // Mask out invisible joints + double min_score = 0.1; + cv::Mat mask1a = (pose1.col(2) >= min_score); + cv::Mat mask2a = (pose2.col(2) >= min_score); + cv::Mat mask = mask1a & mask2a; + + // If too few joints are visible, return a low score + int num_visible = cv::countNonZero(mask); + if (num_visible < 3) + { + std::vector dims = {(int)pose1.rows, 4}; + cv::Mat pose3d(dims, CV_64F); + double score = 0.0; + return std::make_pair(pose3d, score); + } + + // Triangulate points + std::vector dimsA = {2, num_visible}; + cv::Mat points1(dimsA, CV_64F); + cv::Mat points2(dimsA, CV_64F); + int idx = 0; + for (int i = 0; i < pose1.rows; ++i) + { + if (mask.at(i) > 0) + { + points1.at(0, idx) = pose1.at(i, 0); + points1.at(1, idx) = pose1.at(i, 1); + points2.at(0, idx) = pose2.at(i, 0); + points2.at(1, idx) = pose2.at(i, 1); + idx++; + } + } + + cv::Mat points3d_h, points3d; + cv::triangulatePoints(cam1.P, cam2.P, points1, points2, points3d_h); + cv::convertPointsFromHomogeneous(points3d_h.t(), points3d); + + // Create the 3D pose matrix + std::vector dimsB = {(int)pose1.rows, 4}; + cv::Mat pose3d(dimsB, CV_64F); + for (int i = 0; i < pose1.rows; ++i) + { + if (mask.at(i) > 0) + { + pose3d.at(i, 0) = points3d.at(i, 0); + pose3d.at(i, 1) = points3d.at(i, 1); + pose3d.at(i, 2) = points3d.at(i, 2); + pose3d.at(i, 3) = 1.0; + } + } + + // Check if points are inside the room bounds + cv::Mat mean, mins, maxs; + cv::reduce(pose3d.colRange(0, 3), mean, 0, cv::REDUCE_AVG); + cv::reduce(pose3d.colRange(0, 3), mins, 0, cv::REDUCE_MIN); + cv::reduce(pose3d.colRange(0, 3), maxs, 0, cv::REDUCE_MAX); + double wdist = 0.1; + std::array center = roomparams[0]; + std::array size = roomparams[1]; + for (int i = 0; i < 3; ++i) + { + if (mean.at(i) > center[i] + size[i] / 2 || + mean.at(i) < center[i] - size[i] / 2 || + maxs.at(i) > center[i] + size[i] / 2 + wdist || + mins.at(i) < center[i] - size[i] / 2 - wdist) + { + // Very low score if outside room + return {pose3d, 0.001}; + } + } + + // Calculate reprojections + std::vector poses_3d = {pose3d}; + cv::Mat repro1, dists1, repro2, dists2; + auto [repro1s, dists1s] = project_poses(poses_3d, cam1, true); + auto [repro2s, dists2s] = project_poses(poses_3d, cam2, true); + repro1 = repro1s[0]; + dists1 = dists1s[0]; + repro2 = repro2s[0]; + dists2 = dists2s[0]; + + // Calculate scores for each view + double iscale = (cam1.cam.width + cam1.cam.height) / 2.0; + cv::Mat score1 = score_projection(pose1, repro1, dists1, mask, iscale); + cv::Mat score2 = score_projection(pose2, repro2, dists2, mask, iscale); + + // Combine scores + cv::Mat scores = (score1 + score2) / 2.0; + + // Add scores to 3D pose + for (int i = 0; i < pose1.rows; ++i) + { + if (mask.at(i) > 0) + { + pose3d.at(i, 3) = scores.at(i); + } + } + + // Drop lowest scores + int drop_k = static_cast(pose1.rows * 0.2); + std::sort(scores.begin(), scores.end()); + + // Calculate final score + double score = 0.0; + for (int i = drop_k; i < scores.rows; i++) + { + score += scores.at(i); + } + score /= (pose1.rows - drop_k); + + return std::make_pair(pose3d, score); +} + +// ================================================================================================= + +std::vector>> TriangulatorInternal::calc_grouping( + const std::vector, std::pair>> &all_pairs, + const std::vector> &all_scored_poses, + double min_score) +{ + double max_center_dist = 0.6; + double max_joint_avg_dist = 0.3; + size_t num_joints = all_scored_poses[0].first.rows; + + // Calculate pose centers + std::vector centers; + for (size_t i = 0; i < all_pairs.size(); ++i) + { + const cv::Mat &pose_3d = all_scored_poses[i].first; + cv::Point3d center(0, 0, 0); + size_t num_valid = 0; + for (size_t j = 0; j < num_joints; ++j) + { + if (pose_3d.at(j, 3) > min_score) + { + center.x += pose_3d.at(j, 0); + center.y += pose_3d.at(j, 1); + center.z += pose_3d.at(j, 2); + num_valid++; + } + } + if (num_valid > 0) + { + center.x /= num_valid; + center.y /= num_valid; + center.z /= num_valid; + } + centers.push_back(center); + } + + // Calculate Groups + // defined as a tuple of center, pose, and all-pairs-indices of members + std::vector>> groups; + for (size_t i = 0; i < all_pairs.size(); ++i) + { + const cv::Mat &pose_3d = all_scored_poses[i].first; + const cv::Point3d ¢er = centers[i]; + double best_dist = std::numeric_limits::infinity(); + int best_group = -1; + + for (size_t j = 0; j < groups.size(); ++j) + { + auto &group = groups[j]; + cv::Point3d &group_center = std::get<0>(group); + cv::Mat &group_pose = std::get<1>(group); + + // Check if the center is close enough + if (cv::norm(group_center - center) < max_center_dist) + { + // Calculate average joint distance + std::vector dists; + for (size_t row = 0; row < num_joints; row++) + { + if (pose_3d.at(row, 3) > min_score && group_pose.at(row, 3) > min_score) + { + cv::Point3d p1(pose_3d.at(row, 0), pose_3d.at(row, 1), pose_3d.at(row, 2)); + cv::Point3d p2(group_pose.at(row, 0), group_pose.at(row, 1), group_pose.at(row, 2)); + dists.push_back(cv::norm(p1 - p2)); + } + } + double dist = std::numeric_limits::infinity(); + if (!dists.empty()) + { + dist = std::accumulate(dists.begin(), dists.end(), 0.0) / dists.size(); + } + + // Check if the average joint distance is close enough + if (dist < max_joint_avg_dist && dist < best_dist) + { + best_dist = dist; + best_group = j; + } + } + } + + if (best_group == -1) + { + // Create a new group + std::vector new_indices{static_cast(i)}; + groups.emplace_back(center, pose_3d.clone(), new_indices); + } + else + { + // Update existing group + auto &group = groups[best_group]; + cv::Point3d &group_center = std::get<0>(group); + cv::Mat &group_pose = std::get<1>(group); + std::vector &group_indices = std::get<2>(group); + + double n_elems = group_indices.size(); + group_center = (group_center * n_elems + center) / (n_elems + 1); + + cv::Mat new_pose = group_pose.clone(); + for (size_t row = 0; row < num_joints; row++) + { + + new_pose.at(row, 0) = (group_pose.at(row, 0) * n_elems + pose_3d.at(row, 0)) / (n_elems + 1); + new_pose.at(row, 1) = (group_pose.at(row, 1) * n_elems + pose_3d.at(row, 1)) / (n_elems + 1); + new_pose.at(row, 2) = (group_pose.at(row, 2) * n_elems + pose_3d.at(row, 2)) / (n_elems + 1); + new_pose.at(row, 3) = (group_pose.at(row, 3) * n_elems + pose_3d.at(row, 3)) / (n_elems + 1); + } + group_pose = new_pose; + group_indices.push_back(i); + } + } + return groups; +} + +// ================================================================================================= + +cv::Mat TriangulatorInternal::merge_group(const std::vector &poses_3d, double min_score) +{ + int num_poses = poses_3d.size(); + int num_joints = poses_3d[0].rows; + + // Merge poses to create initial pose + // Use only those triangulations with a high score + cv::Mat sum_poses = cv::Mat::zeros(poses_3d[0].size(), poses_3d[0].type()); + cv::Mat sum_mask = cv::Mat::zeros(poses_3d[0].size(), CV_32S); + for (const auto &pose : poses_3d) + { + for (int j = 0; j < num_joints; ++j) + { + if (pose.at(j, 3) > min_score) + { + sum_poses.row(j) += pose.row(j); + sum_mask.at(j, 3) += 1; + } + } + } + cv::Mat initial_pose_3d = cv::Mat::zeros(sum_poses.size(), sum_poses.type()); + for (int j = 0; j < num_joints; ++j) + { + if (sum_mask.at(j, 3) > 0) + { + initial_pose_3d.row(j) = sum_poses.row(j) / sum_mask.at(j, 3); + } + } + + // Use center as default if the initial pose is empty + cv::Mat jmask = cv::Mat::zeros(num_joints, 1, CV_8U); + cv::Point3d center(0, 0, 0); + int valid_joints = 0; + for (int j = 0; j < num_joints; ++j) + { + if (initial_pose_3d.at(j, 3) > 0.0) + { + jmask.at(j) = 1; + center += cv::Point3d(initial_pose_3d.at(j, 0), initial_pose_3d.at(j, 1), initial_pose_3d.at(j, 2)); + valid_joints++; + } + } + if (valid_joints > 0) + { + center *= 1.0 / valid_joints; + } + for (int j = 0; j < num_joints; ++j) + { + if (jmask.at(j) == 0) + { + initial_pose_3d.at(j, 0) = center.x; + initial_pose_3d.at(j, 1) = center.y; + initial_pose_3d.at(j, 2) = center.z; + } + } + + // Drop joints with low scores and filter outlying joints using distance threshold + double offset = 0.1; + double max_dist = 1.2; + cv::Mat mask = cv::Mat::zeros(num_poses, num_joints, CV_8U); + cv::Mat distances = cv::Mat::zeros(num_poses, num_joints, CV_64F); + for (int i = 0; i < num_poses; ++i) + { + for (int j = 0; j < num_joints; ++j) + { + cv::Point3d joint_i(poses_3d[i].at(j, 0), poses_3d[i].at(j, 1), poses_3d[i].at(j, 2)); + cv::Point3d joint_initial(initial_pose_3d.at(j, 0), initial_pose_3d.at(j, 1), initial_pose_3d.at(j, 2)); + double distance = cv::norm(joint_i - joint_initial); + distances.at(i, j) = distance; + if (distance <= max_dist && poses_3d[i].at(j, 3) > (min_score - offset)) + { + mask.at(i, j) = 1; + } + } + } + + // Select the best-k proposals for each joint that are closest to the initial pose + int keep_best = 3; + cv::Mat best_k_mask = cv::Mat::zeros(num_poses, num_joints, CV_8U); + for (int j = 0; j < num_joints; ++j) + { + std::vector> valid_indices; + for (int i = 0; i < num_poses; ++i) + { + if (mask.at(i, j)) + { + valid_indices.push_back({distances.at(i, j), i}); + } + } + std::sort(valid_indices.begin(), valid_indices.end()); + for (int k = 0; k < std::min(keep_best, static_cast(valid_indices.size())); ++k) + { + best_k_mask.at(valid_indices[k].second, j) = 1; + } + } + + // Combine masks + mask = mask & best_k_mask; + + // Compute the final pose + sum_poses = cv::Mat::zeros(sum_poses.size(), sum_poses.type()); + sum_mask = cv::Mat::zeros(sum_mask.size(), CV_32S); + for (int i = 0; i < num_poses; ++i) + { + for (int j = 0; j < num_joints; ++j) + { + if (mask.at(i, j)) + { + sum_poses.row(j) += poses_3d[i].row(j); + sum_mask.at(j, 3) += 1; + } + } + } + cv::Mat final_pose_3d = cv::Mat::zeros(sum_poses.size(), sum_poses.type()); + for (int j = 0; j < num_joints; ++j) + { + if (sum_mask.at(j, 3) > 0) + { + final_pose_3d.row(j) = sum_poses.row(j) / sum_mask.at(j, 3); + } + } + + return final_pose_3d; +} diff --git a/spt/triangulator.hpp b/spt/triangulator.hpp new file mode 100644 index 0000000..3437518 --- /dev/null +++ b/spt/triangulator.hpp @@ -0,0 +1,89 @@ +#pragma once + +#include +#include +#include +#include + +#include + +#include "camera.hpp" + +// ================================================================================================= + +class CameraInternal +{ +public: + CameraInternal(const Camera &cam); + + Camera cam; + cv::Mat K; + cv::Mat DC; + cv::Mat R; + cv::Mat T; + cv::Mat P; + + void update_projection_matrix(); +}; + +// ================================================================================================= + +class TriangulatorInternal +{ +public: + TriangulatorInternal(double min_score); + + std::vector>> triangulate_poses( + const std::vector>>> &poses_2d, + const std::vector &cameras, + const std::array, 2> &roomparams, + const std::vector &joint_names); + + void reset(); + +private: + double min_score; + const std::vector core_joints = { + "shoulder_left", + "shoulder_right", + "hip_left", + "hip_right", + "elbow_left", + "elbow_right", + "knee_left", + "knee_right", + "wrist_left", + "wrist_right", + "ankle_left", + "ankle_right", + }; + + std::vector last_poses_3d; + + void undistort_points(cv::Mat &points, CameraInternal &icam); + void undistort_poses(std::vector &poses, CameraInternal &icam); + + std::tuple, std::vector> project_poses( + const std::vector &bodies3D, const CameraInternal &icam, bool calc_dists); + + cv::Mat score_projection( + const cv::Mat &pose1, + const cv::Mat &repro1, + const cv::Mat &dists1, + const cv::Mat &mask, + double iscale); + + std::pair triangulate_and_score( + const cv::Mat &pose1, + const cv::Mat &pose2, + const CameraInternal &cam1, + const CameraInternal &cam2, + const std::array, 2> &roomparams); + + std::vector>> calc_grouping( + const std::vector, std::pair>> &all_pairs, + const std::vector> &all_scored_poses, + double min_score); + + cv::Mat merge_group(const std::vector &poses_3d, double min_score); +}; diff --git a/swig/Makefile b/swig/Makefile new file mode 100644 index 0000000..8ac0b95 --- /dev/null +++ b/swig/Makefile @@ -0,0 +1,13 @@ +# Standard compile options for the c++ executable +FLAGS = -fPIC + +# The python interface through swig +PYTHONI = -I/usr/include/python3.8/ +PYTHONL = -Xlinker -export-dynamic + +# Default super-target +all: + cd ../spt/ && g++ -fPIC -std=c++2a -Wall -I/usr/include/opencv4 -c *.cpp ; cd ../swig/ + swig -c++ -python -keyword -o spt_wrap.cxx spt.i + g++ $(FLAGS) $(PYTHONI) -I/usr/include/opencv4 -c spt_wrap.cxx -o spt_wrap.o + g++ $(PYTHONL) $(LIBFLAGS) -shared ../spt/*.o spt_wrap.o -lopencv_core -lopencv_imgproc -lopencv_calib3d -o _spt.so diff --git a/swig/spt.i b/swig/spt.i new file mode 100644 index 0000000..85d51a7 --- /dev/null +++ b/swig/spt.i @@ -0,0 +1,51 @@ +%module spt +%{ + // Includes the header in the wrapper code + #include "../spt/camera.hpp" + #include "../spt/interface.hpp" +%} + +// Some modules need extra imports beside the main .hpp file +%include "std_array.i" +%include "std_string.i" +%include "std_vector.i" + +// Instantiate templates used by example +// If the template is too nested (>2), parts of it need to be declared as well +namespace std { + %template(DoubleMatrix_3x3) array, 3>; + %template(VectorDouble) vector; + %template(DoubleMatrix_3x1) array, 3>; + %template(DoubleMatrix_3x4) array, 4>; + %template(Matrix_Jx4) vector>; + %template(Matrix_NxJx4) vector>>; + %template(Matrix_Jx3) vector>; + %template(Matrix_VxNxJx3) vector>>>; + %template(VectorCamera) vector; + %template(DoubleMatrix_2x3) array, 2>; + %template(VectorString) vector; +} + +// Convert vector to native (python) list +%naturalvar Camera::K; +%naturalvar Camera::DC; +%naturalvar Camera::R; +%naturalvar Camera::T; + +// Improve printing of result objects +%extend Camera { + std::string __str__() const { + return $self->to_string(); + } +} + +// Ignore: Warning 503: Can't wrap 'operator <<' unless renamed to a valid identifier. +%warnfilter(503) Camera; + +// Ignore: Warning 511: Can't use keyword arguments with overloaded functions. +// The warning is cause by enabling keyword arguments, which doesn't work for vectors. +#pragma SWIG nowarn=511 + +// Parse the header file to generate wrappers +%include "../spt/camera.hpp" +%include "../spt/interface.hpp" diff --git a/tests/poses_h1.json b/tests/poses_h1.json new file mode 100644 index 0000000..7c56154 --- /dev/null +++ b/tests/poses_h1.json @@ -0,0 +1,544 @@ +{ + "2D": [ + [ + [ + [ + 445.214, + 373.348, + 0.805 + ], + [ + 446.448, + 367.181, + 0.834 + ], + [ + 446.859, + 370.881, + 0.447 + ], + [ + 456.726, + 365.947, + 0.974 + ], + [ + 484.272, + 374.17, + 0.745 + ], + [ + 445.625, + 385.271, + 0.611 + ], + [ + 505.24, + 409.528, + 0.761 + ], + [ + 393.411, + 385.682, + 0.486 + ], + [ + 484.683, + 447.352, + 0.477 + ], + [ + 369.976, + 385.271, + 0.672 + ], + [ + 452.203, + 456.808, + 0.292 + ], + [ + 442.336, + 503.677, + 0.504 + ], + [ + 478.927, + 502.444, + 0.505 + ], + [ + 414.79, + 508.2, + 0.462 + ], + [ + 428.769, + 476.131, + 0.332 + ], + [ + 423.835, + 576.037, + 0.689 + ], + [ + 379.433, + 497.099, + 0.485 + ], + [ + 460.632, + 503.061, + 0.504 + ], + [ + 475.433, + 397.399, + 0.611 + ], + [ + 470.499, + 370.059, + 0.745 + ] + ] + ], + [ + [ + [ + 502.664, + 372.786, + 0.81 + ], + [ + 503.643, + 363.001, + 0.894 + ], + [ + 492.39, + 368.383, + 0.862 + ], + [ + 508.535, + 351.26, + 0.85 + ], + [ + 479.181, + 367.405, + 0.837 + ], + [ + 536.422, + 367.894, + 0.721 + ], + [ + 476.245, + 392.356, + 0.785 + ], + [ + 584.367, + 370.829, + 0.867 + ], + [ + 464.993, + 441.279, + 0.717 + ], + [ + 560.883, + 385.017, + 0.72 + ], + [ + 470.864, + 392.356, + 0.687 + ], + [ + 542.292, + 469.166, + 0.674 + ], + [ + 503.153, + 467.698, + 0.639 + ], + [ + 502.664, + 506.837, + 0.765 + ], + [ + 458.633, + 454.489, + 0.798 + ], + [ + 471.842, + 589.518, + 0.813 + ], + [ + 530.551, + 499.988, + 0.71 + ], + [ + 522.723, + 468.432, + 0.639 + ], + [ + 506.333, + 380.125, + 0.721 + ], + [ + 493.858, + 359.332, + 0.837 + ] + ] + ], + [ + [ + [ + 635.959, + 325.896, + 0.26 + ], + [ + 603.03, + 315.205, + 0.37 + ], + [ + 647.506, + 325.896, + 0.366 + ], + [ + 611.155, + 316.488, + 0.826 + ], + [ + 641.519, + 327.607, + 0.878 + ], + [ + 577.37, + 334.877, + 0.652 + ], + [ + 652.638, + 364.385, + 0.766 + ], + [ + 532.466, + 334.877, + 0.77 + ], + [ + 650.5, + 405.44, + 0.57 + ], + [ + 569.672, + 337.015, + 0.406 + ], + [ + 631.255, + 375.504, + 0.375 + ], + [ + 573.521, + 461.463, + 0.659 + ], + [ + 617.57, + 458.897, + 0.573 + ], + [ + 614.576, + 471.727, + 0.287 + ], + [ + 656.487, + 410.145, + 0.653 + ], + [ + 643.657, + 520.908, + 0.599 + ], + [ + 631.255, + 512.782, + 0.262 + ], + [ + 595.546, + 460.18, + 0.573 + ], + [ + 615.004, + 349.631, + 0.652 + ], + [ + 626.337, + 322.047, + 0.826 + ] + ] + ], + [ + [ + [ + 512.076, + 343.77, + 0.704 + ], + [ + 518.561, + 335.34, + 0.855 + ], + [ + 502.349, + 336.637, + 0.799 + ], + [ + 523.1, + 323.667, + 0.76 + ], + [ + 480.3, + 324.964, + 0.887 + ], + [ + 534.773, + 328.855, + 0.743 + ], + [ + 446.579, + 361.928, + 0.849 + ], + [ + 592.488, + 330.152, + 0.802 + ], + [ + 479.652, + 423.534, + 0.795 + ], + [ + 621.022, + 343.77, + 0.904 + ], + [ + 491.324, + 362.576, + 0.811 + ], + [ + 523.1, + 450.77, + 0.671 + ], + [ + 472.518, + 461.146, + 0.7 + ], + [ + 549.688, + 510.431, + 0.776 + ], + [ + 491.324, + 448.825, + 0.757 + ], + [ + 528.937, + 624.565, + 0.855 + ], + [ + 587.949, + 496.813, + 0.741 + ], + [ + 497.809, + 455.958, + 0.671 + ], + [ + 490.676, + 345.391, + 0.743 + ], + [ + 501.7, + 324.316, + 0.76 + ] + ] + ] + ], + "3D": [ + [ + [ + -0.04, + -0.124, + 1.088, + 0.943 + ], + [ + -0.0, + -0.069, + 1.127, + 0.92 + ], + [ + -0.082, + -0.15, + 1.106, + 0.964 + ], + [ + -0.005, + -0.051, + 1.159, + 0.952 + ], + [ + -0.148, + -0.059, + 1.111, + 0.983 + ], + [ + 0.115, + 0.064, + 1.093, + 0.981 + ], + [ + -0.234, + 0.076, + 0.968, + 0.98 + ], + [ + 0.354, + 0.051, + 1.09, + 0.988 + ], + [ + -0.189, + -0.105, + 0.754, + 0.964 + ], + [ + 0.315, + -0.244, + 1.063, + 0.969 + ], + [ + -0.151, + -0.009, + 0.955, + 0.925 + ], + [ + 0.121, + 0.2, + 0.563, + 0.952 + ], + [ + -0.081, + 0.16, + 0.547, + 0.967 + ], + [ + 0.071, + -0.242, + 0.451, + 0.955 + ], + [ + -0.172, + -0.237, + 0.703, + 0.993 + ], + [ + -0.047, + -0.396, + 0.093, + 0.985 + ], + [ + 0.224, + -0.293, + 0.521, + 0.972 + ], + [ + 0.026, + 0.185, + 0.55, + 0.95 + ], + [ + -0.062, + 0.085, + 1.026, + 0.991 + ], + [ + -0.077, + -0.051, + 1.134, + 0.971 + ] + ] + ] +} \ No newline at end of file diff --git a/tests/test_interface.py b/tests/test_interface.py new file mode 100644 index 0000000..0dc0112 --- /dev/null +++ b/tests/test_interface.py @@ -0,0 +1,85 @@ +import json +import sys +import time + +import numpy as np + +sys.path.append("../swig/") +import spt + +# ================================================================================================== + + +def main(): + print("") + + # Test camera structure + camera = spt.Camera() + camera.name = "Camera 1" + camera.K = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] + camera.DC = [0, 0, 0, 0, 0] + camera.R = [[1, 0, 0], [0, 1, 0], [0, 0, 1]] + camera.T = [[1], [2], [3]] + camera.width = 640 + camera.height = 480 + print(camera) + print("") + + # Load input data + roomparams = [[0, 0, 1], [4, 4, 2]] + joint_names = [ + "nose", + "eye_left", + "eye_right", + "ear_left", + "ear_right", + "shoulder_left", + "shoulder_right", + "elbow_left", + "elbow_right", + "wrist_left", + "wrist_right", + "hip_left", + "hip_right", + "knee_left", + "knee_right", + "ankle_left", + "ankle_right", + "hip_middle", + "shoulder_middle", + "head", + ] + cpath = "/SimplePoseTriangulation/data/h1/sample.json" + ppath = "/SimplePoseTriangulation/tests/poses_h1.json" + with open(cpath, "r") as file: + cdata = json.load(file) + with open(ppath, "r") as file: + pdata = json.load(file) + cams = cdata["cameras"] + poses_2d = pdata["2D"] + cameras = [] + for cam in cams: + camera = spt.Camera() + camera.name = cam["name"] + camera.K = cam["K"] + camera.DC = cam["DC"] + camera.R = cam["R"] + camera.T = cam["T"] + camera.width = cam["width"] + camera.height = cam["height"] + cameras.append(camera) + + # Run triangulation + triangulator = spt.Triangulator(min_score=0.95) + 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)) + + +# ================================================================================================== + +if __name__ == "__main__": + main()