@@ -43,7 +43,32 @@ namespace libfreenect2
4343static const float depth_q = 0.01 ;
4444static const float color_q = 0.002199 ;
4545
46- void Registration::distort (int mx, int my, float & x, float & y) const
46+ class RegistrationImpl
47+ {
48+ public:
49+ RegistrationImpl (Freenect2Device::IrCameraParams depth_p, Freenect2Device::ColorCameraParams rgb_p);
50+
51+ void apply (int dx, int dy, float dz, float & cx, float &cy) const ;
52+ void apply (const Frame* rgb, const Frame* depth, Frame* undistorted, Frame* registered, const bool enable_filter, Frame* bigdepth) const ;
53+ void getPointXYZRGB (const Frame* undistorted, const Frame* registered, int r, int c, float & x, float & y, float & z, float & rgb) const ;
54+ void distort (int mx, int my, float & dx, float & dy) const ;
55+ void depth_to_color (float mx, float my, float & rx, float & ry) const ;
56+
57+ private:
58+ Freenect2Device::IrCameraParams depth; // /< Depth camera parameters.
59+ Freenect2Device::ColorCameraParams color; // /< Color camera parameters.
60+
61+ int distort_map[512 * 424 ];
62+ float depth_to_color_map_x[512 * 424 ];
63+ float depth_to_color_map_y[512 * 424 ];
64+ int depth_to_color_map_yi[512 * 424 ];
65+
66+ const int filter_width_half;
67+ const int filter_height_half;
68+ const float filter_tolerance;
69+ };
70+
71+ void RegistrationImpl::distort (int mx, int my, float & x, float & y) const
4772{
4873 // see http://en.wikipedia.org/wiki/Distortion_(optics) for description
4974 float dx = ((float )mx - depth.cx ) / depth.fx ;
@@ -57,7 +82,7 @@ void Registration::distort(int mx, int my, float& x, float& y) const
5782 y = depth.fy * (dy * kr + depth.p1 * (r2 + 2 * dy2) + depth.p2 * dxdy2) + depth.cy ;
5883}
5984
60- void Registration ::depth_to_color (float mx, float my, float & rx, float & ry) const
85+ void RegistrationImpl ::depth_to_color (float mx, float my, float & rx, float & ry) const
6186{
6287 mx = (mx - depth.cx ) * depth_q;
6388 my = (my - depth.cy ) * depth_q;
@@ -82,6 +107,11 @@ void Registration::depth_to_color(float mx, float my, float& rx, float& ry) cons
82107 * Undistort/register a single depth data point
83108 */
84109void Registration::apply ( int dx, int dy, float dz, float & cx, float &cy) const
110+ {
111+ impl_->apply (dx, dy, dz, cx, cy);
112+ }
113+
114+ void RegistrationImpl::apply ( int dx, int dy, float dz, float & cx, float &cy) const
85115{
86116 const int index = dx + dy * 512 ;
87117 float rx = depth_to_color_map_x[index];
@@ -103,6 +133,11 @@ void Registration::apply( int dx, int dy, float dz, float& cx, float &cy) const
103133 * @note The \a bigdepth frame has a blank top and bottom row.
104134 */
105135void Registration::apply (const Frame *rgb, const Frame *depth, Frame *undistorted, Frame *registered, const bool enable_filter, Frame *bigdepth) const
136+ {
137+ impl_->apply (rgb, depth, undistorted, registered, enable_filter, bigdepth);
138+ }
139+
140+ void RegistrationImpl::apply (const Frame *rgb, const Frame *depth, Frame *undistorted, Frame *registered, const bool enable_filter, Frame *bigdepth) const
106141{
107142 // Check if all frames are valid and have the correct size
108143 if (!rgb || !depth || !undistorted || !registered ||
@@ -263,6 +298,11 @@ void Registration::apply(const Frame *rgb, const Frame *depth, Frame *undistorte
263298 * @param[out] RGB associated rgb color of point.
264299 */
265300void Registration::getPointXYZRGB (const Frame* undistorted, const Frame* registered, int r, int c, float & x, float & y, float & z, float & rgb) const
301+ {
302+ impl_->getPointXYZRGB (undistorted, registered, r, c, x, y, z, rgb);
303+ }
304+
305+ void RegistrationImpl::getPointXYZRGB (const Frame* undistorted, const Frame* registered, int r, int c, float & x, float & y, float & z, float & rgb) const
266306{
267307 const float bad_point = std::numeric_limits<float >::quiet_NaN ();
268308 const float cx (depth.cx ), cy (depth.cy );
@@ -288,6 +328,14 @@ void Registration::getPointXYZRGB (const Frame* undistorted, const Frame* regist
288328}
289329
290330Registration::Registration (Freenect2Device::IrCameraParams depth_p, Freenect2Device::ColorCameraParams rgb_p):
331+ impl_ (new RegistrationImpl(depth_p, rgb_p)) {}
332+
333+ Registration::~Registration ()
334+ {
335+ delete impl_;
336+ }
337+
338+ RegistrationImpl::RegistrationImpl (Freenect2Device::IrCameraParams depth_p, Freenect2Device::ColorCameraParams rgb_p):
291339 depth (depth_p), color(rgb_p), filter_width_half(2 ), filter_height_half(1 ), filter_tolerance(0 .01f )
292340{
293341 float mx, my;
0 commit comments