Skip to content

Commit 3d2d6a2

Browse files
FaaizHaikalFaaizHaikal
authored
Merge pull request #2 from ichiro-its/feature/implement-ipm
[Sprint 24-25 / PD-461] - [Feature] Research on Implementation of IPM
2 parents 4aaabb6 + 8f37491 commit 3d2d6a2

File tree

10 files changed

+996
-11
lines changed

10 files changed

+996
-11
lines changed

‎CMakeLists.txt‎

Lines changed: 79 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,25 +1,94 @@
1-
cmake_minimum_required(VERSION 3.8)
1+
cmake_minimum_required(VERSION 3.5)
22
project(gyakuenki_cpp)
33

4+
if(NOT CMAKE_C_STANDARD)
5+
set(CMAKE_C_STANDARD 99)
6+
endif()
7+
8+
if(NOT CMAKE_CXX_STANDARD)
9+
set(CMAKE_CXX_STANDARD 14)
10+
endif()
11+
12+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
13+
add_compile_options(-Wall -Wextra -Wpedantic)
14+
endif()
15+
416
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
517
add_compile_options(-Wall -Wextra -Wpedantic)
618
endif()
719

8-
# find dependencies
920
find_package(ament_cmake REQUIRED)
1021
find_package(rclcpp REQUIRED)
11-
find_package(std_msgs REQUIRED)
22+
find_package(tf2 REQUIRED)
23+
find_package(tf2_ros REQUIRED)
24+
find_package(tf2_geometry_msgs REQUIRED)
25+
find_package(visualization_msgs REQUIRED)
26+
find_package(geometry_msgs REQUIRED)
27+
find_package(OpenCV REQUIRED)
28+
find_package(jitsuyo REQUIRED)
29+
find_package(keisan REQUIRED)
30+
find_package(ninshiki_interfaces REQUIRED)
31+
find_package(gyakuenki_interfaces REQUIRED)
32+
33+
add_library(${PROJECT_NAME} SHARED
34+
"src/${PROJECT_NAME}/node/gyakuenki_cpp_node.cpp"
35+
"src/${PROJECT_NAME}/projections/ipm.cpp"
36+
"src/${PROJECT_NAME}/utils/camera_info.cpp")
37+
38+
ament_target_dependencies(${PROJECT_NAME}
39+
rclcpp
40+
tf2
41+
tf2_ros
42+
tf2_geometry_msgs
43+
visualization_msgs
44+
geometry_msgs
45+
OpenCV
46+
jitsuyo
47+
keisan
48+
ninshiki_interfaces
49+
gyakuenki_interfaces)
50+
51+
target_include_directories(${PROJECT_NAME} PUBLIC
52+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
53+
$<INSTALL_INTERFACE:include>)
54+
55+
install(DIRECTORY "include" DESTINATION ".")
56+
57+
install(TARGETS ${PROJECT_NAME}
58+
EXPORT export_${PROJECT_NAME}
59+
ARCHIVE DESTINATION "lib"
60+
LIBRARY DESTINATION "lib"
61+
RUNTIME DESTINATION "bin")
62+
63+
add_executable(main "src/gyakuenki_cpp_main.cpp")
64+
target_include_directories(main PUBLIC
65+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
66+
$<INSTALL_INTERFACE:include>)
67+
target_link_libraries(main ${PROJECT_NAME})
68+
69+
install(TARGETS
70+
main
71+
DESTINATION lib/${PROJECT_NAME})
1272

1373
if(BUILD_TESTING)
1474
find_package(ament_lint_auto REQUIRED)
15-
# the following line skips the linter which checks for copyrights
16-
# comment the line when a copyright and license is added to all source files
17-
set(ament_cmake_copyright_FOUND TRUE)
18-
# the following line skips cpplint (only works in a git repo)
19-
# comment the line when this package is in a git repo and when
20-
# a copyright and license is added to all source files
21-
set(ament_cmake_cpplint_FOUND TRUE)
2275
ament_lint_auto_find_test_dependencies()
2376
endif()
2477

