using namespace std;
using namespace cv;
+//#define SHOW_GUI_WINDOWS
+
// Linker errors for the OpenCV sample
//
// If this example gives linker errors about undefined references to cv::namedWindow and cv::imshow,
bool normblurImage = true;
const uint8_t confidence_threshold_ = 0;
+ const double distance_threshold_ = 80;
public :
// create two images which will be filled afterwards
// each image containing one 32Bit channel
zImage.create (Size (data->width, data->height), CV_32FC1);
+#ifdef SHOW_GUI_WINDOWS
grayImage.create (Size (data->width, data->height), CV_32FC1);
+ confidenceImage.create (Size (data->width, data->height), CV_8UC1);
+#endif
// set the image to zero
zImage = Scalar::all (0);
+#ifdef SHOW_GUI_WINDOWS
grayImage = Scalar::all (0);
+#endif
int k = 0;
for (int y = 0; y < zImage.rows; y++)
{
float *zRowPtr = zImage.ptr<float> (y);
+#ifdef SHOW_GUI_WINDOWS
float *grayRowPtr = grayImage.ptr<float> (y);
+ uint8_t *confRowPtr = confidenceImage.ptr<uint8_t> (y);
+#endif
for (int x = 0; x < zImage.cols; x++, k++)
{
auto curPoint = data->points.at (k);
+#ifdef SHOW_GUI_WINDOWS
+ confRowPtr[x] = curPoint.depthConfidence;
+#endif
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);
+#ifdef SHOW_GUI_WINDOWS
grayRowPtr[x] = adjustGrayValue (curPoint.grayValue);
+#endif
} else {
//asume point is as far away as possible and thus "SAFE" for obstacle avoidance
zRowPtr[x] = 255;
+#ifdef SHOW_GUI_WINDOWS
grayRowPtr[x] = 255;
+#endif
}
}
}
// create images to store the 8Bit version (some OpenCV
// functions may only work on 8Bit images)
zImage8.create (Size (data->width, data->height), CV_8UC1);
+#ifdef SHOW_GUI_WINDOWS
grayImage8.create (Size (data->width, data->height), CV_8UC1);
-
+#endif
// convert images to the 8Bit version
// This sample uses a fixed scaling of the values to (0, 255) to avoid flickering.
// You can also replace this with an automatic scaling by using
// normalize(zImage, zImage8, 0, 255, NORM_MINMAX, CV_8UC1)
// normalize(grayImage, grayImage8, 0, 255, NORM_MINMAX, CV_8UC1)
zImage.convertTo (zImage8, CV_8UC1);
+#ifdef SHOW_GUI_WINDOWS
grayImage.convertTo (grayImage8, CV_8UC1);
+#endif
if (undistortImage)
{
//// 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++)
minMaxLoc(subimg,&min,&max);
latest_min_distance_diff_[col]=min-latest_min_distance_[col];
latest_min_distance_[col]=min;
+#ifdef SHOW_GUI_WINDOWS
std::cout << "col" << col << " min:" << min << "("<<latest_min_distance_diff_[col]<<")"<< " max:" << max << std::endl;
+#endif
}
+ //naive obstacle avoidance demo, uses global latest_min_distance_diff_
+ naiveObstacleAvoidanceDemo();
+
// scale and display the depth image
- scaledZImage.create (Size (data->width * 4, data->height * 4), CV_8UC1);
- resize (zImage8, scaledZImage, scaledZImage.size());
+ // scaledZImage.create (Size (data->width * 4, data->height * 4), CV_8UC1);
+ // resize (zImage8, scaledZImage, scaledZImage.size());
+ // imshow ("Depth", scaledZImage);
- imshow ("Depth", scaledZImage);
-
- if (undistortImage)
- {
- // call the undistortion function on the gray image
- Mat temp = grayImage8.clone();
- undistort (temp, grayImage8, cameraMatrix, distortionCoefficients);
- }
+#ifdef SHOW_GUI_WINDOWS
+ imshow ("Depth", zImage8);
// scale and display the gray image
- scaledGrayImage.create (Size (data->width * 4, data->height * 4), CV_8UC1);
- resize (grayImage8, scaledGrayImage, scaledGrayImage.size());
-
- imshow ("Gray", scaledGrayImage);
+ // scaledGrayImage.create (Size (data->width * 4, data->height * 4), CV_8UC1);
+ // resize (grayImage8, scaledGrayImage, scaledGrayImage.size());
+ // imshow ("Gray", scaledGrayImage);
+
+ // if (undistortImage)
+ // {
+ // // call the undistortion function on the gray image
+ // Mat temp = grayImage8.clone();
+ // undistort (temp, grayImage8, cameraMatrix, distortionCoefficients);
+ // }
+
+ imshow ("Confidence", confidenceImage);
+#endif
}
void setLensParameters (const LensParameters &lensParameters)
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
return 1;
}
+#ifdef SHOW_GUI_WINDOWS
// create two windows
namedWindow ("Depth", WINDOW_AUTOSIZE);
- namedWindow ("Gray", WINDOW_AUTOSIZE);
+ // namedWindow ("Gray", WINDOW_AUTOSIZE);
+ namedWindow ("Confidence", WINDOW_AUTOSIZE);
// namedWindow ("column", WINDOW_AUTOSIZE);
+#endif
+
+
+ // set an operation mode
+ if (cameraDevice->setUseCase ("MODE_5_45FPS_500") != royale::CameraStatus::SUCCESS)
+ {
+ cerr << "Error setting use case" << endl;
+ return 1;
+ }
// start capture mode
if (cameraDevice->startCapture() != CameraStatus::SUCCESS)
return 1;
}
+
+ // Change the exposure time for the first stream of the use case (Royale will limit this to an
+ // eye-safe exposure time, with limits defined by the use case). The time is given in
+ // microseconds.
+ //
+ // Non-mixed mode use cases have exactly one stream, mixed mode use cases have more than one.
+ // For this example we only change the first stream.
+ // if (cameraDevice->setExposureTime (200, streamIds[0]) != royale::CameraStatus::SUCCESS)
+ // {
+ // cerr << "Cannot set exposure time for stream" << streamIds[0] << endl;
+ // }
+ // else
+ // {
+ // cout << "Changed exposure time for stream " << streamIds[0] << " to 200 microseconds ..." << endl;
+ // }
+
+
int currentKey = 0;
while (currentKey != 27)
projects / 201903hackathon.git / blobdiff