├── .github
    └── ISSUE_TEMPLATE
    │   ├── custom.md
    │   ├── feature_request.md
    │   └── bug_report.md
├── cyberdog_tracking
    ├── package.xml
    ├── src
    │   ├── main.cpp
    │   ├── distance_filter.cpp
    │   └── object_tracking.cpp
    ├── include
    │   └── cyberdog_tracking
    │   │   ├── distance_filter.hpp
    │   │   ├── common_type.hpp
    │   │   └── object_tracking.hpp
    ├── launch
    │   └── launch.py
    ├── CMakeLists.txt
    └── config
    │   └── params_intrinsic.yaml
└── README.md


/.github/ISSUE_TEMPLATE/custom.md:
--------------------------------------------------------------------------------
 1 | ---
 2 | name: Custom issue template
 3 | about: Describe this issue template's purpose here.
 4 | title: ''
 5 | labels: ''
 6 | assignees: ''
 7 | 
 8 | ---
 9 | 
10 | 
11 | 


--------------------------------------------------------------------------------
/.github/ISSUE_TEMPLATE/feature_request.md:
--------------------------------------------------------------------------------
 1 | ---
 2 | name: Feature request
 3 | about: Suggest an idea for this project
 4 | title: ''
 5 | labels: ''
 6 | assignees: ''
 7 | 
 8 | ---
 9 | 
10 | **Is your feature request related to a problem? Please describe.**
11 | A clear and concise description of what the problem is. Ex. I'm always frustrated when [...]
12 | 
13 | **Describe the solution you'd like**
14 | A clear and concise description of what you want to happen.
15 | 
16 | **Describe alternatives you've considered**
17 | A clear and concise description of any alternative solutions or features you've considered.
18 | 
19 | **Additional context**
20 | Add any other context or screenshots about the feature request here.
21 | 


--------------------------------------------------------------------------------
/.github/ISSUE_TEMPLATE/bug_report.md:
--------------------------------------------------------------------------------
 1 | ---
 2 | name: Bug report
 3 | about: Create a report to help us improve
 4 | title: ''
 5 | labels: ''
 6 | assignees: ''
 7 | 
 8 | ---
 9 | 
10 | **Describe the bug**
11 | A clear and concise description of what the bug is.
12 | 
13 | **To Reproduce**
14 | Steps to reproduce the behavior:
15 | 1. Go to '...'
16 | 2. Click on '....'
17 | 3. Scroll down to '....'
18 | 4. See error
19 | 
20 | **Expected behavior**
21 | A clear and concise description of what you expected to happen.
22 | 
23 | **Screenshots**
24 | If applicable, add screenshots to help explain your problem.
25 | 
26 | **Desktop (please complete the following information):**
27 |  - OS: [e.g. iOS]
28 |  - Browser [e.g. chrome, safari]
29 |  - Version [e.g. 22]
30 | 
31 | **Smartphone (please complete the following information):**
32 |  - Device: [e.g. iPhone6]
33 |  - OS: [e.g. iOS8.1]
34 |  - Browser [e.g. stock browser, safari]
35 |  - Version [e.g. 22]
36 | 
37 | **Additional context**
38 | Add any other context about the problem here.
39 | 


--------------------------------------------------------------------------------
/cyberdog_tracking/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 3 | <package format="3">
 4 |   <name>cyberdog_tracking</name>
 5 |   <version>0.0.0</version>
 6 |   <description>TODO: Package description</description>
 7 |   <maintainer email="liufeifei6@xiaomi.com">lff</maintainer>
 8 |   <license>Apache License, Version 2.0</license>
 9 | 
10 |   <buildtool_depend>ament_cmake</buildtool_depend>
11 | 
12 |   <depend>rclcpp</depend>
13 |   <depend>sensor_msgs</depend>
14 |   <depend>std_msgs</depend>
15 |   <depend>geometry_msgs</depend>
16 |   <depend>cv_bridge</depend>
17 |   <depend>tf2_ros</depend>
18 |   <depend>rclcpp_lifecycle</depend>
19 |   <depend>protocol</depend>
20 |   <depend>yaml-cpp</depend>
21 |   <depend>cyberdog_common</depend>
22 | 
23 |   <test_depend>ament_lint_auto</test_depend>
24 |   <test_depend>ament_lint_common</test_depend>
25 | 
26 |   <export>
27 |     <build_type>ament_cmake</build_type>
28 |   </export>
29 | </package>
30 | 


--------------------------------------------------------------------------------
/cyberdog_tracking/src/main.cpp:
--------------------------------------------------------------------------------
 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved.
 2 | //
 3 | // Licensed under the Apache License, Version 2.0 (the "License");
 4 | // you may not use this file except in compliance with the License.
 5 | // You may obtain a copy of the License at
 6 | //
 7 | //     http://www.apache.org/licenses/LICENSE-2.0
 8 | //
 9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 | 
15 | #include <memory>
16 | 
17 | #include "cyberdog_tracking/object_tracking.hpp"
18 | #include "cyberdog_common/cyberdog_log.hpp"
19 | 
20 | int main(int argc, char * argv[])
21 | {
22 |   LOGGER_MAIN_INSTANCE("cyberdog_tracking");
23 |   rclcpp::init(argc, argv);
24 |   auto node = std::make_shared<cyberdog_tracking::ObjectTracking>();
25 |   rclcpp::executors::MultiThreadedExecutor exec_;
26 |   exec_.add_node(node->get_node_base_interface());
27 |   exec_.spin();
28 |   rclcpp::shutdown();
29 | 
30 |   return 0;
31 | }
32 | 


--------------------------------------------------------------------------------
/cyberdog_tracking/include/cyberdog_tracking/distance_filter.hpp:
--------------------------------------------------------------------------------
 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved.
 2 | //
 3 | // Licensed under the Apache License, Version 2.0 (the "License");
 4 | // you may not use this file except in compliance with the License.
 5 | // You may obtain a copy of the License at
 6 | //
 7 | //     http://www.apache.org/licenses/LICENSE-2.0
 8 | //
 9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 | 
