| 1803 | } |
| 1804 | |
| 1805 | rtpose_wrapper::Joint3DMsg |
| 1806 | getJoint3D(double x, double y, double confidence, const cv::Mat& depth_frame) |
| 1807 | { |
| 1808 | rtpose_wrapper::Joint3DMsg joint3D; |
| 1809 | int min_y = std::max<double>(y - _median_search_y_component, |
| 1810 | 0.0f); |
| 1811 | int min_x = std::max<double>(x - _median_search_x_component, |
| 1812 | 0.0f); |
| 1813 | int max_y = std::min<double>(y + _median_search_y_component, |
| 1814 | depth_frame.rows); |
| 1815 | int max_x = std::min<double>(x + _median_search_x_component, |
| 1816 | depth_frame.cols); |
| 1817 | #if DEBUG > 0 |
| 1818 | cv::rectangle(_colored_depth_image, |
| 1819 | cv::Point(min_x, min_y), cv::Point(max_x, max_y), |
| 1820 | cv::Scalar(0, 255, 255), 3); |
| 1821 | // cv::ellipse(_colored_depth_image,cv::Point(x,y), |
| 1822 | // cv::Size(_median_search_x_component,_median_search_y_component),0, |
| 1823 | // 0,0,cv::Scalar(0,255,255),5); |
| 1824 | #endif |
| 1825 | cv::Rect rect(min_x, min_y, (max_x - min_x), (max_y - min_y)); |
| 1826 | joint3D.z = medianBetweenValidPoints(depth_frame(rect)); |
| 1827 | if (sensor_type != "zed") |
| 1828 | joint3D.z = joint3D.z / 1000.0f; //conversion from mm to m (necessary for openni) |
| 1829 | joint3D.x = (x - _cx) * joint3D.z * _constant_x; |
| 1830 | joint3D.y = (y - _cy) * joint3D.z * _constant_y; |
| 1831 | joint3D.confidence = confidence; |
| 1832 | joint3D.header = _depth_frame_from_sensor.header; |
| 1833 | joint3D.max_height = DISPLAY_RESOLUTION_HEIGHT; |
| 1834 | joint3D.max_width = DISPLAY_RESOLUTION_WIDTH; |
| 1835 | if (fabs(joint3D.z) < 0.001f |
| 1836 | or joint3D.confidence < _min_confidence_per_joint) |
| 1837 | joint3D.x = joint3D.y = joint3D.z =std::numeric_limits<double>::quiet_NaN(); |
| 1838 | return joint3D; |
| 1839 | } |
| 1840 | |
| 1841 | |
| 1842 | bool |
no test coverage detected