78+
ament_export_dependencies(
79+
rclcpp
80+
tf2
81+
tf2_ros
82+
tf2_geometry_msgs
83+
visualization_msgs
84+
geometry_msgs
85+
OpenCV
86+
jitsuyo
87+
keisan
88+
ninshiki_interfaces
89+
gyakuenki_interfaces)
90+
91+
ament_export_include_directories("include")
92+
ament_export_libraries(${PROJECT_NAME})
93+
2594
ament_package()

‎include/gyakuenki_cpp/gyakuenki_cpp.hpp‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
// Copyright (c) 2024 Ichiro ITS
2+
//
3+
// Permission is hereby granted, free of charge, to any person obtaining a copy
4+
// of this software and associated documentation files (the "Software"), to deal
5+
// in the Software without restriction, including without limitation the rights
6+
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7+
// copies of the Software, and to permit persons to whom the Software is
8+
// furnished to do so, subject to the following conditions:
9+
//
10+
// The above copyright notice and this permission notice shall be included in
11+
// all copies or substantial portions of the Software.
12+
//
13+
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14+
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15+
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
16+
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17+
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18+
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
19+
// THE SOFTWARE.
20+
21+
#ifndef GYAKUENKI_CPP__GYAKUENKI_CPP_HPP_
22+
#define GYAKUENKI_CPP__GYAKUENKI_CPP_HPP_
23+
24+
#include "gyakuenki_cpp/node/gyakuenki_cpp_node.hpp"
25+
#include "gyakuenki_cpp/projections/ipm.hpp"
26+
#include "gyakuenki_cpp/utils/camera_info.hpp"
27+
28+
#endif // GYAKUENKI_CPP__GYAKUENKI_CPP_HPP_

‎include/gyakuenki_cpp/node/gyakuenki_cpp_node.hpp‎

Lines changed: 78 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
1+
// Copyright (c) 2024 Ichiro ITS
2+
//
3+
// Permission is hereby granted, free of charge, to any person obtaining a copy
4+
// of this software and associated documentation files (the "Software"), to deal
5+
// in the Software without restriction, including without limitation the rights
6+
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7+
// copies of the Software, and to permit persons to whom the Software is
8+
// furnished to do so, subject to the following conditions:
9+
//
10+
// The above copyright notice and this permission notice shall be included in
11+
// all copies or substantial portions of the Software.
12+
//
13+
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14+
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15+
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
16+
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17+
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18+
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
19+
// THE SOFTWARE.
20+
21+
#ifndef GYAKUENKI_CPP__NODE__GYAKUENKI_CPP_NODE_HPP_
22+
#define GYAKUENKI_CPP__NODE__GYAKUENKI_CPP_NODE_HPP_
23+
24+
#include <tf2_ros/buffer.h>
25+
#include <tf2_ros/transform_listener.h>
26+
27+
#include <memory>
28+
#include <rclcpp/rclcpp.hpp>
29+
#include <visualization_msgs/msg/marker.hpp>
30+
#include <visualization_msgs/msg/marker_array.hpp>
31+
32+
#include "gyakuenki_cpp/projections/ipm.hpp"
33+
#include "gyakuenki_interfaces/msg/projected_object.hpp"
34+
#include "gyakuenki_interfaces/msg/projected_objects.hpp"
35+
#include "gyakuenki_interfaces/srv/get_camera_offset.hpp"
36+
#include "gyakuenki_interfaces/srv/update_camera_offset.hpp"
37+
#include "ninshiki_interfaces/msg/detected_object.hpp"
38+
#include "ninshiki_interfaces/msg/detected_objects.hpp"
39+
40+
namespace gyakuenki_cpp
41+
{
42+
43+
class GyakuenkiCppNode
44+
{
45+
public:
46+
using MarkerArray = visualization_msgs::msg::MarkerArray;
47+
using Marker = visualization_msgs::msg::Marker;
48+
using DetectedObject = ninshiki_interfaces::msg::DetectedObject;
49+
using DetectedObjects = ninshiki_interfaces::msg::DetectedObjects;
50+
using ProjectedObjects = gyakuenki_interfaces::msg::ProjectedObjects;
51+
using ProjectedObject = gyakuenki_interfaces::msg::ProjectedObject;
52+
using GetCameraOffset = gyakuenki_interfaces::srv::GetCameraOffset;
53+
using UpdateCameraOffset = gyakuenki_interfaces::srv::UpdateCameraOffset;
54+
55+
void publish(const DetectedObjects::SharedPtr & message);
56+
57+
GyakuenkiCppNode(const std::shared_ptr<rclcpp::Node> & node, const std::string & path);
58+
59+
private:
60+
rclcpp::Node::SharedPtr node;
61+
std::shared_ptr<tf2_ros::Buffer> tf_buffer;
62+
std::shared_ptr<tf2_ros::TransformListener> tf_listener;
63+
std::shared_ptr<gyakuenki_cpp::IPM> ipm;
64+
65+
rclcpp::Publisher<MarkerArray>::SharedPtr markers_publisher;
66+
67+
rclcpp::Publisher<ProjectedObjects>::SharedPtr projected_objects_publisher;
68+
rclcpp::Publisher<ProjectedObject>::SharedPtr projected_ball_publisher;
69+
rclcpp::Subscription<DetectedObjects>::SharedPtr dnn_detection_subscriber;
70+
rclcpp::Subscription<DetectedObject>::SharedPtr ball_detection_subscriber;
71+
72+
rclcpp::Service<GetCameraOffset>::SharedPtr get_camera_offset_service;
73+
rclcpp::Service<UpdateCameraOffset>::SharedPtr update_camera_offset_service;
74+
};
75+
76+
} // namespace gyakuenki_cpp
77+
78+
#endif // GYAKUENKI_CPP__NODE__GYAKUENKI_CPP_NODE_HPP_