15 | #ifndef CYBERDOG_TRACKING__DISTANCE_FILTER_HPP_
16 | #define CYBERDOG_TRACKING__DISTANCE_FILTER_HPP_
17 | 
18 | #include "opencv2/video/tracking.hpp"
19 | 
20 | namespace cyberdog_tracking
21 | {
22 | 
23 | class DistanceFilter
24 | {
25 | public:
26 |   DistanceFilter();
27 |   ~DistanceFilter();
28 | 
29 |   void Init(const cv::Point3f & pose);
30 |   cv::Point3f Predict(const float & delta);
31 |   cv::Point3f Correct(const cv::Point3f & pose);
32 | 
33 |   bool initialized_;
34 |   cv::KalmanFilter filter_;
35 | 
36 | private:
37 |   void Init();
38 |   void SetInterval(const float & delta);
39 | 
40 |   int state_num_;
41 |   int measure_num_;
42 | 
43 |   cv::Mat state_;
44 |   cv::Mat process_noise_;
45 |   cv::Mat measurement_;
46 | };
47 | 
48 | }  // namespace cyberdog_tracking
49 | 
50 | #endif  // CYBERDOG_TRACKING__DISTANCE_FILTER_HPP_
51 | 


--------------------------------------------------------------------------------
/cyberdog_tracking/launch/launch.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/python3
 2 | #
 3 | # Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved.
 4 | #
 5 | # Licensed under the Apache License, Version 2.0 (the "License");
 6 | # you may not use this file except in compliance with the License.
 7 | # You may obtain a copy of the License at
 8 | #
 9 | #     http://www.apache.org/licenses/LICENSE-2.0
10 | #
11 | # Unless required by applicable law or agreed to in writing, software
12 | # distributed under the License is distributed on an "AS IS" BASIS,
13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 | # See the License for the specific language governing permissions and
15 | # limitations under the License.
16 | 
17 | import os
18 | 
19 | from ament_index_python.packages import get_package_share_directory
20 | from launch import LaunchDescription
21 | from launch.substitutions import LaunchConfiguration
22 | from launch_ros.actions import Node
23 | 
24 | 
25 | def generate_launch_description():
26 |     param_path = os.path.join(get_package_share_directory('cyberdog_tracking'), 'config')
27 | 
28 |     namespace = LaunchConfiguration('namespace', default='')
29 |     return LaunchDescription([
30 |         Node(
31 |             package='cyberdog_tracking',
32 |             executable='cyberdog_tracking',
33 |             namespace=namespace,
34 |             name='tracking',
35 |             parameters=[{'ai_intrinsic': param_path}],
36 |             arguments=['--ros-args', '--log-level', 'INFO'],
37 |             remappings=None,
38 |             output='screen',
39 |         )
40 |     ])
41 | 


--------------------------------------------------------------------------------
/cyberdog_tracking/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.8)
 2 | project(cyberdog_tracking)
 3 | 
 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 5 |   add_compile_options(-Wall -Wextra -Wpedantic)
 6 | endif()
 7 | 
 8 | # find dependencies
 9 | # uncomment the following section in order to fill in
10 | # further dependencies manually.
11 | # find_package(<dependency> REQUIRED)
12 | find_package(ament_cmake REQUIRED)
13 | find_package(rclcpp REQUIRED)
14 | find_package(sensor_msgs REQUIRED)
15 | find_package(std_msgs REQUIRED)
16 | find_package(geometry_msgs REQUIRED)
17 | find_package(cv_bridge REQUIRED)
18 | find_package(tf2_ros REQUIRED)
19 | find_package(rclcpp_lifecycle REQUIRED)
20 | find_package(cyberdog_common REQUIRED)
21 | find_package(protocol REQUIRED)
22 | find_package(OpenCV 4 REQUIRED)
23 | find_package(yaml-cpp REQUIRED)
24 | 
25 | include_directories(${CMAKE_SOURCE_DIR}/include)
26 | 
27 | if(BUILD_TESTING)
28 |   find_package(ament_lint_auto REQUIRED)
29 |   # the following line skips the linter which checks for copyrights
30 |   # uncomment the line when a copyright and license is not present in all source files
31 |   #set(ament_cmake_copyright_FOUND TRUE)
32 |   # the following line skips cpplint (only works in a git repo)
33 |   # uncomment the line when this package is not in a git repo
34 |   #set(ament_cmake_cpplint_FOUND TRUE)
35 |   ament_lint_auto_find_test_dependencies()
36 | endif()
37 | 
38 | add_executable(cyberdog_tracking
39 |   src/main.cpp
40 |   src/distance_filter.cpp
41 |   src/object_tracking.cpp
42 | )
43 | ament_target_dependencies(cyberdog_tracking
44 |   rclcpp
45 |   sensor_msgs
46 |   std_msgs
47 |   geometry_msgs
48 |   cv_bridge
49 |   tf2_ros
50 |   rclcpp_lifecycle
51 |   cyberdog_common
52 |   protocol
53 |   OpenCV
54 | )
55 | target_link_libraries(cyberdog_tracking yaml-cpp)
56 | 
57 | install(TARGETS
58 |   cyberdog_tracking
59 |   DESTINATION lib/${PROJECT_NAME}
60 | )
61 | 
62 | install(DIRECTORY
63 |   launch
64 |   config
65 |   DESTINATION share/${PROJECT_NAME}/
66 | )
67 | 
68 | ament_package()
69 | 


--------------------------------------------------------------------------------
/cyberdog_tracking/config/params_intrinsic.yaml:
--------------------------------------------------------------------------------
 1 | cam0:
 2 |   camera_model: pinhole
 3 |   distortion_coeffs: [0.000962259382895234, -0.0013681629896331769, 0.0, -0.0003379311632667432,
 4 |     0.0004425976923226831]
 5 |   distortion_model: radtan
 6 |   intrinsics: [389.41100083499396, 389.4965157085501, 323.5695255689104, 234.38443516229873]
 7 |   resolution: [640, 480]
 8 |   rostopic: /cam0/image_raw
 9 | cam1:
