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

Drop rotation option again.

Browse Source
This commit is contained in:
Daniel
2025-02-20 18:47:16 +01:00
parent 2f16179850
commit 63e00e9b13
5 changed files with 4 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files

83
extras/ros/rpt2d_wrapper_cpp/src/rpt2d_wrapper.cpp
View File

@ -1,8 +1,6 @@
#include <atomic>
#include <chrono>
#include <cmath>
#include <iostream>
#include <memory>
#include <string>
#include <vector>
@ -43,7 +41,7 @@ static const std::map<std::string, bool> whole_body = {
class Rpt2DWrapperNode : public rclcpp::Node
{
public:
Rpt2DWrapperNode(const std::string &cam_id, const std::string &rotate)
Rpt2DWrapperNode(const std::string &cam_id)
: Node("rpt2d_wrapper_" + cam_id)
{
this->is_busy = false;
@ -52,14 +50,6 @@ public:
std::string pose_topic = std::string(pose_out_topic)
.replace(pose_out_topic.find("{}"), 2, cam_id);
// Check rotation
if (rotate != "0" && rotate != "90" && rotate != "180" && rotate != "-90")
{
RCLCPP_ERROR(this->get_logger(), "Invalid rotation value: %s", rotate.c_str());
return;
}
this->rotate_deg = std::stoi(rotate);
// QoS
rclcpp::QoS qos_profile(1);
qos_profile.reliable();
@ -85,7 +75,6 @@ private:
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
rclcpp::Publisher<rpt_msgs::msg::Poses>::SharedPtr pose_pub_;
std::atomic<bool> is_busy;
int rotate_deg;
// Pose model pointer
std::unique_ptr<utils_2d_pose::PosePredictor> kpt_model;
@ -197,24 +186,6 @@ std::vector<std::vector<std::array<float, 3>>> Rpt2DWrapperNode::call_model(cons
cv::Mat rgb_image = utils_pipeline::bayer2rgb(image);
std::vector<cv::Mat> images_2d = {rgb_image};
// Optionally rotate image
if (this->rotate_deg == 0)
{
// pass
}
else if (this->rotate_deg == 90)
{
cv::rotate(images_2d[0], images_2d[0], cv::ROTATE_90_CLOCKWISE);
}
else if (this->rotate_deg == 180)
{
cv::rotate(images_2d[0], images_2d[0], cv::ROTATE_180);
}
else if (this->rotate_deg == -90)
{
cv::rotate(images_2d[0], images_2d[0], cv::ROTATE_90_COUNTERCLOCKWISE);
}
// Predict 2D poses
auto poses_2d_all = kpt_model->predict(images_2d);
auto poses_2d_upd = utils_pipeline::update_keypoints(
@ -247,54 +218,6 @@ std::vector<std::vector<std::array<float, 3>>> Rpt2DWrapperNode::call_model(cons
}
}
// Rotate poses back
float img_width = image.cols;
float img_height = image.rows;
if (this->rotate_deg == 0)
{
// pass
}
else if (this->rotate_deg == 90)
{
for (auto &person : valid_poses)
{
for (auto &kp : person)
{
float x = kp[0];
float y = kp[1];
kp[0] = y;
kp[1] = img_height - 1 - x;
}
}
}
else if (this->rotate_deg == 180)
{
for (auto &person : valid_poses)
{
for (auto &kp : person)
{
float x = kp[0];
float y = kp[1];
kp[0] = img_width - 1 - x;
kp[1] = img_height - 1 - y;
}
}
}
else if (this->rotate_deg == -90)
{
for (auto &person : valid_poses)
{
for (auto &kp : person)
{
float x = kp[0];
float y = kp[1];
kp[0] = img_width - 1 - y;
kp[1] = x;
}
}
}
return valid_poses;
}
@ -304,11 +227,9 @@ std::vector<std::vector<std::array<float, 3>>> Rpt2DWrapperNode::call_model(cons
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
const std::string cam_id = std::getenv("CAMID");
const std::string rotate = std::getenv("ROTATE") ? std::getenv("ROTATE") : "0";
auto node = std::make_shared<Rpt2DWrapperNode>(cam_id, rotate);
auto node = std::make_shared<Rpt2DWrapperNode>(cam_id);
rclcpp::spin(node);
rclcpp::shutdown();