‎include/gyakuenki_cpp/projections/ipm.hpp‎

Lines changed: 99 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,99 @@
1+
// Copyright (c) 2024 Ichiro ITS
2+
//
3+
// Permission is hereby granted, free of charge, to any person obtaining a copy
4+
// of this software and associated documentation files (the "Software"), to deal
5+
// in the Software without restriction, including without limitation the rights
6+
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7+
// copies of the Software, and to permit persons to whom the Software is
8+
// furnished to do so, subject to the following conditions:
9+
//
10+
// The above copyright notice and this permission notice shall be included in
11+
// all copies or substantial portions of the Software.
12+
//
13+
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14+
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15+
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
16+
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17+
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18+
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
19+
// THE SOFTWARE.
20+
21+
#ifndef GYAKUENKI_CPP__IPM__GYAKUENKI_CPP_IPM_HPP_
22+
#define GYAKUENKI_CPP__IPM__GYAKUENKI_CPP_IPM_HPP_
23+
24+
#include <tf2/LinearMath/Quaternion.h>
25+
#include <tf2/exceptions.h>
26+
#include <tf2_ros/buffer.h>
27+
#include <tf2_ros/transform_listener.h>
28+
29+
#include <geometry_msgs/msg/quaternion.hpp>
30+
#include <memory>
31+
#include <opencv2/opencv.hpp>
32+
#include <rclcpp/rclcpp.hpp>
33+
34+
#include "gyakuenki_cpp/utils/camera_info.hpp"
35+
#include "gyakuenki_interfaces/msg/projected_object.hpp"
36+
#include "keisan/matrix.hpp"
37+
#include "ninshiki_interfaces/msg/detected_object.hpp"
38+
#include "nlohmann/json.hpp"
39+
40+
namespace gyakuenki_cpp
41+
{
42+
43+
class IPM
44+
{
45+
public:
46+
struct CameraOffset
47+
{
48+
keisan::Point3 position;
49+
keisan::Angle<double> roll;
50+
keisan::Angle<double> pitch;
51+
keisan::Angle<double> yaw;
52+
};
53+
54+
using DetectedObject = ninshiki_interfaces::msg::DetectedObject;
55+
using Quaternion = geometry_msgs::msg::Quaternion;
56+
57+
IPM(
58+
const std::shared_ptr<rclcpp::Node> & node, const std::shared_ptr<tf2_ros::Buffer> & tf_buffer,
59+
const std::shared_ptr<tf2_ros::TransformListener> & tf_listener, const std::string & path);
60+
61+
std::string config_path;
62+
void load_config(const std::string & path);
63+
void set_config(double x, double y, double z, double roll, double pitch, double yaw);
64+
void save_config();
65+
66+
bool object_at_bottom_of_image(const DetectedObject & detected_object);
67+
void normalize_pixel(cv::Point2d & pixel);
68+
void undistort_pixel(cv::Point2d & pixel);
69+
70+
Quaternion tf2_to_msg(const tf2::Quaternion & tf2_quat);
71+
tf2::Quaternion msg_to_tf2(const Quaternion & msg_quat);
72+
keisan::Matrix<4, 4> quat_to_rotation_matrix(const Quaternion & q);
73+
74+
keisan::Matrix<4, 1> point_in_camera_frame(
75+
const cv::Point2d & pixel, const keisan::Matrix<4, 4> & T, const keisan::Matrix<4, 4> & R,
76+
const std::string & object_label);
77+
78+
cv::Point2d get_target_pixel(const DetectedObject & detected_object);
79+
cv::Point2d get_normalized_target_pixel(const DetectedObject & detected_object);
80+
81+
gyakuenki_interfaces::msg::ProjectedObject map_object(
82+
const DetectedObject & detected_object, const std::string & output_frame,
83+
keisan::Matrix<4, 1> & Pc);
84+
85+
const CameraOffset & get_camera_offset() const { return camera_offset; }
86+
87+
private:
88+
rclcpp::Node::SharedPtr node;
89+
std::shared_ptr<tf2_ros::Buffer> tf_buffer;
90+
std::shared_ptr<tf2_ros::TransformListener> tf_listener;
91+
92+
utils::CameraInfo camera_info;
93+
CameraOffset camera_offset;
94+
tf2::Quaternion rotation_offset;
95+
};
96+
97+
} // namespace gyakuenki_cpp
98+
99+
#endif // GYAKUENKI_CPP__IPM__GYAKUENKI_CPP_IPM_HPP_