10 |   camera_model: pinhole
11 |   distortion_coeffs: [0.030469243901796395, -0.03865946100199035, 0.0, -0.0008902591568414498,
12 |     -0.0006466849619902206]
13 |   distortion_model: radtan
14 |   intrinsics: [449.7381091552895, 449.36255303033175, 322.96126512743393, 241.0067470703169]
15 |   resolution: [640, 480]
16 |   rostopic: /cam1/image_raw
17 | cam2:
18 |   camera_model: pinhole
19 |   distortion_coeffs: [0.0005739394906252917, -0.0028622966352254095, 0.0, -0.00036455189092527297,
20 |     -0.0001955886911109604]
21 |   distortion_model: radtan
22 |   intrinsics: [388.0592012707454, 388.3084034679293, 322.84154996135624, 233.65233801726626]
23 |   resolution: [640, 480]
24 |   rostopic: /cam2/image_raw
25 | cam3:
26 |   camera_model: pinhole
27 |   distortion_coeffs: [0.023975130328665446, -0.042382138044280766, 0.0, 0.0033467922545996304,
28 |     -0.0006137133656299577]
29 |   distortion_model: radtan
30 |   intrinsics: [289.5161106804787, 291.2975243243869, 320.8957528102377, 240.586864355793]
31 |   resolution: [640, 480]
32 |   rostopic: /cam3/image_raw
33 | cam4:
34 |   camera_model: omni
35 |   distortion_coeffs: [-0.07549632307642228, 0.06190449326561846, 0.0, -0.0003110318298419796,
36 |     0.0005646261243863548]
37 |   distortion_model: radtan
38 |   intrinsics: [1.141926817701968, 466.6134742372605, 466.4304577043905, 249.03682499076623,
39 |     198.74611547126347]
40 |   resolution: [500, 400]
41 |   rostopic: /cam4/image_raw
42 | cam5:
43 |   camera_model: omni
44 |   distortion_coeffs: [-0.0677567993473825, 0.07732025795388184, 0.0, 0.0008445201862779853,
45 |     -0.001368149569359663]
46 |   distortion_model: radtan
47 |   intrinsics: [1.1353790526221885, 461.35448500849867, 462.107018641397, 243.6229215038181,
48 |     200.18659495153437]
49 |   resolution: [500, 400]
50 |   rostopic: /cam5/image_raw
51 | 


--------------------------------------------------------------------------------
/cyberdog_tracking/include/cyberdog_tracking/common_type.hpp:
--------------------------------------------------------------------------------
 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved.
 2 | //
 3 | // Licensed under the Apache License, Version 2.0 (the "License");
 4 | // you may not use this file except in compliance with the License.
 5 | // You may obtain a copy of the License at
 6 | //
 7 | //     http://www.apache.org/licenses/LICENSE-2.0
 8 | //
 9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 | 
15 | #ifndef CYBERDOG_TRACKING__COMMON_TYPE_HPP_
16 | #define CYBERDOG_TRACKING__COMMON_TYPE_HPP_
17 | 
18 | #include <string>
19 | #include <vector>
20 | #include <mutex>
21 | #include <thread>
22 | #include <condition_variable>
23 | 
24 | #include "opencv2/opencv.hpp"
25 | 
26 | #include "std_msgs/msg/header.hpp"
27 | #include "sensor_msgs/msg/image.hpp"
28 | #include "sensor_msgs/msg/camera_info.hpp"
29 | #include "geometry_msgs/msg/pose_stamped.hpp"
30 | #include "builtin_interfaces/msg/time.hpp"
31 | 
32 | #include "protocol/msg/body.hpp"
33 | #include "protocol/msg/body_info.hpp"
34 | #include "protocol/msg/person.hpp"
35 | #include "protocol/msg/tracking_status.hpp"
36 | #include "protocol/srv/camera_service.hpp"
37 | #include "protocol/srv/body_region.hpp"
38 | #include "protocol/srv/nav_mode.hpp"
39 | 
40 | namespace cyberdog_tracking
41 | {
42 | 
43 | using StdHeaderT = std_msgs::msg::Header;
44 | using SensorImageT = sensor_msgs::msg::Image;
45 | using SensorCameraInfoT = sensor_msgs::msg::CameraInfo;
46 | using GeometryPointT = geometry_msgs::msg::Point;
47 | using GeometryPoseT = geometry_msgs::msg::Pose;
48 | using GeometryPoseStampedT = geometry_msgs::msg::PoseStamped;
49 | using BuiltinTimeT = builtin_interfaces::msg::Time;
50 | 
51 | using BodyT = protocol::msg::Body;
52 | using BodyInfoT = protocol::msg::BodyInfo;
53 | using PersonT = protocol::msg::Person;
54 | using TrackingStatusT = protocol::msg::TrackingStatus;
55 | using BodyRegionT = protocol::srv::BodyRegion;
56 | using CameraServiceT = protocol::srv::CameraService;
57 | 
58 | struct StampedImage
59 | {
60 |   StdHeaderT header;
61 |   cv::Mat image;
62 | };
63 | 
64 | struct StampedBbox
65 | {
66 |   StdHeaderT header;
67 |   std::vector<cv::Rect> vecInfo;
68 | };
69 | 
70 | struct HandlerStruct
71 | {
72 |   std::mutex mtx;
73 |   std::condition_variable cond;
74 |   std::vector<StampedBbox> vecFrame;
75 | };
76 | 
77 | }  // namespace cyberdog_tracking
78 | 
79 | #endif  // CYBERDOG_TRACKING__COMMON_TYPE_HPP_
80 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # cyberdog_tracking
 2 | 
 3 | ## Introduction
 4 | 
 5 | Cyberdog_tracking is a package based on ROS2. This package mainly used to estimate the 3D position of the tracking target and provide target point for tracking_base module to plan the path and control the robot. The objects of position estimation include pedestrians and any regular and irregular targets selected by the user.
 6 | 
 7 | ## Dependencies
 8 | 
 9 | In addition to the basic msg type and pkgs of ros2, the external dependencies are as follows:
