//normalized (0,1) and inverted Sobel edge detector
std::shared_ptr<Picture> SobelEdgeFilter::ApplyImpl(const std::shared_ptr<Picture>& pic)
{
const auto& src = pic->Mat();
double minVal, maxVal;
cv::Mat res;
cv::Mat XGrad, YGrad;
cv::Mat XGradAbs, YGradAbs;
Sobel( src, XGrad, Depth, 1, 0, 3, Scale, Delta, cv::BORDER_DEFAULT );
convertScaleAbs( XGrad, XGradAbs );
Sobel( src, YGrad, Depth, 0, 1, 3, Scale, Delta, cv::BORDER_DEFAULT );
convertScaleAbs( YGrad, YGradAbs );
addWeighted( XGradAbs, 0.5, YGradAbs, 0.5, 0, res );
cvtColor(res, res, CV_BGR2GRAY);
minMaxLoc(res, &minVal, &maxVal);
res.convertTo(res, CV_64F,1/(maxVal - minVal));
minMaxLoc(res, &minVal, &maxVal);
res = 1 - res;
return std::make_shared<Picture>(res);
}
void AntiShake::sobelOperator(Mat &src, Mat &output, int scale, int delta) {
int ddepth = CV_16S;
GaussianBlur(src, output, Size(3, 3), 0, 0, BORDER_DEFAULT);
// blur( src1, output1, Size( blurSize, blurSize), Point(point, point) );
if (output.type() == 16) {
/// Convert it to gray
cvtColor(output, output, CV_RGB2GRAY);
}
/// Generate grad_x and grad_y
Mat grad_x, grad_y, grad;
Mat abs_grad_x, abs_grad_y;
int border = BORDER_ISOLATED;
/// Gradient X
//Scharr( src_gray, grad_x, ddepth, 1, 0, scale, delta, BORDER_DEFAULT );
Sobel(output, grad_x, ddepth, 1, 0, 3, scale, delta, border);
convertScaleAbs(grad_x, abs_grad_x);
/// Gradient Y
//Scharr( src_gray, grad_y, ddepth, 0, 1, scale, delta, BORDER_DEFAULT );
Sobel(output, grad_y, ddepth, 0, 1, 3, scale, delta, border);
convertScaleAbs(grad_y, abs_grad_y);
/// Total Gradient (approximate)
addWeighted(abs_grad_x, 0.5, abs_grad_y, 0.5, 0, grad);
grad.copyTo(output);
grad_x.release();
grad_y.release();
abs_grad_x.release();
abs_grad_y.release();
}
std::vector<PBASFeature> PBASFeature::calcFeatureMap(const cv::Mat& inputFrame)
{
std::vector<PBASFeature> result;
// get gradient magnitude map
cv::Mat sobelX, sobelY, inputGray, gradMagnMap;
cv::GaussianBlur(inputFrame, inputGray, cv::Size(5, 5), 2, 2, cv::BORDER_DEFAULT);
cv::cvtColor(inputGray, inputGray, CV_BGR2GRAY);
// Gradient Y
Sobel(inputGray, sobelY, CV_64F, 1, 0, 5, 1, 0, cv::BORDER_DEFAULT);
convertScaleAbs(sobelY, sobelY);
// Gradient X
Sobel(inputGray, sobelX, CV_64F, 0, 1, 5, 1, 0, cv::BORDER_DEFAULT);
convertScaleAbs(sobelX, sobelX);
sobelY.convertTo(sobelY, CV_8U);
sobelX.convertTo(sobelX, CV_8U);
int kernelSize = 7;
cv::Mat avgKernel = cv::Mat::ones(kernelSize, kernelSize, CV_32F) / (kernelSize * kernelSize);
cv::Mat avgMatB, avgMatG, avgMatR;
cv::Mat channel[3];
cv::split(inputFrame, channel);
cv::filter2D(channel[0], avgMatB, CV_8U, avgKernel, cv::Point(-1, -1), 0, cv::BORDER_DEFAULT);
cv::filter2D(channel[1], avgMatG, CV_8U, avgKernel, cv::Point(-1, -1), 0, cv::BORDER_DEFAULT);
cv::filter2D(channel[2], avgMatR, CV_8U, avgKernel, cv::Point(-1, -1), 0, cv::BORDER_DEFAULT);
cv::Mat tmp(1, 1, CV_8UC3);
for (int y = 0; y < inputFrame.rows; ++y)
{
for (int x = 0; x < inputFrame.cols; ++x)
{
tmp.ptr<uchar>(0)[0] = inputFrame.at<cv::Vec3b>(y, x)[0];
tmp.ptr<uchar>(0)[1] = inputFrame.at<cv::Vec3b>(y, x)[1];
tmp.ptr<uchar>(0)[2] = inputFrame.at<cv::Vec3b>(y, x)[2];
result.push_back(PBASFeature(tmp, avgMatB.at<uchar>(y, x), avgMatG.at<uchar>(y, x), avgMatR.at<uchar>(y, x), sobelX.at<uchar>(y,x), sobelY.at<uchar>(y, x)));
}
}
return result;
}
int EdgeHandle::singleEdgeHandle( string InImgPosition , string InImageName, string OutImgPosition, string OutImgName ){
Mat src, src_gray;
Mat grad;
int scale = 1;
int delta = 0;
int ddepth = CV_16S;
int c;
/// Load an image
src = imread(InImgPosition+InImageName);
if( !src.data )
{ return -1; }
GaussianBlur( src, src, Size(3,3), 0, 0, BORDER_DEFAULT );
/// Convert it to gray
cvtColor( src, src_gray, CV_RGB2GRAY );
/// Generate grad_x and grad_y
Mat grad_x, grad_y;
Mat abs_grad_x, abs_grad_y;
//sobel
Sobel( src_gray, grad_x, ddepth, 1, 0, 3, scale, delta, BORDER_DEFAULT );
convertScaleAbs( grad_x, abs_grad_x );
Sobel( src_gray, grad_y, ddepth, 0, 1, 3, scale, delta, BORDER_DEFAULT );
convertScaleAbs( grad_y, abs_grad_y );
addWeighted( abs_grad_x, 0.5, abs_grad_y, 0.5, 0, grad );
// binary image.
//threshold(grad,grad,0,255,THRESH_BINARY);
// Apply the specified morphology operation
int morph_size = 1;
Mat element = getStructuringElement( 1, Size( 2*morph_size + 1, 2*morph_size+1 ), Point( morph_size, morph_size ) );
morphologyEx( grad, grad, 5, element );
morphologyEx( grad, grad, 5, element );
morphologyEx( grad, grad, 5, element );
// Create window
string outs = OutImgPosition + OutImgName;
imwrite(outs,grad);
return 0;
}
void edgeDetect(Mat& img) {
GaussianBlur(img, img, Size(3, 3), 0, 0);
Mat tmp;
img.copyTo(tmp);
Canny(img, img, 30, 100, 3, true);
bitwise_not(img, img);
erode(img, img, getStructuringElement(MORPH_ELLIPSE, Size(dilation_size + 1, dilation_size + 1), Point(dilation_size, dilation_size)));
if(loaded) {
std::vector<Rect> faces;
cvtColor(tmp, tmp, COLOR_BGR2GRAY );
equalizeHist( tmp, tmp);
face_cascade.detectMultiScale(tmp, faces, 1.1, 2, 0, Size(80, 80));
for( size_t i = 0; i < faces.size(); i++ ) {
Mat faceROI = tmp(faces[i]);
Laplacian(faceROI, faceROI, CV_16S, 3, 2.5, 1, BORDER_DEFAULT);
threshold(faceROI, faceROI, 7, 255, THRESH_BINARY_INV);
convertScaleAbs(faceROI, faceROI);
dilate(faceROI, faceROI, getStructuringElement(MORPH_ELLIPSE, Size(2, 2), Point(1, 1)));
faceROI.copyTo(img(faces[i]));
rectangle(img, faces[i], Scalar( 255, 190, 0 ));
}
}
cvtColor(img, img, CV_GRAY2BGR);
}
int InterframeRegister::cvutStdDevStretch( Mat src, Mat dst)
{
// Contrast Stretching:
// Stretching by Standard deviation
// A two standard deviation linear contrast stretch is applied
Scalar mean;
Scalar std_deviation;
double min, max, scale, shift;
// Computes mean and std dev of image src
meanStdDev( src, mean, std_deviation);
// Computes the stretch min and max
min = mean.val[0] - 2.0 * std_deviation.val[0];
max = mean.val[0] + 2.0 * std_deviation.val[0];
if ( min != max )
{
scale = 255.0/(max - min);
shift = -min * scale;
}
else
{
scale = 1.0;
shift = 0.0;
}
// Computes:
// image = ((image-mind)/(maxd-mind))*255;
convertScaleAbs( src, dst, scale, shift );
return true;
}
KDvoid Laplace ( KDint nIdx )
{
Mat tSrc;
Mat tDst;
Mat tGray;
Mat tTemp;
KDint nSize;
KDint nScale;
KDint nDelta;
KDint nDepth;
// Load the source image
tSrc = imread ( "/res/image/lena.jpg" );
nSize = 3;
nScale = 1;
nDelta = 0;
nDepth = CV_16S;
// Reduce noise
GaussianBlur ( tSrc, tSrc, Size ( 3, 3 ), 0, 0, BORDER_DEFAULT );
// Convert it to gray
cvtColor ( tSrc, tGray, CV_RGB2GRAY );
// Apply Laplace function
Laplacian ( tGray, tTemp, nDepth, nSize, nScale, nDelta, BORDER_DEFAULT );
convertScaleAbs ( tTemp, tDst );
g_pController->setFrame ( 1, tSrc );
g_pController->setFrame ( 2, tDst );
}
void SpecificWorker::compute()
{
try
{
vector<string> names;
names.push_back("default");
RoboCompRGBDBus::ImageMap imgs;
rgbdbus_proxy->getImages(names, imgs);
Mat frame(imgs["default"].camera.colorHeight, imgs["default"].camera.colorWidth, CV_8UC3, &(imgs["default"].colorImage)[0]);
drawQtImage((const uchar *)(&(imgs["default"].colorImage)[0]), imgs["default"].camera.colorWidth, imgs["default"].camera.colorHeight, QImage::Format_RGB888, label);
/// Apply Laplace function
Mat gray, dst, abs_dst;
cvtColor( frame, gray, CV_RGB2GRAY );
Laplacian( gray, dst, CV_16S, 3, 1, 0, BORDER_DEFAULT );
convertScaleAbs( dst, abs_dst );
drawQtImage( (const uchar *)(abs_dst.data), imgs["default"].camera.colorWidth, imgs["default"].camera.colorHeight, QImage::Format_Indexed8, label_edges);
printFPS();
//imshow("img", frame);
}
catch(const Ice::Exception &e)
{
std::cout << "Error reading from Camera" << e << std::endl;
}
}
void my_Sobel(cv::Mat& srcImg, cv::Mat& dstImg)
{
Mat dst_x, dst_y;
Sobel(srcImg, dst_x, srcImg.depth(), 1, 0, 3);
Sobel(srcImg, dst_y, srcImg.depth(), 0, 1, 3);
convertScaleAbs(dst_x, dst_x);
convertScaleAbs(dst_y, dst_y);
addWeighted( dst_x, 0.5, dst_y, 0.5, 0, dstImg);
if(dstImg.channels() == 3)
{
cvtColor(dstImg, dstImg, CV_BGR2GRAY);
cvtColor(dstImg, dstImg, CV_GRAY2BGR);
}
}
void harrisCornerDetection() {
cv::Mat grad_x, grad_y;
cv::Mat abs_grad_x, abs_grad_y;
cv::Mat grad;
cv::Mat R(inputImage.rows,inputImage.cols,CV_64F);
double maxR = 0;
Sobel(inputImage,grad_x,CV_16S,1,0,3,1,0,cv::BORDER_DEFAULT);
Sobel(inputImage,grad_y,CV_16S,0,1,3,1,0,cv::BORDER_DEFAULT);
convertScaleAbs(grad_x,abs_grad_x);
convertScaleAbs(grad_y,abs_grad_y);
addWeighted(abs_grad_x,0.5,abs_grad_y,0.5,0,grad);
for (int x=0;x<inputImage.cols;x++) {
for (int y=0;y<inputImage.rows;y++) {
double A = 0;
double B = 0;
double C = 0;
for (int u=-templateSize/2;u<=templateSize/2;u++) {
for (int v=-templateSize/2;v<=templateSize/2;v++) {
int columnIndex = x+u;
int rowIndex = y+v;
if (columnIndex<0 || columnIndex>=inputImage.cols) {
continue;
}
if (rowIndex<0 || rowIndex>=inputImage.rows) {
continue;
}
A += pow(abs_grad_x.at<uchar>(rowIndex,columnIndex),2);
B += abs_grad_x.at<uchar>(rowIndex,columnIndex)*abs_grad_y.at<uchar>(rowIndex,columnIndex);
C += pow(abs_grad_y.at<uchar>(rowIndex,columnIndex),2);
}
}
double cornerness = A*C-B*B-0.04*(A+C);
R.at<double>(y,x) = cornerness;
maxR = std::max(cornerness,maxR);
}
}
cv::Mat annotatedImage;
cv::cvtColor(inputImage,annotatedImage,CV_GRAY2RGB);
for (int x=0;x<inputImage.cols;x++) {
for (int y=0;y<inputImage.rows;y++) {
if (R.at<double>(y,x) >= maxR*threshold) {
circle(annotatedImage,cvPoint(x,y),2,CV_RGB(0,0,255));
}
}
}
imshow("Harris Corner Detection",annotatedImage);
}
CMat& CMat::soble(){
Mat src_gray;
pic.copyTo(src_gray);
pic.release();
int scale = 1,delta = 0,ddepth = CV_16S;
Mat grad_x, grad_y;
Mat abs_grad_x, abs_grad_y;
Sobel(src_gray, grad_x, ddepth, 1, 0, 3, scale, delta, BORDER_DEFAULT);
convertScaleAbs(grad_x, abs_grad_x );
Sobel(src_gray, grad_y, ddepth, 0, 1, 3, scale, delta, BORDER_DEFAULT);
convertScaleAbs(grad_y, abs_grad_y);
addWeighted(abs_grad_x, 0.5, abs_grad_y, 0.5, 0, pic);
src_gray.release();
grad_x.release(), grad_y.release();
abs_grad_x.release(), abs_grad_y.release();
return (*this);
}
void edgeDetectLaplacian(Mat& img) {
GaussianBlur(img, img, Size(3, 3), 0, 0);
cvtColor(img, img, CV_RGB2GRAY);
Laplacian(img, img, CV_16S, 3, 2, 1, BORDER_DEFAULT);
threshold(img, img, 7, 255, THRESH_BINARY_INV);
convertScaleAbs(img, img);
dilate(img, img,
getStructuringElement(MORPH_ELLIPSE, Size(2, 2), Point(1, 1)));
cvtColor(img, img, CV_GRAY2BGR);
}
RawImage* DisparityMap::ProcessInput(CommandLineArgModel* arg, RawImage* image)
{
DisparityMapModel* model = (DisparityMapModel*)arg->ParsedModel;
Rectangle left, roi = model->Roi, right;
left.Width = roi.Width / 2;
right.Width = roi.Width / 2;
left.X = roi.X;
right.X = roi.X + left.Width;
left.Right = roi.Right - right.Width;
right.Right = roi.Right;
left.Height = right.Height = roi.Height;
left.Bottom = right.Bottom = roi.Bottom;
left.Y = right.Y = roi.Y;
Mat leftImg = image->CloneToMat(left);
Mat rightImg = image->CloneToMat(right);
cvtColor(leftImg, leftImg, CV_BGR2GRAY);
cvtColor(rightImg, rightImg, CV_BGR2GRAY);
Ptr<StereoSGBM> stereo = StereoSGBM::create(
0, //minDisparity
144, //numDisparities
3, //blockSize
3 * 3 * 4,
3 * 3 * 32,
1, //disp12MaxDiff
10, //preFilterCap
10, //uniquenessRatio
100, //speckleWindowSize
32, //speckleRange
true); //mode
Mat disparity;
stereo->compute(rightImg, leftImg, disparity);
double max, min;
minMaxIdx(disparity, &min, &max);
convertScaleAbs(disparity, disparity, 255 / max);
cvtColor(disparity, disparity, CV_GRAY2RGB);
imwrite("F:\\1\\raw-stereo.disparity.opencv.png", disparity);
RawImage* newImage = new RawImage(left.Width, left.Height);
newImage->Import(disparity, 0, 0);
return newImage;
}
Mat EdgeHandle::MatIllumination(Mat img){
Mat src, src_gray;
Mat grad;
int scale = 1;
int delta = 0;
int ddepth = CV_16S;
int c;
/// Load an image
src = img;
GaussianBlur( src, src, Size(3,3), 0, 0, BORDER_DEFAULT );
/// Convert it to gray
cvtColor( src, src_gray, CV_RGB2GRAY );
/// Generate grad_x and grad_y
Mat grad_x, grad_y;
Mat abs_grad_x, abs_grad_y;
//sobel
Sobel( src_gray, grad_x, ddepth, 1, 0, 3, scale, delta, BORDER_DEFAULT );
convertScaleAbs( grad_x, abs_grad_x );
Sobel( src_gray, grad_y, ddepth, 0, 1, 3, scale, delta, BORDER_DEFAULT );
convertScaleAbs( grad_y, abs_grad_y );
addWeighted( abs_grad_x, 0.5, abs_grad_y, 0.5, 0, grad );
// binary image.
//threshold(grad,grad,0,255,THRESH_BINARY);
// Apply the specified morphology operation
int morph_size = 1;
Mat element = getStructuringElement( 1, Size( 2*morph_size + 1, 2*morph_size+1 ), Point( morph_size, morph_size ) );
morphologyEx( grad, grad, 5, element );
morphologyEx( grad, grad, 5, element );
morphologyEx( grad, grad, 5, element );
return grad;
}
void FWImage::createGrad(int scale,int delta)
{
/// Generate grad_x and grad_y
Mat grad_x, grad_y;
Mat abs_grad_x, abs_grad_y;
int ddepth = CV_16S;
/// Gradient X
//Scharr( src_gray, grad_x, ddepth, 1, 0, scale, delta, BORDER_DEFAULT );
Sobel( this->iGray, grad_x, ddepth, 1, 0, 3, scale, delta, BORDER_DEFAULT );
convertScaleAbs( grad_x, abs_grad_x );
/// Gradient Y
//Scharr( src_gray, grad_y, ddepth, 0, 1, scale, delta, BORDER_DEFAULT );
Sobel( this->iGray, grad_y, ddepth, 0, 1, 3, scale, delta, BORDER_DEFAULT );
convertScaleAbs( grad_y, abs_grad_y );
/// Total Gradient (approximate)
addWeighted( abs_grad_x, 0.5, abs_grad_y, 0.5, 0, this->iGrad );
}
/**
* Efeito de pintura.
*/
void edgeDetectSobel(Mat& img) {
Mat gray, temp;
GaussianBlur(img, img, Size(3, 3), 0, 0);
cvtColor(img, gray, CV_RGB2GRAY);
//x
Sobel(gray, img, CV_16S, 1, 0, 3, 1.5, 0.3, BORDER_DEFAULT);
convertScaleAbs(img, img);
//y
Sobel(gray, temp, CV_16S, 0, 1, 3, 1.5, 0.4, BORDER_DEFAULT);
convertScaleAbs(temp, temp);
addWeighted(img, .3, temp, .3, .3, img);
erode(img, img,
getStructuringElement(MORPH_ELLIPSE,
Size(dilation_size + 1, dilation_size + 1),
Point(dilation_size, dilation_size)));
bitwise_not(img, img);
cvtColor(img, img, CV_GRAY2BGR);
}
/**
* @brief Applies the Laplacian edge transform
* @param[in] inImage [const cv::Mat&] Input image in CV_8UC1 format
* @param[out] outImage [cv::Mat*] The processed image in CV_8UC1 format
* @return void
*/
void applyLaplacian(const cv::Mat& inImage, cv::Mat* outImage)
{
//!< appropriate values for scale, delta and ddepth
int scale = 1;
int delta = 0; //!< the value for the non-edges
int ddepth = CV_16S;
cv::Mat edges;
inImage.copyTo(edges);
cvtColor( edges, edges, CV_RGB2GRAY );
cv::Laplacian(edges, *outImage, ddepth, 1, scale, delta, cv::BORDER_DEFAULT);
convertScaleAbs(*outImage, *outImage);
}
Mat gradient(Mat src) {
int scale = 1;
int delta = 0;
int ddepth = CV_16S;
Mat grad_x, grad_y;
Mat abs_grad_x, abs_grad_y, grad, gradh, gradl;
/// Gradient X
//Scharr( src_gray, grad_x, ddepth, 1, 0, scale, delta, BORDER_DEFAULT );
Sobel(src, grad_x, ddepth, 1, 0, 3, scale, delta, BORDER_DEFAULT);
convertScaleAbs(grad_x, abs_grad_x);
/// Gradient Y
//Scharr( src_gray, grad_y, ddepth, 0, 1, scale, delta, BORDER_DEFAULT );
Sobel(src, grad_y, ddepth, 0, 1, 3, scale, delta, BORDER_DEFAULT);
convertScaleAbs(grad_y, abs_grad_y);
/// Total Gradient (approximate)
addWeighted(abs_grad_x, 0.5, abs_grad_y, 0.5, 0, grad);
return grad;
}
Mat AntiShake::BorderDetector(Mat src, int type) {
Mat dst;
if (type == 0) {
// ----- Sobel:
Mat grad_x, grad_y, grad;
Mat abs_grad_x, abs_grad_y;
int ddepth = CV_16S;
int border = BORDER_ISOLATED;
Sobel(src, grad_x, ddepth, 1, 0, 3, 1, 1, border);
convertScaleAbs(grad_x, abs_grad_x);
Sobel(src, grad_y, ddepth, 0, 1, 3, 1, 1, border);
convertScaleAbs(grad_y, abs_grad_y);
addWeighted(abs_grad_x, 0.5, abs_grad_y, 0.5, 0, grad);
grad.copyTo(dst);
// threshold(dst, dst, 150, 255, cv::THRESH_TOZERO);
} else if (type == 1) {
// ----- Canny Corner Detector
Canny(src, dst, 200, 6000, 5, true);
} else if (type == 2) {
// ----- Laplacian:
Laplacian(src, dst, CV_8UC1, 1, 1, 1);
} else {
// ----- Magic Laplacian:
cv::Laplacian(src, dst, CV_32F, 9);
double scale = -1.0;
if (scale < 0) {
double lapmin, lapmax;
cv::minMaxLoc(dst, &lapmin, &lapmax);
scale = 255 / std::max(-lapmin, lapmax);
}
dst.convertTo(dst, CV_8U, scale, 256);
}
return dst;
}
cv::Mat laneTracker::cvLaplicain()
{
cv::Mat src, dst, abs_dst;
GaussianBlur(gray_, src, cv::Size(5,5), 0, 0);
int kernel_size = 3;
int scale = 1;
int delta = 0;
int ddepth = CV_16S;
Laplacian(src, dst, ddepth, kernel_size, scale, delta, cv::BORDER_DEFAULT);
convertScaleAbs(dst, abs_dst);
return abs_dst;
}
KDvoid CornerHarris ( KDint nIdx )
{
Mat tSrc;
Mat tGray;
KDint nThresh;
nThresh = 205;
// Load source image and convert it to gray
tSrc = imread ( "/res/image/building.png" );
cvtColor ( tSrc, tGray, CV_BGR2GRAY );
//
// Executes the corner detection and draw a circle around the possible corners
//
Mat tCorner;
Mat tNorm;
Mat tNormScaled;
// Detector parameters
KDint nBlockSize = 2;
KDint nApertureSize = 3;
KDdouble dK = 0.04;
tCorner = Mat::zeros ( tSrc.size ( ), CV_32FC1 );
// Detecting corners
cornerHarris ( tGray, tCorner, nBlockSize, nApertureSize, dK, BORDER_DEFAULT );
// Normalizing
normalize ( tCorner, tNorm, 0, 255, NORM_MINMAX, CV_32FC1, Mat ( ) );
convertScaleAbs ( tNorm, tNormScaled );
// Drawing a circle around corners
for ( KDint j = 0; j < tNorm.rows ; j++ )
{
for ( KDint i = 0; i < tNorm.cols; i++ )
{
if( (KDint) tNorm.at<KDfloat> ( j, i ) > nThresh )
{
circle ( tNormScaled, Point ( i, j ), 5, Scalar ( 0 ), 2, 8, 0 );
}
}
}
g_pController->setFrame ( 1, tSrc );
g_pController->setFrame ( 2, tNormScaled );
}
void harris(int thresh, Mat img, char *window_name, char *filename)
{
cout << filename << endl;
char *filename_cornerless = "harris_cornerless.jpg", *filename_final = "harris_final.jpg";
Mat dst, dst_norm, dst_norm_scaled;
Mat img_gray;
cvtColor(img, img_gray, CV_BGR2GRAY);
DWORD t1, t2, t;
dst = Mat::zeros(img.size(), CV_32FC1);
/// Detector parameters
int blockSize = 2;
int count = 0;
int apertureSize = 3;
double k = 0.04;
/// Detecting corners
t1 = ticks;
cornerHarris(img_gray, dst, blockSize, apertureSize, k, BORDER_DEFAULT);
t2 = ticks;
t = t2 - t1;
/// Normalizing
normalize(dst, dst_norm, 0, 255, NORM_MINMAX, CV_32FC1, Mat());
convertScaleAbs(dst_norm, dst_norm_scaled);
imwrite(filename_cornerless, dst_norm_scaled);
/// Drawing a circle around corners
for(int j = 0; j < dst_norm.rows ; j++)
{ for(int i = 0; i < dst_norm.cols; i++)
{
if((int) dst_norm.at<float>(j,i) > thresh)
{
circle(img, Point(i, j), 5, Scalar(0, 0, 255), 1, 8, 0);
count++;
}
}
}
/// Showing the result
cout << "Harris features detected: " << count << endl << "Time to execute: " << t << endl;
imwrite(filename, img);
namedWindow(window_name, CV_WINDOW_AUTOSIZE);
imshow(window_name, img);
}
vector<int> FRID::getFeaturePoints()
{
int i,j;
vector<int> r;
uint _order = getOrder();
int sz=w4*h4;
Mat sobelx, gradx, sobely,grady,sobel;
Mat sobelgray_,sobelgray__,sobelgray;
int totalPoints;
Sobel(orderMap[_order],sobelx,CV_16S,1,0,3,1, 0, BORDER_DEFAULT );
convertScaleAbs( sobelx, gradx );
Sobel(orderMap[_order],sobely,CV_16S,0,1,3,1, 0, BORDER_DEFAULT );
convertScaleAbs( sobely, grady );
addWeighted( gradx, 0.5, grady, 0.5, 0, sobel );
cvtColor(sobel,sobelgray_,CV_RGB2GRAY);
sobelgray_.convertTo(sobelgray__,CV_8UC1);
double min,max;
int maxid[2];
minMaxIdx(sobelgray__,&min,&max,0,maxid);
threshold(sobelgray__,sobelgray__,(int)(((float)max)/255*100),1,THRESH_BINARY);
totalPoints = sum(sobelgray__)[0];
cout<<"max: "<<max<<endl;
cout<<"totalPoints: "<<totalPoints<<endl;
double br;
double bg;
double bb;
double bsr;
double bsg;
double bsb;
int xmin,xmax,ymin,ymax;
//Mat bufABSmat;
//Mat bufABS2mat;
Mat result1;
Mat cdmat;
orderMap[_order].copyTo(cdmat);
for (i=0; i<sz; i++){
if (sobelgray__.at<uchar>(i/w4,i%w4) == 0) continue;
float* bufABS = getFeatureVector(i%w4,i/w4,br,bg,bb,bsr,bsg,bsb);
Mat bufABSmat ((_order/2-1)*3+3,1,CV_32FC1,bufABS);
//bufABSmat;
Mat corImg = Mat::zeros(Size(w4,h4),CV_8UC1);
xmin=w4-1,xmax=0,ymin=h4-1,ymax=0;
for (j=0; j<sz; j++){
if (sobelgray__.at<uchar>(j/w4,j%w4) == 0) continue;
float* bufABS2 = getFeatureVector(j%w4,j/w4,br,bg,bb,bsr,bsg,bsb);
Mat bufABS2mat ((_order/2-1)*3+3,1,CV_32FC1,bufABS2);
matchTemplate(bufABSmat, bufABS2mat, result1, CV_TM_CCOEFF_NORMED);
if (bsr>255 && bsg>255 && bsb>255 && pow(result1.at<float>(0),3)>0.5) {
if (xmin > j%w4) xmin = j%w4;
if (xmax < j%w4) xmax = j%w4;
if (ymin > j/w4) ymin = j/w4;
if (ymax < j/w4) ymax = j/w4;
corImg.at<uchar>(j) = 1;//(result1.at<float>(0) > 0)? pow(result1.at<float>(0),10):0;
if ((xmax-xmin)>6 && (ymax-ymin)>6)break;
}
}
// namedWindow( "c", CV_WINDOW_AUTOSIZE );
// imshow( "c", corImg*255);//thrCrCb[0] );
//
// waitKey(0);
if ((xmax-xmin)<=6 && (ymax-ymin)<=6) {
cout<<"xy: "<<i%w4<<", "<<i/w4<<endl;
cout<<"totalPoints: "<<sum(sobelgray__)[0]<<endl;
circle( cdmat, Point(i%w4,i/w4), 1, Scalar( 0, 0, 255 ), -1, 8 );
r.push_back(i%w4);
r.push_back(i/w4);
}
//sobelgray__=sobelgray__-corImg;
}
//circle( cdmat, Point(maxid[1],maxid[0]), 1, Scalar( 0, 0, 255 ), -1, 8 );
return r;
}
void Harris::get(){
cv::cornerHarris( image, imageHarris, BLOCK_SIZE, APERTURE, K, cv::BORDER_DEFAULT );
cv::normalize( imageHarris, imageHarris, 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat() );
convertScaleAbs( imageHarris, imageHarris);
}
/*
Point2f Recognition::RefineCornerPosition(Mat& img, Point2f ROICenter, vector<Point2f> refinedSquares){
vector<Point2f> refinedCorners;
//check if point to be refined is in an occluded part or there is a corner close!!
unsigned int i= 0;
bool found= false;
while(i< refinedSquares.size() && !found){
if(norm(ROICenter - refinedSquares[i]) < projectedSquareSize)
found= true;
i++;
}
//if found is true, there should be a corner any close and we refine the coordinates of it
if(found){
Mat srcGray;
cvtColor(img, srcGray, CV_BGR2GRAY);
Mat mask = Mat::zeros(img.size(), CV_8UC1);
int edgeLenght= projectedSquareSize-5;
int leftBoundX;
int rightBoundX;
int upperBoundY;
int lowerBoundY;
if((ROICenter.x- edgeLenght/2)< 0) leftBoundX= 0;
else leftBoundX= ROICenter.x- edgeLenght/2;
if((ROICenter.x+ edgeLenght/2)> img.cols) rightBoundX= img.cols;
else rightBoundX= ROICenter.x+ edgeLenght/2;
if((ROICenter.y- edgeLenght/2)< 0) upperBoundY= 0;
else upperBoundY= ROICenter.y- edgeLenght/2;
if((ROICenter.y+ edgeLenght/2)> img.rows) lowerBoundY= img.rows;
else lowerBoundY= ROICenter.y+ edgeLenght/2;
// set region of interest
for (int i= leftBoundX ; i< rightBoundX; i++){
for (int j= upperBoundY ; j< lowerBoundY; j++){
mask.row(j).col(i) = 1;
}
}
cornerHarris(img, OutputArray dst, int blockSize, int ksize, double k, int borderType=BORDER_DEFAULT )
goodFeaturesToTrack(srcGray, refinedCorners, 1, .1, 0.2, mask);
}
if(refinedCorners.empty())
return ROICenter;
else
return refinedCorners[0];
}
*/
vector<Point2f> Recognition::RefineCornerPositions(Mat& img, vector<Point2f> ROICenters, vector<Point2f> refinedSquares, int horizontalSize){
Mat srcGray;
Mat refinedCorners, refinedCorners_norm, refinedCorners_norm_scaled;
vector<Point2f> finalMesh;
int edgeLenght= projectedSquareSize-5;
cvtColor(img, srcGray, CV_BGR2GRAY);
cornerHarris(srcGray, refinedCorners, 2, 3, .04, BORDER_DEFAULT);
normalize( refinedCorners, refinedCorners_norm, 0, 255, NORM_MINMAX, CV_32FC1, Mat() );
convertScaleAbs( refinedCorners_norm, refinedCorners_norm_scaled );
for(unsigned int i=0; i< ROICenters.size(); i++){
int leftBoundX, rightBoundX, upperBoundY, lowerBoundY;
if((ROICenters[i].x- edgeLenght/2)< 0) leftBoundX= 0;
else leftBoundX= ROICenters[i].x- edgeLenght/2;
if((ROICenters[i].x+ edgeLenght/2)> img.cols) rightBoundX= img.cols;
else rightBoundX= ROICenters[i].x+ edgeLenght/2;
if((ROICenters[i].y- edgeLenght/2)< 0) upperBoundY= 0;
else upperBoundY= ROICenters[i].y- edgeLenght/2;
if((ROICenters[i].y+ edgeLenght/2)> img.rows) lowerBoundY= img.rows;
else lowerBoundY= ROICenters[i].y+ edgeLenght/2;
Point2f maxCorner(0,0);
for(int j=upperBoundY; j< lowerBoundY; j++){
for(int k=leftBoundX; k< rightBoundX; k++){
if( refinedCorners_norm.at<float>(j,k) > refinedCorners_norm.at<float>(maxCorner.y, maxCorner.x)){
maxCorner.y= j;
maxCorner.x= k;
}
}
}
//check if there is a square any close and return true if so
unsigned int s= 0;
bool found= false;
while(s< refinedSquares.size() && !found){
if(norm(ROICenters[i] - refinedSquares[s]) < projectedSquareSize*0.2)
found= true;
s++;
}
if(maxCorner.x!=0 && maxCorner.y!=0 && found){
if(i==0)
finalMesh.push_back(maxCorner);
else if(i%(horizontalSize+1)== 0){
if(fabs(finalMesh[i-(horizontalSize+1)].y - maxCorner.y) < 1.2*projectedSquareSize && fabs(finalMesh[i-(horizontalSize+1)].y - maxCorner.y)> .8*projectedSquareSize
&& maxCorner.x< 1.2* finalMesh[i-(horizontalSize+1)].x && maxCorner.x> .8* finalMesh[i-(horizontalSize+1)].x)
finalMesh.push_back(maxCorner);
else
finalMesh.push_back(Point2f(finalMesh[i-(horizontalSize+1)].x, finalMesh[i-(horizontalSize+1)].y+ projectedSquareSize));
}
else
if(fabs(finalMesh[i-1].x - maxCorner.x) < 1.2*projectedSquareSize && fabs(finalMesh[i-1].x - maxCorner.x)> .8*projectedSquareSize
&& maxCorner.y< 1.2* finalMesh[i-1].y && maxCorner.y> .8* finalMesh[i-1].y)
finalMesh.push_back(maxCorner);
else
finalMesh.push_back(Point2f(finalMesh[i-1].x + projectedSquareSize, finalMesh[i-1].y));
}
else{
if(i==0)
finalMesh.push_back(ROICenters[i]);
else if(i%(horizontalSize+1)== 0)
finalMesh.push_back(Point2f(finalMesh[i-(horizontalSize+1)].x, finalMesh[i-(horizontalSize+1)].y+ projectedSquareSize));
else
finalMesh.push_back(Point2f(finalMesh[i-1].x + projectedSquareSize, finalMesh[i-1].y));
//finalMesh.push_back(ROICenters[i]);
}
}
return finalMesh;
}
int main(int argc, char **argv) {
std::string image_str = std::string(argv[1]);
cv::Mat src = cv::imread(image_str, 1);
// convert to luminance
cv::Mat lum, lum_8;
cv::cvtColor(src, lum, CV_RGB2GRAY);
cv::convertScaleAbs(lum, lum_8);
// run sobel filter
cv::Mat sobel, sobel_abs, img_threshold;
cv::Sobel(lum_8, sobel, CV_16S, 1, 0, 3, 1, 0);
convertScaleAbs(sobel, sobel_abs);
cv::imwrite("raw_edge.bmp", sobel);
cv::imwrite("edge_abs.bmp", sobel_abs);
int processed_blocks = 0;
float threshold = T * (CHUNK_SIZE*CHUNK_SIZE);
cv::Size im_size = sobel.size();
int x_chunks = ceil(im_size.width / CHUNK_SIZE);
int y_chunks = ceil(im_size.height / CHUNK_SIZE);
std::vector<float> block_distortions;
// scan each 64*64 block
for (int x = 0; x < x_chunks; ++x) {
int cx = x*CHUNK_SIZE;
for (int y = 0; y < y_chunks; ++y) {
int cy = y*CHUNK_SIZE;
cv::Mat edge_chunk = sobel_abs(cv::Range(cy, cy+CHUNK_SIZE), cv::Range(cx, cx+CHUNK_SIZE));
// count the number of edges in the chunk
int edge_count = 0;
for (int i = 0; i < edge_chunk.rows; ++i) {
for (int j = 0; j < edge_chunk.cols; ++j) {
if (edge_chunk.at<unsigned char>(i, j) > 0) {
edge_count++;
}
}
}
// process further if chunk is edge chunk
if (edge_count > threshold) {
processed_blocks++;
cv::Mat lum_chunk = lum_8(cv::Range(cy, cy+CHUNK_SIZE), cv::Range(cx, cx+CHUNK_SIZE));
// estimage local contrast of edge and compute corresponding edge width
float contrast = local_contrast(lum_chunk);
int jnb_width = jnb_edge_width(contrast);
// for each edge compute corresponding edge width
std::vector<int> edge_widths;
for (int i = 0; i < edge_chunk.rows; ++i) {
cv::Mat row = lum_chunk.row(i);
for (int j = 0; j < edge_chunk.cols; ++j) {
if (edge_chunk.at<unsigned char>(i, j) > 0) {
int edge_width = Util::local_extrema(row, j);
edge_widths.push_back(edge_width);
}
}
}
// compute block distortion
float block_dist = get_block_distortion(edge_widths, jnb_width, BETA);
block_distortions.push_back(block_dist);
}
}
}
// obtain overall distortion by pooling over blocks
float total_distortion = get_image_distortion(block_distortions, BETA);
// return measures
float sharp_distortion = get_sharpness_measure(total_distortion, processed_blocks);
float blur_distortion = get_blurriness_measure(total_distortion, processed_blocks);
// output the calculated values
std::cout << "Sharpness measure: " << sharp_distortion << std::endl;
std::cout << "Blurriness measure: " << blur_distortion << std::endl;
return 0;
}
int main(int argc,char**argv)
{
int scale = 1;
int delta = 0;
int ddepth = CV_16S;
// check the number of parameter
if(argc !=2)
{
printf("please follow like this\n");
printf("exe[] img_name\n");
return -1;
}
// reads image
img_src = imread(argv[1]);
// check whether read operation is ok or not
if(img_src.data == NULL)
{
printf("could not open or find the image!\n");
return -1;
}
// use Gaussian blur to reduce the noise
GaussianBlur(img_src,img_src,Size(3,3),0,0,BORDER_DEFAULT);
// convert source image to gray image
cvtColor(img_src,img_gray,CV_BGR2GRAY);
// sobel in x direction
Sobel(img_gray,grad_x,ddepth,1,0,3,scale,delta,BORDER_DEFAULT);
convertScaleAbs(grad_x,abs_grad_x);
// use sobel in y direction
Sobel(img_gray,grad_y,ddepth,0,1,3,scale,delta,BORDER_DEFAULT);
convertScaleAbs(grad_y,abs_grad_y);
// add weight,and
addWeighted(abs_grad_x,0.5,abs_grad_y,0.5,0,grad);
// use threshold to binarition and threshold select use the OTSU method
threshold(grad,img_bin_thre,0,255,THRESH_BINARY|THRESH_OTSU);
// first Dilate,second erode
Mat element = getStructuringElement(MORPH_RECT,Size(2*1+1,2*1+1),Point(-1,-1));
for(int i = 0;i < 3; i++)
{
morphologyEx(img_bin_thre,img_bin_thre,MORPH_OPEN,element);
morphologyEx(img_bin_thre,img_bin_thre,MORPH_CLOSE,element);
}
// origin method ,this is worse than morphologyEx
// dilate(img_bin_thre,img_bin_thre,element);
// namedWindow("dilated",CV_WINDOW_NORMAL);
// imshow("dilated",img_bin_thre);
// erode(img_bin_thre,img_bin_thre,element);
// namedWindow("erode",CV_WINDOW_NORMAL);
// imshow("erode",img_bin_thre);
// find contour,in here must use the binarition image
// define
vector<Vec4i> hierarchy;
vector< vector<Point> >contours;
// use function
findContours(img_bin_thre,contours,hierarchy,CV_RETR_CCOMP,CV_CHAIN_APPROX_SIMPLE,Point(0,0));
// please change min and the max area value based on reality
int min_area = 100000;
int max_area = 300000;
Rect mRect;
int tempArea;
// define the color drawing contour
Scalar color = Scalar(255,255,0);
Mat drawing = Mat::zeros(img_bin_thre.size(),CV_8UC1);
for(int i = 0;i < contours.size();i++)
{
// get the minimum rectangle of the contours
mRect = boundingRect(contours[i]);
// computer the square of mRect
tempArea = mRect.height * mRect.width;
// for debug
// printf("tempArea.height:%d\ttempArea.width:%d\ttempArea.area=%d\n",mRect.height,mRect.width,tempArea);
// filter area which meet the requirement
if(((double)mRect.width/(double)mRect.height) > 2.0 && (tempArea > min_area) && ((double)mRect.width/(double)mRect.height < 4) && (tempArea < max_area))
// draw contours
{
drawContours(drawing,contours,i,color,2,8,hierarchy);
// here use 2 image ,one is just from image which be processed by threshold,the other is the original gray image,if you just use first,you
// may not
getRectSubPix(img_bin_thre,Size(mRect.width,mRect.height),Point(mRect.x+mRect.width/2,mRect.y\
+mRect.height/2),img_get_rect);
getRectSubPix(img_gray,Size(mRect.width,mRect.height),Point(mRect.x+mRect.width/2,mRect.y\
+mRect.height/2),img_get_rect_new);
}
}
if(img_get_rect.data == NULL)
{
printf("img_get rect is null\n");
return -1;
}
if(img_get_rect_new.data == NULL)
{
printf("img_get_rect_new is null!\n");
return -1;
}
// use the HoughLinesP
// define lines
vector<Vec4i> lines;
// Mat color_dst;
// img_lines = img_get_rect.clone();
cvtColor(img_get_rect,img_lines,CV_GRAY2BGR);
// check the line in image img_get_rect
HoughLinesP(img_get_rect,lines,1,CV_PI/180,200,200,10);
printf("lines.size()=%d\n",lines.size());
int distance = 0;
// int theta;
double temp_slope = 0,slope;
int res_x1,res_y1,res_x2,res_y2;
// define map vector for computer the line used frequency
// vector <int,int> ivect;//first is the number of this line , next is the longest distance
// map <double,ivect> imap;
int delta_x,delta_y;
std::vector <dou_int> ivec;
std::vector <dou_int>::iterator iter;
for(size_t i = 0;i < lines.size();i++)
{
Vec4i l = lines[i];
line(img_lines,Point(l[0],l[1]),Point(l[2],l[3]),Scalar(0,0,255),3);
// find tilt angle
if(l[2]-l[0] == 0)
;
else
{
// computer this line 's slope
// delta_x / delta_y
delta_y = (l[3]-l[1]);
delta_x = (l[2]-l[0]);
distance = delta_y*delta_y+delta_x*delta_x;
temp_slope = ((double)delta_y)/((double)(delta_x));
printf("in i=%d,delta_y=%d,delta_x=%d\n",i,delta_y,delta_x);
for(iter = ivec.begin();iter != ivec.end();iter++)
{
// if in one line,num++,update the max length
if(abs(iter->slope - temp_slope) < (double)0.01)
{
iter->num++;
if(iter->maxlength < distance)
{
iter->maxlength = distance;
iter->v0 = Point(l[0],l[1]);
iter->v1 = Point(l[2],l[3]);
}
break;
}
}
// not find this slope ,must add it by hand
if(iter == ivec.end())
{
ivec.push_back(dou_int(temp_slope,distance,1,Point(l[0],l[1]),Point(l[2],l[3])));
}
}
}
int max = 0;
int j = 0;
int index = 0;
dou_int res;
for(j=0,iter = ivec.begin();iter != ivec.end();j++,iter++)
{
if(iter->num > max)
{
max = iter->num;
index = j;
}
}
printf("index is %d\n",index);
for(j=0,iter = ivec.begin();iter != ivec.end() && j <= index;j++,iter++)
{
if(j == index)
{
res = dou_int(iter->slope,iter->maxlength,iter->num,iter->v0,iter->v1);
printf("slope is %f\n",iter->slope);
break;
}
}
// drawing the tilt line
line(img_lines,res.v0,res.v1,Scalar(255,255,0),1);
Mat img_lines_out;
Point center = Point(img_lines.cols/2,img_lines.rows/2);
double angle =(double)(180/CV_PI)*(double)atan(res.slope);
printf("angle is :%f\n",angle);
Mat rot_mat = getRotationMatrix2D(center,angle,1.0);
warpAffine(img_lines,img_lines_out,rot_mat,img_lines.size());
Mat img_rect;
warpAffine(img_get_rect_new,img_rect,rot_mat,img_get_rect_new.size());
cvtColor(img_lines_out,img_lines_out,CV_BGR2GRAY);
printf("img_clip's channel is:%d\n",img_lines_out.channels());
threshold(img_lines_out,img_lines_out,10,255,THRESH_BINARY | THRESH_OTSU);
Mat img_clip;
int up,down;
if(-1 != remove_Border_Vertical(img_lines_out,up,down))
{
printf("up=%d,down=%d\n",up,down);
getRectSubPix(img_lines_out,Size(img_lines_out.cols,down-up),Point(img_lines_out.cols/2,up+(down-up)/2),img_clip);
namedWindow("line_clip",CV_WINDOW_NORMAL);
imshow("line_clip",img_clip);
getRectSubPix(img_rect,Size(img_rect.cols,down-up),Point(img_rect.cols/2,up+(down-up)/2),img_clip);
namedWindow("new_clip",CV_WINDOW_NORMAL);
imshow("new_clip",img_clip);
}
// binarition OTSU
threshold(img_clip,img_clip,10,255,THRESH_BINARY | THRESH_OTSU);
namedWindow("newrect",CV_WINDOW_NORMAL);
imshow("newrect",img_clip);
parting_char(img_clip);
waitKey(0);
return 0;
}
void AndarPelaParedeAteLinha::execute(Robotino *robotino)
{
float Vx = 200, Vy, w, distParede;
float erroDist = 0;
int paredeAlvo = robotino->paredeAlvo();
static State<Robotino> * voltar;
static float a = std::sin(60*PI/180)/std::sin(80*PI/180);
static float cos20 = std::cos(20*PI/180);
static float K = R*(a-1);
static float erro_int = 0;
float e1 = robotino->irDistance(Robotino::IR_ESQUERDO_1);
float e2 = robotino->irDistance(Robotino::IR_ESQUERDO_2);
float ref_e1 = e2*a+K;
float d1 = robotino->irDistance(Robotino::IR_DIREITO_1);
float d2 = robotino->irDistance(Robotino::IR_DIREITO_2);
float ref_d1 = 1.15*(d2*a+K);
float distancia_da_esquerda, distancia_da_direita;
float erro;
vector<Vec4i> lines;
Vec4i l, l2;
Mat img, cdst;
int num_linha = 0;
int min_Hough = 70, dist_Hough = 50;
int min_canny =150 , max_canny = 3*min_canny;
distParede = robotino->getRefDistParede();
distParede += R;
img = robotino->getImage();
cvtColor( img, cdst, CV_BGR2GRAY );
Canny( cdst, cdst, (double)min_canny, (double)max_canny, 3 );
convertScaleAbs(cdst, cdst);
//cv::imshow("Canny",cdst);
//cv::waitKey(1);
threshold(cdst, cdst, (double)5, (double)255, CV_THRESH_BINARY);
HoughLinesP(cdst, lines, 1, CV_PI/180, min_Hough, min_Hough, dist_Hough );
cvtColor( cdst, cdst, CV_GRAY2BGR );
if (paredeAlvo == Robotino::NORTEN90 || paredeAlvo == Robotino::OESTE0 || paredeAlvo == Robotino::SUL90 || paredeAlvo == Robotino::LESTE180){
erro = (e1-ref_e1);
erro_int += erro*dt;
w = Kp*erro+Ki*erro_int;
distancia_da_esquerda = ((e1+ref_e1+2*R)*cos20)/2;
erroDist = (distancia_da_esquerda) - distParede;
Vy = Kpy*erroDist;
std::cout << "erro dist: " << erroDist << "\n";
std::cout<< "Esquerda 1: " << e1 << std::endl;
std::cout<< "RefEsquerda 1: " << ref_e1 << std::endl;
std::cout<< "Esquerda 2: " << e2 << std::endl;
std::cout << "Distância da esquerda: " << distancia_da_esquerda << "\n";
if (lines.size() > numeroLinhasMin){
if (paredeAlvo == Robotino::OESTE0) {
robotino->setOdometry(robotino->odometryX(),-(distancia_da_esquerda*10+15),0);
}
if (paredeAlvo == Robotino::NORTEN90) {
robotino->setOdometry((robotino->getAlturaMapa())*10 -(distancia_da_esquerda*10+15),robotino->odometryY(),-90);
}
if (paredeAlvo == Robotino::SUL90) {
robotino->setOdometry((distancia_da_esquerda*10+15),robotino->odometryY(),90);
}
if (paredeAlvo == Robotino::LESTE180) {
robotino->setOdometry(robotino->odometryX(),-((robotino->getLarguraMapa())*10 -(distancia_da_esquerda*10+15)),180);
}
}
}else if (paredeAlvo == Robotino::SULN90 || paredeAlvo == Robotino::LESTE0 || paredeAlvo == Robotino::NORTE90 || paredeAlvo == Robotino::OESTE180) {
erro = (d1-ref_d1);
erro_int += erro*dt;
w = -Kp*erro-Ki*erro_int;
distancia_da_direita = ((d1+ref_d1+2*R)*cos20)/2;
erroDist = distParede - ( distancia_da_direita );
Vy = Kpy*erroDist;
std::cout<< "Direita 1: " << d1 << std::endl;
std::cout<< "RefDireita 1: " << ref_d1 << std::endl;
std::cout<< "Direita 2: " << d2 << std::endl;
std::cout << "Distância da direita: " << distancia_da_direita << "\n";
if (lines.size() > numeroLinhasMin){
if (paredeAlvo == Robotino::SULN90) {
robotino->setOdometry((distancia_da_direita*10+15),robotino->odometryY(),-90);
}
if (paredeAlvo == Robotino::LESTE0) {
robotino->setOdometry(robotino->odometryX(),-((robotino->getLarguraMapa()) * 10-(distancia_da_direita*10+15)),0);
}
if (paredeAlvo == Robotino::NORTE90) {
robotino->setOdometry((robotino->getAlturaMapa()*10 - (distancia_da_direita*10+15)),robotino->odometryY(),90);
}
if (paredeAlvo == Robotino::OESTE180) {
robotino->setOdometry(robotino->odometryX(),-((distancia_da_direita*10+15)),180);
}
}
}
if(distParede > 99){
robotino->setVelocity(Vx,0,0);
}else{
robotino->setVelocity(Vx,Vy,w);
}
for( size_t i = 0; i < lines.size(); i++ ){
Vec4i l = lines[i];
//if (l[3] > 100 || l[1] > 100){
num_linha++;
//}
}
if (num_linha > numeroLinhasMin){
robotino->setVelocity(0,0,0);
robotino->change_state(robotino->previous_state());
}
}
//! 定位车牌图像
//! src 原始图像
//! resultVec 一个Mat的向量，存储所有抓取到的图像
//! 成功返回0，否则返回-1
int CPlateLocate::plateLocate(Mat src, vector<Mat>& resultVec)
{
Mat src_blur, src_gray;
Mat grad;
int scale = SOBEL_SCALE;
int delta = SOBEL_DELTA;
int ddepth = SOBEL_DDEPTH;
if( !src.data )
{ return -1; }
//高斯均衡。Size中的数字影响车牌定位的效果。
GaussianBlur( src, src_blur, Size(m_GaussianBlurSize, m_GaussianBlurSize),
0, 0, BORDER_DEFAULT );
/// Convert it to gray
cvtColor( src_blur, src_gray, CV_RGB2GRAY );
/// Generate grad_x and grad_y
Mat grad_x, grad_y;
Mat abs_grad_x, abs_grad_y;
/// Gradient X
//Scharr( src_gray, grad_x, ddepth, 1, 0, scale, delta, BORDER_DEFAULT );
Sobel( src_gray, grad_x, ddepth, 1, 0, 3, scale, delta, BORDER_DEFAULT );
convertScaleAbs( grad_x, abs_grad_x );
/// Gradient Y
//Scharr( src_gray, grad_y, ddepth, 0, 1, scale, delta, BORDER_DEFAULT );
Sobel( src_gray, grad_y, ddepth, 0, 1, 3, scale, delta, BORDER_DEFAULT );
convertScaleAbs( grad_y, abs_grad_y );
/// Total Gradient (approximate)
addWeighted( abs_grad_x, SOBEL_X_WEIGHT, abs_grad_y, SOBEL_Y_WEIGHT, 0, grad );
Mat img_threshold;
threshold(grad, img_threshold, 0, 255, CV_THRESH_OTSU+CV_THRESH_BINARY);
//threshold(grad, img_threshold, 75, 255, CV_THRESH_BINARY);
Mat element = getStructuringElement(MORPH_RECT, Size(m_MorphSizeWidth, m_MorphSizeHeight) );
morphologyEx(img_threshold, img_threshold, CV_MOP_CLOSE, element);
//Find 轮廓 of possibles plates
vector< vector< Point> > contours;
findContours(img_threshold,
contours, // a vector of contours
CV_RETR_EXTERNAL, // 提取外部轮廓
CV_CHAIN_APPROX_NONE); // all pixels of each contours
//Start to iterate to each contour founded
vector<vector<Point> >::iterator itc = contours.begin();
vector<RotatedRect> rects;
//Remove patch that are no inside limits of aspect ratio and area.
int t = 0;
while (itc != contours.end())
{
//Create bounding rect of object
RotatedRect mr = minAreaRect(Mat(*itc));
//large the rect for more
if( !verifySizes(mr))
{
itc = contours.erase(itc);
}
else
{
++itc;
rects.push_back(mr);
}
}
for(int i=0; i< rects.size(); i++)
{
RotatedRect minRect = rects[i];
if(verifySizes(minRect))
{
// rotated rectangle drawing
// Get rotation matrix
// 旋转这部分代码确实可以将某些倾斜的车牌调整正，
// 但是它也会误将更多正的车牌搞成倾斜！所以综合考虑，还是不使用这段代码。
// 2014-08-14,由于新到的一批图片中发现有很多车牌是倾斜的，因此决定再次尝试
// 这段代码。
float r = (float)minRect.size.width / (float)minRect.size.height;
float angle = minRect.angle;
Size rect_size = minRect.size;
if (r < 1)
{
angle = 90 + angle;
swap(rect_size.width, rect_size.height);
}
//如果抓取的方块旋转超过m_angle角度，则不是车牌，放弃处理
if (angle - m_angle < 0 && angle + m_angle > 0)
{
//Create and rotate image
Mat rotmat = getRotationMatrix2D(minRect.center, angle, 1);
Mat img_rotated;
warpAffine(src, img_rotated, rotmat, src.size(), CV_INTER_CUBIC);
Mat resultMat;
resultMat = showResultMat(img_rotated, rect_size, minRect.center);
resultVec.push_back(resultMat);
}
}
}
return 0;
}
void Normalized::hough_transform(Mat& im, Mat& orig, double* skew)
{
double max_r = sqrt(pow(.5*im.cols, 2) + pow(.5*im.rows, 2));
int angleBins = 180;
Mat acc = Mat::zeros(Size(2 * max_r, angleBins), CV_32SC1);
int cenx = im.cols / 2;
int ceny = im.rows / 2;
for (int x = 1; x<im.cols - 1; x++)
{
for (int y = 1; y<im.rows - 1; y++)
{
if (im.at<uchar>(y, x) == 255)
{
for (int t = 0; t<angleBins; t++)
{
double r = (x - cenx)*cos((double)t / angleBins*CV_PI) + (y - ceny)*sin((double)t / angleBins*CV_PI);
r += max_r;
acc.at<int>(t, int(r))++;
}
}
}
}
Mat thresh;
normalize(acc, acc, 255, 0, NORM_MINMAX);
convertScaleAbs(acc, acc);
/*debug
Mat cmap;
applyColorMap(acc,cmap,COLORMAP_JET);
imshow("cmap",cmap);
imshow("acc",acc);*/
Point maxLoc;
minMaxLoc(acc, 0, 0, 0, &maxLoc);
double theta = (double)maxLoc.y / angleBins*CV_PI;
double rho = maxLoc.x - max_r;
if (abs(sin(theta))<0.000001)//check vertical
{
//when vertical, line equation becomes
//x = rho
double m = -cos(theta) / sin(theta);
Point2d p1 = Point2d(rho + im.cols / 2, 0);
Point2d p2 = Point2d(rho + im.cols / 2, im.rows);
line(orig, p1, p2, Scalar(0, 0, 255), 1);
*skew = 90;
cout << "skew angle " << " 90" << endl;
}
else
{
//convert normal form back to slope intercept form
//y = mx + b
double m = -cos(theta) / sin(theta);
double b = rho / sin(theta) + im.rows / 2. - m*im.cols / 2.;
Point2d p1 = Point2d(0, b);
Point2d p2 = Point2d(im.cols, im.cols*m + b);
line(orig, p1, p2, Scalar(0, 0, 255), 1);
double skewangle;
skewangle = p1.x - p2.x>0 ? (atan2(p1.y - p2.y, p1.x - p2.x)*180. / CV_PI) : (atan2(p2.y - p1.y, p2.x - p1.x)*180. / CV_PI);
*skew = skewangle;
cout << "skew angle " << skewangle << endl;
}
}