‎include/gyakuenki_cpp/utils/camera_info.hpp‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
// Copyright (c) 2024 Ichiro ITS
2+
//
3+
// Permission is hereby granted, free of charge, to any person obtaining a copy
4+
// of this software and associated documentation files (the "Software"), to deal
5+
// in the Software without restriction, including without limitation the rights
6+
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7+
// copies of the Software, and to permit persons to whom the Software is
8+
// furnished to do so, subject to the following conditions:
9+
//
10+
// The above copyright notice and this permission notice shall be included in
11+
// all copies or substantial portions of the Software.
12+
//
13+
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14+
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15+
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
16+
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17+
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18+
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
19+
// THE SOFTWARE.
20+
21+
#ifndef GYAKUENKI_CPP__UTILS__CAMERA_INFO_HPP_
22+
#define GYAKUENKI_CPP__UTILS__CAMERA_INFO_HPP_
23+
24+
#include <opencv2/opencv.hpp>
25+
#include <string>
26+
#include <vector>
27+
28+
namespace gyakuenki_cpp::utils
29+
{
30+
31+
struct CameraInfo
32+
{
33+
std::string frame_id; // Camera frame id
34+
double fx; // Focal length x
35+
double fy; // Focal length y
36+
double cx; // Principal point x
37+
double cy; // Principal point y
38+
std::vector<double> D; // Distortion coefficients
39+
int image_width; // Image width
40+
int image_height; // Image height
41+
bool use_distortion; // Use distortion coefficients
42+
43+
CameraInfo();
44+
void load_configuration(const std::string & config_path);
45+
};
46+
47+
} // namespace gyakuenki_cpp::utils
48+
49+
#endif // GYAKUENKI_CPP__UTILS__CAMERA_INFO_HPP_

‎package.xml‎

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,16 @@
1010
<buildtool_depend>ament_cmake</buildtool_depend>
1111

1212
<depend>rclcpp</depend>
13-
<depend>std_msgs</depend>
13+
<depend>tf2</depend>
14+
<depend>tf2_ros</depend>
15+
<depend>tf2_geometry_msgs</depend>
16+
<depend>visualization_msgs</depend>
17+
<depend>geometry_msgs</depend>
18+
<depend>libopencv-dev</depend>
19+
<depend>jitsuyo</depend>
20+
<depend>keisan</depend>
21+
<depend>ninshiki_interfaces</depend>
22+
<depend>gyakuenki_interfaces</depend>
1423

1524
<test_depend>ament_lint_auto</test_depend>
1625
<test_depend>ament_lint_common</test_depend>

0 commit comments

Comments
 (0)