10 | 
11 | - cyberdog_common
12 | - protocol
13 | - OpenCV4
14 | - yaml-cpp
15 | 
16 | ## Installation
17 | 
18 | You can install this package on Linux system as follows:
19 | 
20 | - create your workspace
21 | - clone the code from git
22 | - compile and install
23 | 
24 | ```
25 | colcon build --packages-up-to cyberdog_tracking --install-base=/opt/ros2/cyberdog/ --merge-install
26 | ```
27 | 
28 | ## Usage
29 | 
30 | ```
31 | # 1. Start tracking node
32 | ros2 launch cyberdog_tracking launch.py
33 | 
34 | # 2. Configure tracking node
35 | ros2 service call /tracking/change_state lifecycle_msgs/srv/ChangeState "{transition: {id: 1}}"
36 | 
37 | # 3. Activate tracking node
38 | ros2 service call /tracking/change_state lifecycle_msgs/srv/ChangeState "{transition: {id: 3}}"
39 | 
40 | # 4. Deactivate tracking node
41 | ros2 service call /tracking/change_state lifecycle_msgs/srv/ChangeState "{transition: {id: 4}}"
42 | 
43 | # 5. Cleanup tracking node
44 | ros2 service call /tracking/change_state lifecycle_msgs/srv/ChangeState "{transition: {id: 2}}"
45 | ```
46 | 
47 | ## Node info
48 | 
49 | ```
50 | /tracking
51 |   Subscribers:
52 |     /camera/aligned_depth_to_extcolor/image_raw: sensor_msgs/msg/Image
53 |     /person: protocol/msg/Person
54 |     /parameter_events: rcl_interfaces/msg/ParameterEvent
55 |   Publishers:
56 |     /tracking/transition_event: lifecycle_msgs/msg/TransitionEvent
57 |     /tracking_pose: geometry_msgs/msg/PoseStamped
58 |     /tracking_status: protocol/msg/TrackingStatus
59 |     /parameter_events: rcl_interfaces/msg/ParameterEvent
60 |     /rosout: rcl_interfaces/msg/Log
61 |   Service Servers:
62 |     /tracking/change_state: lifecycle_msgs/srv/ChangeState
63 |     /tracking/describe_parameters: rcl_interfaces/srv/DescribeParameters
64 |     /tracking/get_available_states: lifecycle_msgs/srv/GetAvailableStates
65 |     /tracking/get_available_transitions: lifecycle_msgs/srv/GetAvailableTransitions
66 |     /tracking/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
67 |     /tracking/get_parameters: rcl_interfaces/srv/GetParameters
68 |     /tracking/get_state: lifecycle_msgs/srv/GetState
69 |     /tracking/get_transition_graph: lifecycle_msgs/srv/GetAvailableTransitions
70 |     /tracking/list_parameters: rcl_interfaces/srv/ListParameters
71 |     /tracking/set_parameters: rcl_interfaces/srv/SetParameters
72 |     /tracking/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
73 |   Service Clients:
74 | 
75 |   Action Servers:
76 | 
77 |   Action Clients:
78 | ```
79 | 


--------------------------------------------------------------------------------
/cyberdog_tracking/src/distance_filter.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved.
  2 | //
  3 | // Licensed under the Apache License, Version 2.0 (the "License");
  4 | // you may not use this file except in compliance with the License.
  5 | // You may obtain a copy of the License at
  6 | //
  7 | //     http://www.apache.org/licenses/LICENSE-2.0
  8 | //
  9 | // Unless required by applicable law or agreed to in writing, software
 10 | // distributed under the License is distributed on an "AS IS" BASIS,
 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 12 | // See the License for the specific language governing permissions and
 13 | // limitations under the License.
 14 | 
 15 | #include "cyberdog_tracking/distance_filter.hpp"
 16 | 
 17 | namespace cyberdog_tracking
 18 | {
 19 | 
 20 | cv::Point3f Convert(const cv::Mat & from)
 21 | {
 22 |   cv::Point3f to = cv::Point3f(from.at<float>(0), from.at<float>(1), from.at<float>(2));
 23 |   return to;
 24 | }
 25 | 
 26 | DistanceFilter::DistanceFilter()
 27 | : initialized_(false), state_num_(6), measure_num_(3)
 28 | {
 29 |   Init();
 30 | }
 31 | 
 32 | void DistanceFilter::Init(const cv::Point3f & pose)
 33 | {
 34 |   filter_.statePost.at<float>(0) = pose.x;
 35 |   filter_.statePost.at<float>(1) = pose.y;
 36 |   filter_.statePost.at<float>(2) = pose.z;
 37 |   initialized_ = true;
 38 | }
 39 | 
 40 | cv::Point3f DistanceFilter::Predict(const float & delta)
 41 | {
 42 |   SetInterval(delta);
 43 |   cv::Mat prediction = filter_.predict();
 44 |   cv::Point3f predict_pose = Convert(prediction);
 45 |   return predict_pose;
 46 | }
 47 | 
 48 | cv::Point3f DistanceFilter::Correct(const cv::Point3f & pose)
 49 | {
 50 |   if (!initialized_) {
 51 |     Init(pose);
 52 |     return pose;
 53 |   }
 54 | 
 55 |   measurement_.at<float>(0) = pose.x;
 56 |   measurement_.at<float>(1) = pose.y;
 57 |   measurement_.at<float>(2) = pose.z;
 58 |   cv::Mat pose_post = filter_.correct(measurement_);
 59 |   cv::Point3f pose_corrected = Convert(pose_post);
 60 |   return pose_corrected;
 61 | }
 62 | 
 63 | void DistanceFilter::Init()
 64 | {
 65 |   filter_ = cv::KalmanFilter(state_num_, measure_num_, 0);
 66 |   state_.create(state_num_, 1, CV_32FC1);
 67 |   process_noise_.create(state_num_, 1, CV_32F);
 68 |   measurement_ = cv::Mat::zeros(measure_num_, 1, CV_32F);
 69 | 
 70 |   randn(state_, cv::Scalar::all(0), cv::Scalar::all(0.1));
 71 |   filter_.transitionMatrix = (cv::Mat_<float>(6, 6) << \
 72 |     1, 0, 0, 1, 0, 0, \
 73 |     0, 1, 0, 0, 1, 0, \
 74 |     0, 0, 1, 0, 0, 1, \
 75 |     0, 0, 0, 1, 0, 0, \
 76 |     0, 0, 0, 0, 1, 0, \
 77 |     0, 0, 0, 0, 0, 1);
 78 | 
 79 |   filter_.measurementMatrix = (cv::Mat_<float>(3, 6) << \
 80 |     1, 0, 0, 0, 0, 0, \
 81 |     0, 1, 0, 0, 0, 0, \
 82 |     0, 0, 1, 0, 0, 0);
 83 | 
 84 |   setIdentity(filter_.processNoiseCov, cv::Scalar::all(1e-5));
 85 |   setIdentity(filter_.measurementNoiseCov, cv::Scalar::all(1e-4));
 86 |   setIdentity(filter_.errorCovPost, cv::Scalar::all(1));
 87 | }
 88 | 
 89 | void DistanceFilter::SetInterval(const float & delta)
 90 | {
 91 |   filter_.transitionMatrix.at<float>(0, 3) = delta;
 92 |   filter_.transitionMatrix.at<float>(1, 4) = delta;
 93 |   filter_.transitionMatrix.at<float>(2, 5) = delta;
 94 | }
 95 | 
 96 | DistanceFilter::~DistanceFilter()
 97 | {
 98 | }
 99 | 
100 | }  // namespace cyberdog_tracking
101 | 


