95 lines
3.4 KiB
C++
95 lines
3.4 KiB
C++
#include "2d_depth_detector.h"
|
|
|
|
using namespace cv;
|
|
using namespace std;
|
|
|
|
DepthDetector::DepthDetector() {
|
|
//positions = new vector<Point>;
|
|
}
|
|
|
|
vector<Point2d> DepthDetector::getPositions() {
|
|
//TO DO: use main camera class
|
|
//logging method
|
|
o3d3xx::Logging::Init();
|
|
//initialise camera constructor expects IP address, using default one
|
|
o3d3xx::Camera::Ptr cam = make_shared<o3d3xx::Camera>("192.168.1.69");
|
|
//create buffer to fetch image
|
|
o3d3xx::ImageBuffer::Ptr img = make_shared<o3d3xx::ImageBuffer>();
|
|
//framegrabber
|
|
o3d3xx::FrameGrabber::Ptr fg =
|
|
make_shared<o3d3xx::FrameGrabber>(
|
|
cam, o3d3xx::IMG_AMP|o3d3xx::IMG_RDIS|o3d3xx::IMG_CART);
|
|
//get frame from camera
|
|
if (! fg->WaitForFrame(img.get(), 2000))
|
|
{
|
|
//TO DO: error handling
|
|
cerr << "Timeout waiting for camera!" << endl;
|
|
//return positions;
|
|
}
|
|
//2D images
|
|
double min, max;
|
|
////create depth image
|
|
Mat depthImageRaw = Mat(img->DepthImage());
|
|
//Mat xyzImageRaw = Mat(img->XYZImage());
|
|
//resize image for better visibility when debugging
|
|
resize(depthImageRaw,depthImageRaw, Size(depthImageRaw.cols*2,depthImageRaw.rows*2), 2, 2, INTER_CUBIC);
|
|
|
|
//resize(xyzImageRaw,xyzImageRaw, Size(xyzImageRaw.cols*2,xyzImageRaw.rows*2), 2, 2, INTER_CUBIC);
|
|
//apply region of interest to only view inside of bin
|
|
//Xmin = -120, Xmax = 130, Ymin = -800, Ymax = 250
|
|
Rect roi = Rect(120,45,110,145);
|
|
Mat depthImage = depthImageRaw(roi);
|
|
//Mat xyzImage = xyzImageRaw(roi);
|
|
//minMaxIdx(xyzImage, &min, &max);
|
|
//convertScaleAbs(xyzImage, xyzImage, 255 / max);
|
|
//applyColorMap(xyzImage, xyzImage, COLORMAP_JET);
|
|
//Mat channel[3];
|
|
//split(xyzImage, channel);
|
|
minMaxIdx(depthImage, &min, &max);
|
|
convertScaleAbs(depthImage, depthImage, 255 / max);
|
|
applyColorMap(depthImage, depthImage, COLORMAP_JET);
|
|
//convert to gray scale
|
|
Mat depthImageGray;
|
|
cvtColor(depthImage, depthImageGray, CV_BGR2GRAY);
|
|
//create new mat objects to hold binary data
|
|
Mat depthImageBinary;
|
|
Mat depthImageBinary16S;
|
|
Mat depthImageBinary16SLabeled;
|
|
//create variables for blob detection
|
|
vector<Point2d*> firstPixelvec;
|
|
vector<Point2d*> posVec;
|
|
vector<int> areaVec;
|
|
double x,y;
|
|
//detect blobs going from high to low threshold
|
|
for(int i = 12; i > -1; i--){
|
|
//dynamically apply thresholds
|
|
threshold(depthImageGray, depthImageBinary, 20*i, 1, CV_THRESH_BINARY);
|
|
//use 16S image
|
|
depthImageBinary.convertTo(depthImageBinary16S, CV_16S);
|
|
//call blob detection
|
|
labelBLOBsInfo(depthImageBinary16S,depthImageBinary16SLabeled,firstPixelvec, posVec,areaVec, 200, 10000);
|
|
for(int j = 0; j < firstPixelvec.size(); j++){
|
|
//calculate in meters the x and y position
|
|
x = ((posVec[j]->x * 1.65) - 430)/1000;
|
|
y = ((posVec[j]->y*1.8) - 250)/1000;
|
|
positions.push_back(Point2d(x,y));
|
|
|
|
//get depth value (z value?)
|
|
/*uchar val;
|
|
int posy = posVec[j]->y;
|
|
int posx = posVec[j]->x;
|
|
for(int k = 0; k < 3; k++){
|
|
val = channel[k].at<uchar>(posy,posx);
|
|
cout << "Value Channel"<<k<<": " << (double)val << " thresh: " << to_string(20*i) << " at("<<posy<<","<<posx<<")"<< endl;
|
|
}
|
|
circle(depthImageBinary16S, Point(posy,posx), 5, Scalar(10));*/
|
|
show16SImageStretch(depthImageBinary16S, "16s binary");
|
|
}
|
|
//clear vectors to search next threshold
|
|
firstPixelvec.clear();
|
|
areaVec.clear();
|
|
posVec.clear();
|
|
}
|
|
return positions;
|
|
}
|