class MyListener : public IDepthDataListener
{
+ static uint32_t const num_dist_columns_=4;
+ double latest_min_distance_[num_dist_columns_];
+ double latest_min_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_ = 0;
+ const double distance_threshold_ = 50;
+
public :
+
MyListener() :
undistortImage (false)
{
// each image containing one 32Bit channel
zImage.create (Size (data->width, data->height), CV_32FC1);
grayImage.create (Size (data->width, data->height), CV_32FC1);
+ confidenceImage.create (Size (data->width, data->height), CV_8UC1);
// set the image to zero
zImage = Scalar::all (0);
{
float *zRowPtr = zImage.ptr<float> (y);
float *grayRowPtr = grayImage.ptr<float> (y);
+ uint8_t *confRowPtr = confidenceImage.ptr<uint8_t> (y);
for (int x = 0; x < zImage.cols; x++, k++)
{
auto curPoint = data->points.at (k);
- if (curPoint.depthConfidence > 0)
+ confRowPtr[x] = curPoint.depthConfidence;
+ 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) );
+ }
+ }
+
+ //// Debug: show column part of image
+ // Mat subimg = zImage8(Rect((zImage.cols/num_dist_columns_)*3,0, zImage.cols/num_dist_columns_ , zImage8.rows));
+ // imshow ("column", subimg);
+
+ //detect column distance
+ auto col_width = zImage.cols/num_dist_columns_;
+ for (uint32_t col=0; col<num_dist_columns_; col++)
+ {
+ auto col_y_start = col*col_width;
+ Mat subimg = zImage8(Rect(col_y_start, 0, col_width , zImage8.rows));
+ double min, max;
+ minMaxLoc(subimg,&min,&max);
+ latest_min_distance_diff_[col]=min-latest_min_distance_[col];
+ latest_min_distance_[col]=min;
+ std::cout << "col" << col << " min:" << min << "("<<latest_min_distance_diff_[col]<<")"<< " max:" << max << std::endl;
+ }
+
+ //naive obstacle avoidance demo
+ naiveObstacleAvoidanceDemo();
+
// scale and display the depth image
scaledZImage.create (Size (data->width * 4, data->height * 4), CV_8UC1);
resize (zImage8, scaledZImage, scaledZImage.size());
}
// scale and display the gray image
- scaledGrayImage.create (Size (data->width * 4, data->height * 4), CV_8UC1);
- resize (grayImage8, scaledGrayImage, scaledGrayImage.size());
+ // scaledGrayImage.create (Size (data->width * 4, data->height * 4), CV_8UC1);
+ // resize (grayImage8, scaledGrayImage, scaledGrayImage.size());
+ // imshow ("Gray", scaledGrayImage);
- imshow ("Gray", scaledGrayImage);
+ imshow ("Confidence", confidenceImage);
}
void setLensParameters (const LensParameters &lensParameters)
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
return newGrayValue;
}
+ bool naiveIsObstaclePresent(bool free_paths[num_dist_columns_])
+ {
+ bool rv=false;
+ for (uint32_t col=0; col<num_dist_columns_; col++)
+ {
+ free_paths[col] = latest_min_distance_[col] > distance_threshold_;
+ if (!free_paths[col])
+ {
+ rv=true;
+ }
+ }
+ return rv;
+ }
+
+ void naiveObstacleAvoidanceDemo()
+ {
+ bool free_paths[num_dist_columns_];
+ if (naiveIsObstaclePresent(free_paths))
+ {
+ assert(num_dist_columns_ == 4);
+ if (free_paths[1] && free_paths[2])
+ {
+ std::cout << "GOSTRAIGHT" << std::endl;
+ } else if (free_paths[0])
+ {
+ std::cout << "GOLEFT" << std::endl;
+ } else if (free_paths[3])
+ {
+ std::cout << "GORIGHT" << std::endl;
+ } else {
+ std::cout << "STOP" << std::endl;
+ }
+ }
+ }
+
// define images for depth and gray
// and for their 8Bit and scaled versions
Mat zImage, zImage8, scaledZImage;
Mat grayImage, grayImage8, scaledGrayImage;
+ Mat confidenceImage;
// lens matrices used for the undistortion of
// the image
// create two windows
namedWindow ("Depth", WINDOW_AUTOSIZE);
- namedWindow ("Gray", WINDOW_AUTOSIZE);
+ // namedWindow ("Gray", WINDOW_AUTOSIZE);
+ namedWindow ("Confidence", WINDOW_AUTOSIZE);
+ // namedWindow ("column", WINDOW_AUTOSIZE);
// start capture mode
if (cameraDevice->startCapture() != CameraStatus::SUCCESS)
// toggle the undistortion of the image
listener.toggleUndistort();
}
+
+ if (currentKey == 'b')
+ {
+ // toggle the undistortion of the image
+ listener.toggleNormBlur();
+ }
}
// stop capture mode