--------------------------------------------------------------------------------
/cyberdog_tracking/include/cyberdog_tracking/object_tracking.hpp:
--------------------------------------------------------------------------------
 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved.
 2 | //
 3 | // Licensed under the Apache License, Version 2.0 (the "License");
 4 | // you may not use this file except in compliance with the License.
 5 | // You may obtain a copy of the License at
 6 | //
 7 | //     http://www.apache.org/licenses/LICENSE-2.0
 8 | //
 9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 | 
15 | #ifndef CYBERDOG_TRACKING__OBJECT_TRACKING_HPP_
16 | #define CYBERDOG_TRACKING__OBJECT_TRACKING_HPP_
17 | 
18 | #include <string>
19 | #include <memory>
20 | #include <vector>
21 | 
22 | #include "rclcpp/rclcpp.hpp"
23 | #include "tf2_ros/buffer.h"
24 | #include "tf2_ros/transform_listener.h"
25 | #include "rclcpp_lifecycle/lifecycle_node.hpp"
26 | #include "rclcpp_lifecycle/lifecycle_publisher.hpp"
27 | 
28 | #include "cyberdog_tracking/common_type.hpp"
29 | #include "cyberdog_tracking/distance_filter.hpp"
30 | 
31 | namespace cyberdog_tracking
32 | {
33 | using ReturnResult = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
34 | 
35 | class ObjectTracking : public rclcpp_lifecycle::LifecycleNode
36 | {
37 | public:
38 |   ObjectTracking();
39 |   virtual ~ObjectTracking();
40 | 
41 | protected:
42 |   ReturnResult on_configure(const rclcpp_lifecycle::State & state) override;
43 |   ReturnResult on_activate(const rclcpp_lifecycle::State & state) override;
44 |   ReturnResult on_deactivate(const rclcpp_lifecycle::State & state) override;
45 |   ReturnResult on_cleanup(const rclcpp_lifecycle::State & state) override;
46 |   ReturnResult on_shutdown(const rclcpp_lifecycle::State & state) override;
47 | 
48 | private:
49 |   void Initialize();
50 |   void CreateObject();
51 |   void CreateSub();
52 |   void CreatePub();
53 | 
54 |   void ProcessDepth(const SensorImageT::SharedPtr msg);
55 |   void ProcessBody(const PersonT::SharedPtr msg);
56 | 
57 |   void HandlerThread();
58 | 
59 |   void PubStatus(const uint8_t & status);
60 |   void PubPose(const StdHeaderT & header, const cv::Rect & tracked);
61 | 
62 |   float GetDistance(const StdHeaderT & header, const cv::Rect & tracked);
63 |   float GetDistance(const cv::Mat & image, const cv::Rect2d & body_tracked);
64 | 
65 |   void WakeThread();
66 |   void LoadCameraParam();
67 | 
68 | private:
69 |   rclcpp::Subscription<SensorImageT>::SharedPtr depth_sub_;
70 |   rclcpp::Subscription<PersonT>::SharedPtr body_sub_;
71 | 
72 |   rclcpp_lifecycle::LifecyclePublisher<GeometryPoseStampedT>::SharedPtr pose_pub_;
73 |   rclcpp_lifecycle::LifecyclePublisher<TrackingStatusT>::SharedPtr status_pub_;
74 | 
75 |   BuiltinTimeT last_stamp_;
76 | 
77 |   DistanceFilter * filter_ptr_;
78 | 
79 |   HandlerStruct handler_;
80 |   std::shared_ptr<std::thread> handler_thread_;
81 | 
82 |   std::mutex depth_mtx_;
83 |   std::vector<StampedImage> vec_stamped_depth_;
84 | 
85 |   int unfound_count_;
86 |   bool is_activate_;
87 | 
88 |   std::string ai_param_path_;
89 |   std::vector<float> ai_intrinsics_;
90 |   int img_width_;
91 |   int img_height_;
92 | };
93 | 
94 | }  // namespace cyberdog_tracking
95 | 
96 | #endif  // CYBERDOG_TRACKING__OBJECT_TRACKING_HPP_
97 | 


