class MyListener : public IDepthDataListener
{
+ static uint32_t const num_dist_columns_=5;
+ uint32_t latest_distance_[num_dist_columns_];
+ uint32_t latest_distance_diff_[num_dist_columns_];
+
+ const int DELAY_BLUR = 100;
+ // const int MAX_KERNEL_LENGTH = 31;
+ const int MAX_KERNEL_LENGTH = 23;
+
+ bool normblurImage = true;
+
+ const uint8_t confidence_threshold_ = 5;
+
public :
+
MyListener() :
undistortImage (false)
{
for (int x = 0; x < zImage.cols; x++, k++)
{
auto curPoint = data->points.at (k);
- if (curPoint.depthConfidence > 0)
+ if (curPoint.depthConfidence > confidence_threshold_)
{
// if the point is valid, map the pixel from 3D world
// coordinates to a 2D plane (this will distort the image)
zRowPtr[x] = adjustZValue (curPoint.z);
grayRowPtr[x] = adjustGrayValue (curPoint.grayValue);
+ } else {
+ //asume point is as far away as possible and thus "SAFE" for obstacle avoidance
+ zRowPtr[x] = 255;
+ grayRowPtr[x] = 255;
}
}
}
undistort (temp, zImage8, cameraMatrix, distortionCoefficients);
}
+ if (normblurImage)
+ {
+ auto temp = zImage8.clone();
+ for ( int i = 1; i < MAX_KERNEL_LENGTH; i = i + 2 )
+ {
+ blur( temp, zImage8, Size( i, i ), Point(-1,-1) );
+ }
+ }
+
+
+
+
// scale and display the depth image
scaledZImage.create (Size (data->width * 4, data->height * 4), CV_8UC1);
resize (zImage8, scaledZImage, scaledZImage.size());
undistortImage = !undistortImage;
}
+ void toggleNormBlur()
+ {
+ std::lock_guard<std::mutex> lock (flagMutex);
+ normblurImage = !normblurImage;
+ }
+
private:
// adjust z value to fit fixed scaling, here max dist is 2.5m
// toggle the undistortion of the image
listener.toggleUndistort();
}
+
+ if (currentKey == 'b')
+ {
+ // toggle the undistortion of the image
+ listener.toggleNormBlur();
+ }
}
// stop capture mode