--------------------------------------------------------------------------------
/cyberdog_tracking/src/object_tracking.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved.
  2 | //
  3 | // Licensed under the Apache License, Version 2.0 (the "License");
  4 | // you may not use this file except in compliance with the License.
  5 | // You may obtain a copy of the License at
  6 | //
  7 | //     http://www.apache.org/licenses/LICENSE-2.0
  8 | //
  9 | // Unless required by applicable law or agreed to in writing, software
 10 | // distributed under the License is distributed on an "AS IS" BASIS,
 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 12 | // See the License for the specific language governing permissions and
 13 | // limitations under the License.
 14 | 
 15 | #include "cyberdog_tracking/object_tracking.hpp"
 16 | 
 17 | #include <unistd.h>
 18 | #include <yaml-cpp/yaml.h>
 19 | 
 20 | #include <map>
 21 | #include <utility>
 22 | #include <string>
 23 | #include <vector>
 24 | #include <memory>
 25 | #include <algorithm>
 26 | #include <chrono>
 27 | #include <cmath>
 28 | 
 29 | #include "cv_bridge/cv_bridge.h"
 30 | #include "tf2/LinearMath/Transform.h"
 31 | #include "rcutils/error_handling.h"
 32 | 
 33 | #include "cyberdog_common/cyberdog_log.hpp"
 34 | 
 35 | const int kBoundaryTh = 25;
 36 | const int kPredictFrame = 5;
 37 | const char kCailbrateParam[] = "/params/camera/calibration/params_intrinsic.yaml";
 38 | 
 39 | namespace cyberdog_tracking
 40 | {
 41 | 
 42 | ObjectTracking::ObjectTracking()
 43 | : rclcpp_lifecycle::LifecycleNode("object_tracking"),
 44 |   filter_ptr_(nullptr), unfound_count_(0),
 45 |   is_activate_(false)
 46 | {
 47 |   setvbuf(stdout, NULL, _IONBF, BUFSIZ);
 48 |   this->declare_parameter("ai_intrinsic", "./config/params_intrinsic.yaml");
 49 | }
 50 | 
 51 | ReturnResult ObjectTracking::on_configure(const rclcpp_lifecycle::State & /*state*/)
 52 | {
 53 |   INFO("Configuring tracking. ");
 54 |   Initialize();
 55 |   LoadCameraParam();
 56 |   return ReturnResult::SUCCESS;
 57 | }
 58 | 
 59 | ReturnResult ObjectTracking::on_activate(const rclcpp_lifecycle::State & /*state*/)
 60 | {
 61 |   INFO("Activating tracking. ");
 62 |   is_activate_ = true;
 63 |   // Create process thread
 64 |   handler_thread_ = std::make_shared<std::thread>(&ObjectTracking::HandlerThread, this);
 65 |   pose_pub_->on_activate();
 66 |   status_pub_->on_activate();
 67 |   return ReturnResult::SUCCESS;
 68 | }
 69 | 
 70 | ReturnResult ObjectTracking::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
 71 | {
 72 |   INFO("Deactivating tracking. ");
 73 |   is_activate_ = false;
 74 |   WakeThread();
 75 |   if (handler_thread_->joinable()) {
 76 |     handler_thread_->join();
 77 |   }
 78 |   pose_pub_->on_deactivate();
 79 |   status_pub_->on_deactivate();
 80 |   return ReturnResult::SUCCESS;
 81 | }
 82 | 
 83 | ReturnResult ObjectTracking::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
 84 | {
 85 |   INFO("Cleaning up tracking. ");
 86 |   handler_thread_.reset();
 87 |   pose_pub_.reset();
 88 |   status_pub_.reset();
 89 |   depth_sub_.reset();
 90 |   body_sub_.reset();
 91 |   return ReturnResult::SUCCESS;
 92 | }
 93 | 
 94 | ReturnResult ObjectTracking::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
 95 | {
 96 |   INFO("Shutting down tracking. ");
 97 |   return ReturnResult::SUCCESS;
 98 | }
 99 | 
100 | void ObjectTracking::Initialize()
101 | {
102 |   CreateObject();
103 |   CreateSub();
104 |   CreatePub();
105 | }
106 | 
107 | void ObjectTracking::CreateObject()
108 | {
109 |   // Get parameter
110 |   this->get_parameter<std::string>("ai_intrinsic", ai_param_path_);
111 |   ai_param_path_ += "/params_intrinsic.yaml";
112 | 
113 |   // Create object
114 |   INFO("Create object.");
115 |   filter_ptr_ = new DistanceFilter();
116 | }
117 | 
118 | void ObjectTracking::CreateSub()
119 | {
120 |   // Subscribe body detection topic to process
121 |   auto body_callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
122 |   rclcpp::SubscriptionOptions body_options;
123 |   body_options.callback_group = body_callback_group;
124 | 
125 |   rclcpp::SensorDataQoS sub_qos;
126 |   sub_qos.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
127 |   auto body_callback = [this](const PersonT::SharedPtr msg) {
128 |       ProcessBody(msg);
129 |     };
130 |   INFO("Subscribing to body detection topic. ");
131 |   body_sub_ = create_subscription<PersonT>("person", sub_qos, body_callback, body_options);
132 | 
133 |   // Subscribe depth image to process
134 |   auto depth_callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
135 |   rclcpp::SubscriptionOptions depth_options;
136 |   depth_options.callback_group = depth_callback_group;
137 | 
138 |   auto depth_callback = [this](const SensorImageT::SharedPtr msg) {
139 |       ProcessDepth(msg);
140 |     };
141 |   INFO("Subscribing to depth image topic. ");
142 |   depth_sub_ = create_subscription<SensorImageT>(
143 |     "camera/aligned_depth_to_extcolor/image_raw",
144 |     sub_qos, depth_callback, depth_options);
145 | }
146 | 
147 | void ObjectTracking::CreatePub()
148 | {
149 |   // Publish person position
150 |   rclcpp::ServicesQoS pub_qos;
151 |   pub_qos.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);
152 |   pose_pub_ = create_publisher<GeometryPoseStampedT>("tracking_pose", pub_qos);
153 |   status_pub_ = create_publisher<TrackingStatusT>("tracking_status", pub_qos);
154 | }
155 | 
156 | void ObjectTracking::ProcessDepth(const SensorImageT::SharedPtr msg)
157 | {
158 |   if (!is_activate_) {
159 |     return;
160 |   }
161 | 
162 |   INFO(
163 |     "Received depth image, ts: %.9d.%.9d", msg->header.stamp.sec,
164 |     msg->header.stamp.nanosec);
165 | 
166 |   cv_bridge::CvImagePtr depth_ptr = cv_bridge::toCvCopy(
167 |     msg, sensor_msgs::image_encodings::TYPE_16UC1);
168 |   StampedImage stamped_img;
169 |   stamped_img.header = msg->header;
170 |   stamped_img.image = depth_ptr->image.clone();
171 |   std::unique_lock<std::mutex> lk(depth_mtx_);
172 |   if (vec_stamped_depth_.size() > 6) {
173 |     vec_stamped_depth_.erase(vec_stamped_depth_.begin());
174 |   }
175 |   vec_stamped_depth_.push_back(stamped_img);
176 | }
177 | 
178 | float CalInterval(const BuiltinTimeT & stamp1, const BuiltinTimeT & stamp2)
179 | {
180 |   float interval = abs(
181 |     static_cast<double>(stamp1.sec) - static_cast<double>(stamp2.sec) +
182 |     static_cast<double>(static_cast<int>(stamp1.nanosec) - static_cast<int>(stamp2.nanosec)) *
183 |     static_cast<double>(1.0e-9));
184 |   return interval;
185 | }
186 | 
187 | int FindNearest(const std::vector<StampedImage> & img_buffer, const StdHeaderT & header)
188 | {
189 |   int position = -1;
190 |   float min_interval = 0.2f;
191 |   for (size_t i = 0; i < img_buffer.size(); ++i) {
192 |     float interval = CalInterval(img_buffer[i].header.stamp, header.stamp);
193 |     if (interval < min_interval) {
194 |       min_interval = interval;
195 |       position = i;
196 |     }
197 |     std::cout << "interval: " << interval << std::endl;
198 |   }
199 |   return position;
200 | }
201 | 
202 | cv::Rect Convert2CV(const sensor_msgs::msg::RegionOfInterest & roi)
203 | {
204 |   cv::Rect rect = cv::Rect(roi.x_offset, roi.y_offset, roi.width, roi.height);
205 |   return rect;
206 | }
207 | 
208 | sensor_msgs::msg::RegionOfInterest Convert2ROS(const cv::Rect & rect)
209 | {
210 |   sensor_msgs::msg::RegionOfInterest roi;
211 |   roi.x_offset = rect.x;
212 |   roi.y_offset = rect.y;
213 |   roi.width = rect.width;
214 |   roi.height = rect.height;
215 |   return roi;
216 | }
217 | 
218 | void ObjectTracking::ProcessBody(const PersonT::SharedPtr msg)
219 | {
220 |   if (!is_activate_) {
221 |     return;
222 |   }
223 | 
224 |   INFO(
225 |     "Received detection result, ts %.9d.%.9d",
226 |     msg->header.stamp.sec, msg->header.stamp.nanosec);
227 |   INFO("Received body num %d", msg->body_info.count);
228 | 
229 |   StampedBbox tracked;
230 |   if ((0 != msg->track_res.roi.width && 0 != msg->track_res.roi.height) ||
231 |     (0 != msg->body_info.count))
232 |   {
233 |     INFO("Process to get pose.");
234 |     cv::Rect bbox = Convert2CV(msg->track_res.roi);
235 |     tracked.header = msg->header;
236 |     tracked.vecInfo.push_back(bbox);
237 |     std::unique_lock<std::mutex> lk(handler_.mtx);
238 |     handler_.vecFrame.clear();
239 |     handler_.vecFrame.push_back(tracked);
240 |     INFO("Tracking completed, activate cloud handler to cal pose. ");
241 |     handler_.cond.notify_one();
242 |   }
243 | }
244 | 
245 | void ObjectTracking::HandlerThread()
246 | {
247 |   while (rclcpp::ok()) {
248 |     StampedBbox bodies;
249 |     {
250 |       std::unique_lock<std::mutex> lk(handler_.mtx);
251 |       handler_.cond.wait(lk, [this] {return !handler_.vecFrame.empty();});
252 |       INFO("Cloud handler thread is active. ");
253 |       bodies = handler_.vecFrame[0];
254 |       handler_.vecFrame.clear();
255 |     }
256 |     if (!is_activate_) {
257 |       return;
258 |     }
259 | 
260 |     // Get pose of tracked bbox and pub
261 |     PubPose(bodies.header, bodies.vecInfo[0]);
262 |   }
263 | }
264 | 
265 | GeometryPoseT Convert2ROS(const cv::Point3f & from)
266 | {
267 |   GeometryPoseT to;
268 |   to.position.x = from.x;
269 |   to.position.y = from.y;
270 |   to.position.z = from.z;
271 |   return to;
272 | }
273 | 
274 | GeometryPointT Dis2Position(
275 |   float & distance, const std::vector<float> & cam_param,
276 |   const cv::Vec2d & center)
277 | {
278 |   GeometryPointT position;
279 |   position.x = distance;
280 |   position.y = -distance *
281 |     (center[0] - cam_param[2]) / cam_param[0];
282 |   position.z = -distance *
283 |     (center[1] - cam_param[3]) / cam_param[1];
284 |   return position;
285 | }
286 | 
287 | cv::Point3f Convert2CV(const GeometryPointT & point)
288 | {
289 |   cv::Point3f cv_point = cv::Point3f(point.x, point.y, point.z);
290 |   return cv_point;
291 | }
292 | 
293 | void ObjectTracking::PubStatus(const uint8_t & status)
294 | {
295 |   TrackingStatusT TrackingStatusT;
296 |   TrackingStatusT.status = status;
297 |   status_pub_->publish(TrackingStatusT);
298 | }
299 | 
300 | void ObjectTracking::PubPose(const StdHeaderT & header, const cv::Rect & tracked)
301 | {
302 |   INFO("To find depth and get pose according to stamped bbox. ");
303 | 
304 |   // Get body position
305 |   GeometryPoseStampedT pose_pub;
306 |   pose_pub.header.stamp = header.stamp;
307 |   pose_pub.header.frame_id = "camera_link";
308 |   cv::Vec2d body_center = cv::Vec2d(
309 |     tracked.x + tracked.width / 2.0,
310 |     tracked.y + tracked.height / 2.0);
311 | 
312 |   if (filter_ptr_->initialized_ && unfound_count_ < kPredictFrame) {
313 |     // Get prediction with pre frame
314 |     float delta = CalInterval(pose_pub.header.stamp, last_stamp_);
315 |     cv::Point3f pose_predict = filter_ptr_->Predict(delta);
316 |     pose_pub.pose = Convert2ROS(pose_predict);
317 |   }
318 | 
319 |   // Get measurement value and correct
320 |   float distance = GetDistance(header, tracked);
321 |   GeometryPoseStampedT pose;
322 |   pose.header = pose_pub.header;
323 |   pose.pose.position = Dis2Position(distance, ai_intrinsics_, body_center);
324 |   pose.pose.orientation.w = 1.0;
325 |   if (0.0 != distance) {
326 |     INFO("Get pose success, correct kf. ");
327 |     cv::Point3f posePost = filter_ptr_->Correct(Convert2CV(pose.pose.position));
328 |     pose_pub.pose = Convert2ROS(posePost);
329 |   }
330 | 
331 |   // Publish position only valid
332 |   if (filter_ptr_->initialized_ && unfound_count_ < kPredictFrame) {
333 |     pose_pub_->publish(pose_pub);
334 |     last_stamp_ = pose_pub.header.stamp;
335 |     INFO(
336 |       "Pose pub: %.5f, %.5f, %.5f, timestamp: %.9d.%.9d",
337 |       pose_pub.pose.position.x, pose_pub.pose.position.y,
338 |       pose_pub.pose.position.z, pose_pub.header.stamp.sec,
339 |       pose_pub.header.stamp.nanosec);
340 | 
341 |     double dis = sqrt(pow(pose_pub.pose.position.x, 2) + pow(pose_pub.pose.position.y, 2));
342 |     INFO("Straight line distance from person to dog: %f", dis);
343 |     if (dis > 3.0) {
344 |       INFO("Status OBJECT_FAR.");
345 |       PubStatus(TrackingStatusT::OBJECT_FAR);
346 |     } else if (dis < 0.8) {
347 |       INFO("Status OBJECT_NEAR.");
348 |       PubStatus(TrackingStatusT::OBJECT_NEAR);
349 |     }
350 |     std::cout << "###posepub: " << pose_pub.pose.position.x << std::endl;
351 |   } else {
352 |     std::cout << "###posepub: null " << std::endl;
353 |   }
354 | }
355 | 
356 | float ObjectTracking::GetDistance(const StdHeaderT & header, const cv::Rect & tracked)
357 | {
358 |   // Find depth image according to timestamp
359 |   cv::Mat depth_image;
360 |   {
361 |     std::unique_lock<std::mutex> lk(depth_mtx_);
362 |     int position = FindNearest(vec_stamped_depth_, header);
363 |     if (position >= 0) {
364 |       depth_image = vec_stamped_depth_[position].image.clone();
365 |       INFO(
366 |         "Find depth image ts:  %.9d.%.9d",
367 |         vec_stamped_depth_[position].header.stamp.sec,
368 |         vec_stamped_depth_[position].header.stamp.nanosec);
369 |     } else {
370 |       ERROR("Cannot find depth image, cannot calculate pose. ");
371 |     }
372 |   }
373 | 
374 |   // Align depth to ai and get distance
375 |   float distance = 0.f;
376 |   cv::Mat ai_depth = depth_image.clone();
377 |   if (!depth_image.empty() && 0 != tracked.width && 0 != tracked.height) {
378 |     if (tracked.x > kBoundaryTh && tracked.x + tracked.width < (img_width_ - kBoundaryTh)) {
379 |       INFO("Get distance according to cloud point. ");
380 |       double start = static_cast<double>(cv::getTickCount());
381 |       double time = (static_cast<double>(cv::getTickCount()) - start) / cv::getTickFrequency();
382 |       DEBUG("Cloud handler cost: %f ms", time * 1000);
383 |     } else {
384 |       INFO("Status OBJECT_EDGE.");
385 |       PubStatus(TrackingStatusT::OBJECT_EDGE);
386 |     }
387 | 
388 |     // Get person position and publish
389 |     distance = GetDistance(ai_depth.clone(), tracked);
390 |     if (0.0 != distance) {
391 |       unfound_count_ = 0;
392 |     } else {
393 |       unfound_count_++;
394 |     }
395 |     std::cout << "###discal: " << distance << std::endl;
396 |   } else {
397 |     unfound_count_++;
398 |     std::cout << "###discal: null " << std::endl;
399 |   }
400 | 
401 |   if (unfound_count_ >= kPredictFrame) {
402 |     filter_ptr_->initialized_ = false;
403 |   }
404 | 
405 |   return distance;
406 | }
407 | 
408 | float ObjectTracking::GetDistance(const cv::Mat & image, const cv::Rect2d & body_tracked)
409 | {
410 |   float distance = 0.f;   // unit m
411 | 
412 |   if (!image.empty()) {
413 |     std::map<int, int> map_depth;
414 |     for (size_t i = body_tracked.x; i < body_tracked.x + body_tracked.width; ++i) {
415 |       for (size_t j = body_tracked.y; j < body_tracked.y + body_tracked.height; ++j) {
416 |         if (0.0 != image.at<ushort>(j, i)) {
417 |           int val = floor(image.at<ushort>(j, i) / 100.0);
418 |           std::map<int, int>::iterator iter = map_depth.find(val);
419 |           if (iter != map_depth.end()) {
420 |             iter->second++;
421 |           } else {
422 |             map_depth.insert(std::pair<int, int>(val, 1));
423 |           }
424 |         }
425 |       }
426 |     }
427 | 
428 |     int sum_count = 0;
429 |     for (std::map<int, int>::iterator it = map_depth.begin(); it != map_depth.end(); ++it) {
430 |       // std::cout << it->first << "-" << it->second << "; ";
431 |       sum_count += it->second;
432 |     }
433 |     // std::cout << std::endl;
434 |     INFO("===sum_count: %d", sum_count);
435 | 
436 |     int dis_sum = 0;
437 |     int dis_count = 0;
438 |     bool is_find = false;
439 | 
440 |     int last_value = 0;
441 |     if (!map_depth.empty()) {
442 |       last_value = map_depth.begin()->first;
443 |     }
444 |     if (sum_count > 100) {
445 |       for (std::map<int, int>::iterator it = map_depth.begin(); it != map_depth.end() && !is_find;
446 |         ++it)
447 |       {
448 |         if ((static_cast<double>(it->second) / sum_count) > 0.02 && (it->first - last_value) < 2) {
449 |           dis_count += it->second;
450 |           dis_sum += it->first / 10.0 * it->second;
451 |         } else {
452 |           if (dis_count / static_cast<double>(sum_count) > 0.2) {
453 |             is_find = true;
454 |             distance = dis_sum / static_cast<double>(dis_count);
455 |           }
456 |           dis_sum = 0;
457 |           dis_count = 0;
458 |         }
459 |         last_value = it->first;
460 |       }
461 |     } else {
462 |       ERROR("Cloud point < 100 . ");
463 |     }
464 |   }
465 | 
466 |   return distance;
467 | }
468 | 
469 | void ObjectTracking::WakeThread()
470 | {
471 |   StampedBbox tracked;
472 |   std::unique_lock<std::mutex> lk(handler_.mtx);
473 |   handler_.vecFrame.clear();
474 |   handler_.vecFrame.push_back(tracked);
475 |   handler_.cond.notify_one();
476 | }
477 | 
478 | void ObjectTracking::LoadCameraParam()
479 | {
480 |   YAML::Node param;
481 |   try {
482 |     param = YAML::LoadFile(kCailbrateParam);
483 |     ai_intrinsics_ = param["cam3"]["intrinsics"].as<std::vector<float>>();
484 |     if (ai_intrinsics_.size() != 4) {
485 |       ERROR("Non-pinhole model, use default param. ");
486 |       param = YAML::LoadFile(ai_param_path_);
487 |       ai_intrinsics_.clear();
488 |       ai_intrinsics_ = param["cam3"]["intrinsics"].as<std::vector<float>>();
489 |     }
490 |   } catch (...) {
491 |     ERROR("Load param fail, use default param. ");
492 |     param = YAML::LoadFile(ai_param_path_);
493 |     ai_intrinsics_ = param["cam3"]["intrinsics"].as<std::vector<float>>();
494 |   }
495 | 
496 |   std::vector<int> img_size = param["cam3"]["resolution"].as<std::vector<int>>();
497 |   img_width_ = img_size[0];
498 |   img_height_ = img_size[1];
499 | }
500 | 
501 | ObjectTracking::~ObjectTracking()
502 | {
503 |   if (filter_ptr_ != nullptr) {
504 |     delete filter_ptr_;
505 |   }
506 | 
507 |   WakeThread();
508 |   if (handler_thread_->joinable()) {
509 |     handler_thread_->join();
510 |   }
511 | }
512 | 
513 | }  // namespace cyberdog_tracking
514 | 


--------------------------------------------------------------------------------