I'm trying to implement the method of improving fingerprint images by Anil Jain. As a starter, I encountered some difficulties while extracting the orientation image, and am strictly following those steps described in Section 2.4 of that paper.
So, this is the input image:
And this is after normalization using exactly the same method as in that paper:
I'm expecting to see something like this (an example from the internet):
However, this is what I got for displaying obtained orientation matrix:
Obviously this is wrong, and it also gives non-zero values for those zero points in the original input image.
This is the code I wrote:
cv::Mat orientation(cv::Mat inputImage)
{
cv::Mat orientationMat = cv::Mat::zeros(inputImage.size(), CV_8UC1);
// compute gradients at each pixel
cv::Mat grad_x, grad_y;
cv::Sobel(inputImage, grad_x, CV_16SC1, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT);
cv::Sobel(inputImage, grad_y, CV_16SC1, 0, 1, 3, 1, 0, cv::BORDER_DEFAULT);
cv::Mat Vx, Vy, theta, lowPassX, lowPassY;
cv::Mat lowPassX2, lowPassY2;
Vx = cv::Mat::zeros(inputImage.size(), inputImage.type());
Vx.copyTo(Vy);
Vx.copyTo(theta);
Vx.copyTo(lowPassX);
Vx.copyTo(lowPassY);
Vx.copyTo(lowPassX2);
Vx.copyTo(lowPassY2);
// estimate the local orientation of each block
int blockSize = 16;
for(int i = blockSize/2; i < inputImage.rows - blockSize/2; i+=blockSize)
{
for(int j = blockSize / 2; j < inputImage.cols - blockSize/2; j+= blockSize)
{
float sum1 = 0.0;
float sum2 = 0.0;
for ( int u = i - blockSize/2; u < i + blockSize/2; u++)
{
for( int v = j - blockSize/2; v < j+blockSize/2; v++)
{
sum1 += grad_x.at<float>(u,v) * grad_y.at<float>(u,v);
sum2 += (grad_x.at<float>(u,v)*grad_x.at<float>(u,v)) * (grad_y.at<float>(u,v)*grad_y.at<float>(u,v));
}
}
Vx.at<float>(i,j) = sum1;
Vy.at<float>(i,j) = sum2;
double calc = 0.0;
if(sum1 != 0 && sum2 != 0)
{
calc = 0.5 * atan(Vy.at<float>(i,j) / Vx.at<float>(i,j));
}
theta.at<float>(i,j) = calc;
// Perform low-pass filtering
float angle = 2 * calc;
lowPassX.at<float>(i,j) = cos(angle * pi / 180);
lowPassY.at<float>(i,j) = sin(angle * pi / 180);
float sum3 = 0.0;
float sum4 = 0.0;
for(int u = -lowPassSize / 2; u < lowPassSize / 2; u++)
{
for(int v = -lowPassSize / 2; v < lowPassSize / 2; v++)
{
sum3 += inputImage.at<float>(u,v) * lowPassX.at<float>(i - u*lowPassSize, j - v * lowPassSize);
sum4 += inputImage.at<float>(u, v) * lowPassY.at<float>(i - u*lowPassSize, j - v * lowPassSize);
}
}
lowPassX2.at<float>(i,j) = sum3;
lowPassY2.at<float>(i,j) = sum4;
float calc2 = 0.0;
if(sum3 != 0 && sum4 != 0)
{
calc2 = 0.5 * atan(lowPassY2.at<float>(i, j) / lowPassX2.at<float>(i, j)) * 180 / pi;
}
orientationMat.at<float>(i,j) = calc2;
}
}
return orientationMat;
}
I've already searched a lot on the web, but almost all of them are in Matlab. And there exist very few ones using OpenCV, but they didn't help me either. I sincerely hope someone could go through my code and point out any error to help. Thank you in advance.
Update
Here are the steps that I followed according to the paper:
Obtain normalized image G.
Divide G into blocks of size wxw (16x16).
Compute the x and y gradients at each pixel (i,j).
Estimate the local orientation of each block centered at pixel (i,j) using equations:
Perform low-pass filtering to remove noise. For that, convert the orientation image into a continuous vector field defined as:
where W is a two-dimensional low-pass filter, and w(phi) x w(phi) is its size, which equals to 5.
Finally, compute the local ridge orientation at (i,j) using:
Update2
This is the output of orientationMat after changing the mat type to CV_16SC1 in Sobel operation as Micka suggested:
Maybe it's too late for me to answer, but anyway somebody could read this later and solve the same problem.
I've been working for a while in the same algorithm, same method you posted... But there's some writting errors when the papper was redacted (I guess). After fighting a lot with the equations I found this errors by looking other similar works.
Here is what worked for me...
Vy(i, j) = 2*dx(u,v)*dy(u,v)
Vx(i,j) = dx(u,v)^2 - dy(u,v)^2
O(i,j) = 0.5*arctan(Vy(i,j)/Vx(i,j)
(Excuse me I wasn't able to post images, so I wrote the modified ecuations. Remeber "u" and "v" are positions of the summation across the BlockSize by BlockSize window)
The first thing and most important (obviously) are the equations, I saw that in different works this expressions were really different and in every one they talked about the same algorithm of Hong et al.
The Key is finding the Least Mean Square (First 3 equations) of the gradients (Vx and Vy), I provided the corrected formulas above for this ation. Then you can compute angle theta for the non overlapping window (16x16 size recommended in the papper), after that the algorithm says you must calculate the magnitud of the doubled angle in "x" and "y" directions (Phi_x and Phi_y).
Phi_x(i,j) = V(i,j) * cos(2*O(i,j))
Phi_y(i,j) = V(i,j) * sin(2*O(i,j))
Magnitud is just:
V = sqrt(Vx(i,j)^2 + Vy(i,j)^2)
Note that in the related work doesn't mention that you have to use the gradient magnitud, but it make sense (for me) in doing it. After all this corrections you can apply the low pass filter to Phi_x and Phi_y, I used a simple Mask of size 5x5 to average this magnitudes (something like medianblur() of opencv).
Last thing is to calculate new angle, that is the average of the 25ith neighbors in the O(i,j) image, for this you just have to:
O'(i,j) = 0.5*arctan(Phi_y/Phi_x)
We're just there... All this just for calculating the angle of the NORMAL VECTOR TO THE RIDGES DIRECTIONS (O'(i,j)) in the BlockSize by BlockSize non overlapping window, what does it mean? it means that the angle we just calculated is perpendicular to the ridges, in simple words we just calculated the angle of the riges plus 90 degrees... To get the angle we need, we just have to substract to the obtained angle 90°.
To draw the lines we need to have an initial point (X0, Y0) and a final point(X1, Y1). For that imagine a circle centered on (X0, Y0) with a radious of "r":
x0 = i + blocksize/2
y0 = j + blocksize/2
r = blocksize/2
Note we add i and j to the first coordinates becouse the window is moving and we are gonna draw the line starting from the center of the non overlaping window, so we can't use just the center of the non overlaping window.
Then to calculate the end coordinates to draw a line we can just have to use a right triangle so...
X1 = r*cos(O'(i,j)-90°)+X0
Y1 = r*sin(O'(i,j)-90°)+Y0
X2 = X0-r*cos(O'(i,j)-90°)
Y2 = Y0-r*cos(O'(i,j)-90°)
Then just use opencv line function, where initial Point is (X0,Y0) and final Point is (X1, Y1). Additional to it, I drawed the windows of 16x16 and computed the oposite points of X1 and Y1 (X2 and Y2) to draw a line of the entire window.
Hope this help somebody.
My results...
Main function:
Mat mat = imread("nwmPa.png",0);
mat.convertTo(mat, CV_32F, 1.0/255, 0);
Normalize(mat);
int blockSize = 6;
int height = mat.rows;
int width = mat.cols;
Mat orientationMap;
orientation(mat, orientationMap, blockSize);
Normalize:
void Normalize(Mat & image)
{
Scalar mean, dev;
meanStdDev(image, mean, dev);
double M = mean.val[0];
double D = dev.val[0];
for(int i(0) ; i<image.rows ; i++)
{
for(int j(0) ; j<image.cols ; j++)
{
if(image.at<float>(i,j) > M)
image.at<float>(i,j) = 100.0/255 + sqrt( 100.0/255*pow(image.at<float>(i,j)-M,2)/D );
else
image.at<float>(i,j) = 100.0/255 - sqrt( 100.0/255*pow(image.at<float>(i,j)-M,2)/D );
}
}
}
Orientation map:
void orientation(const Mat &inputImage, Mat &orientationMap, int blockSize)
{
Mat fprintWithDirectionsSmoo = inputImage.clone();
Mat tmp(inputImage.size(), inputImage.type());
Mat coherence(inputImage.size(), inputImage.type());
orientationMap = tmp.clone();
//Gradiants x and y
Mat grad_x, grad_y;
// Sobel(inputImage, grad_x, CV_32F, 1, 0, 3, 1, 0, BORDER_DEFAULT);
// Sobel(inputImage, grad_y, CV_32F, 0, 1, 3, 1, 0, BORDER_DEFAULT);
Scharr(inputImage, grad_x, CV_32F, 1, 0, 1, 0);
Scharr(inputImage, grad_y, CV_32F, 0, 1, 1, 0);
//Vector vield
Mat Fx(inputImage.size(), inputImage.type()),
Fy(inputImage.size(), inputImage.type()),
Fx_gauss,
Fy_gauss;
Mat smoothed(inputImage.size(), inputImage.type());
// Local orientation for each block
int width = inputImage.cols;
int height = inputImage.rows;
int blockH;
int blockW;
//select block
for(int i = 0; i < height; i+=blockSize)
{
for(int j = 0; j < width; j+=blockSize)
{
float Gsx = 0.0;
float Gsy = 0.0;
float Gxx = 0.0;
float Gyy = 0.0;
//for check bounds of img
blockH = ((height-i)<blockSize)?(height-i):blockSize;
blockW = ((width-j)<blockSize)?(width-j):blockSize;
//average at block WхW
for ( int u = i ; u < i + blockH; u++)
{
for( int v = j ; v < j + blockW ; v++)
{
Gsx += (grad_x.at<float>(u,v)*grad_x.at<float>(u,v)) - (grad_y.at<float>(u,v)*grad_y.at<float>(u,v));
Gsy += 2*grad_x.at<float>(u,v) * grad_y.at<float>(u,v);
Gxx += grad_x.at<float>(u,v)*grad_x.at<float>(u,v);
Gyy += grad_y.at<float>(u,v)*grad_y.at<float>(u,v);
}
}
float coh = sqrt(pow(Gsx,2) + pow(Gsy,2)) / (Gxx + Gyy);
//smoothed
float fi = 0.5*fastAtan2(Gsy, Gsx)*CV_PI/180;
Fx.at<float>(i,j) = cos(2*fi);
Fy.at<float>(i,j) = sin(2*fi);
//fill blocks
for ( int u = i ; u < i + blockH; u++)
{
for( int v = j ; v < j + blockW ; v++)
{
orientationMap.at<float>(u,v) = fi;
Fx.at<float>(u,v) = Fx.at<float>(i,j);
Fy.at<float>(u,v) = Fy.at<float>(i,j);
coherence.at<float>(u,v) = (coh<0.85)?1:0;
}
}
}
} ///for
GaussConvolveWithStep(Fx, Fx_gauss, 5, blockSize);
GaussConvolveWithStep(Fy, Fy_gauss, 5, blockSize);
for(int m = 0; m < height; m++)
{
for(int n = 0; n < width; n++)
{
smoothed.at<float>(m,n) = 0.5*fastAtan2(Fy_gauss.at<float>(m,n), Fx_gauss.at<float>(m,n))*CV_PI/180;
if((m%blockSize)==0 && (n%blockSize)==0){
int x = n;
int y = m;
int ln = sqrt(2*pow(blockSize,2))/2;
float dx = ln*cos( smoothed.at<float>(m,n) - CV_PI/2);
float dy = ln*sin( smoothed.at<float>(m,n) - CV_PI/2);
arrowedLine(fprintWithDirectionsSmoo, Point(x, y+blockH), Point(x + dx, y + blockW + dy), Scalar::all(255), 1, CV_AA, 0, 0.06*blockSize);
// qDebug () << Fx_gauss.at<float>(m,n) << Fy_gauss.at<float>(m,n) << smoothed.at<float>(m,n);
// imshow("Orientation", fprintWithDirectionsSmoo);
// waitKey(0);
}
}
}///for2
normalize(orientationMap, orientationMap,0,1,NORM_MINMAX);
imshow("Orientation field", orientationMap);
orientationMap = smoothed.clone();
normalize(smoothed, smoothed, 0, 1, NORM_MINMAX);
imshow("Smoothed orientation field", smoothed);
imshow("Coherence", coherence);
imshow("Orientation", fprintWithDirectionsSmoo);
}
seems nothing forgot )
I have read your code thoroughly and found that you have made a mistake while calculating sum3 and sum4:
sum3 += inputImage.at<float>(u,v) * lowPassX.at<float>(i - u*lowPassSize, j - v * lowPassSize);
sum4 += inputImage.at<float>(u, v) * lowPassY.at<float>(i - u*lowPassSize, j - v * lowPassSize);
instead of inputImage you should use a low pass filter.
Related
My english is not my native language, I have no idea how title it, how to explain it clearly and not sure if it's the right term. I've tried to search on google first but for the reason quoted above, I couldn't find anything related.
Could you guys first please check the imgur album : https://imgur.com/a/4mMuCil
So...
Black square is an "obstacle"
Red Square is a "player"
Grey square are "area where player is not able to see"
Depending on the distance of the player from the obstacle, the player can see more or less "things"
Is there a general formula to determine the area that he can see or can't see ?
Or I have to write a unique formula depending on the player position relative to the obstacle
I'm sorry If what I wrote doesn't make sense
Thanks for your help
EDIT :
point player(5, 0);
point obstacle(4, 2);
.....o...
.........
....#....
.........
.....#...
.....##..
.....###.
.....####
......###
This needs a little work. I'll say the steps to make it work:
(1) Let's define the player position p and the obstacle position o.
We know we want to paint a "triangle" after the obstacle.
(2) Let's define the angle according to proximity.
The nearer, the bigger the angle is, so I set the angle to 45 if the distance is 1 and it decreases as the player gets further from the obstacle. The angle is 5 + (max(0, 50 - distance * 10)). You can tune this angle.
(3) Let's build a big triangle. The first vertex is the obstacle. Then, throw a big line from the player through the obstacle. Rotate this line around the obstacle half angle clockwise (to get the second vertex) and half angle anti-clockwise (to get the third vertex), as shown in the image:
(4) Lastly, iterate to the matrix and for each position, ask if that coordinates are inside the triangle.
#include <bits/stdc++.h>
using namespace std;
struct point{
float x, y;
point(){}
point(float x, float y){this->x = x; this->y = y;}
//point(int x, int y){this->x = x; this->y = y;}
};
point rotate(point pivot, float angle, point p, bool clockwise){
float s = sin(angle);
float c = cos(angle);
p.x -= pivot.x;
p.y -= pivot.y;
if(clockwise){
return point(p.x * c + p.y * s + pivot.x, -p.x * s + p.y * c + pivot.y);
}
else{
return point(p.x * c - p.y * s + pivot.x, p.x * s + p.y * c + pivot.y);
}
}
float triangleArea(point p1, point p2, point p3) { //find area of triangle formed by p1, p2 and p3
return abs((p1.x*(p2.y-p3.y) + p2.x*(p3.y-p1.y)+ p3.x*(p1.y-p2.y))/2.0);
}
bool inside(point p1, point p2, point p3, point p) { //check whether p is inside or outside
float area = triangleArea (p1, p2, p3); //area of triangle ABC
float area1 = triangleArea (p, p2, p3); //area of PBC
float area2 = triangleArea (p1, p, p3); //area of APC
float area3 = triangleArea (p1, p2, p); //area of ABP
return abs(area - area1 + area2 + area3) < 1; //when three triangles are forming the whole triangle
}
char m[9][9];
point player(4, 0);
point obstacle(4, 2);
float angle(){
float dist = sqrt(pow(player.x - obstacle.x, 2) + pow(player.y - obstacle.y, 2));
cout<<"dist: "<<(5.0 + max(0.0, 50.0 - 10.0 * dist))<<endl;
return (5.0 + max(0.0, 50.0 - 10.0 * dist)) * 0.0174533;
}
void print(){
for(int i = 0; i < 9; i++){
for(int j = 0; j < 9; j++){
cout<<m[i][j];
}
cout<<endl;
}
}
int main(){
for(int i = 0; i < 9; i++){
for(int j = 0; j < 9; j++){
m[i][j] = '.';
}
}
m[(int)player.y][(int)player.x] = 'o';
m[(int)obstacle.y][(int)obstacle.x] = '#';
float rad = angle();
point end(20.0 * (obstacle.x - player.x) + obstacle.x, 20.0 * (player.y - obstacle.y) + obstacle.y);
point p2 = rotate(obstacle, rad / 2.0, end, true);
point p3 = rotate(obstacle, rad / 2.0, end, false);
for(int i = 0; i < 9; i++){
for(int j = 0; j < 9; j++){
if(i == (int) player.y && j == (int) player.x) continue;
if(i == (int) obstacle.y && j == (int) obstacle.x) continue;
if(inside(obstacle, p2, p3, point(j, i))) m[i][j] = '#';
}
}
print();
return 0;
}
OUTPUT:
....o....
.........
....#....
....#....
....#....
....#....
...###...
...###...
...###...
Suppose the player is at (0,0), and the obstacle is at (j,k), with j>0 and k>=0.
Then a square at (x, y) will be visible if either (2j-1)y >= (2k+1)x or (2k-1)x >= (2j+1)y.
Applying this rule to the other three quadrants is straightforward.
I'm trying to implement an algorithm to detect lanes, I use gabor filter in order to detect edges and line then apply hough transform but I've a problem as it gives exception when trying to either do canny detection or houghline directly
I want to know what is the output of Gabor filter is it complex values ? and how to work with the complex values in opencv?
here is a part of my code :
int kernel_size = 9;
double sig = 2, th = 0, lm =10, gm =1, ps = CV_PI/4;
cv::Mat kernel = cv::getGaborKernel(cv::Size(kernel_size,kernel_size), sig, th, lm, gm, ps);
cv::filter2D(src_f, src_f, CV_32F, kernel);
imshow("kernel",kernel);
Mat viz;
src_f.convertTo(viz,CV_8U,1.0/255.0);
imshow("d",viz);
imshow("Result", src_f);
std::vector<float> uniquev = unique(src_f, true);
int TotalPixel = uniquev.size();
int nTotalThresholdPixel = TotalPixel * 2.5/100;
float thresholdValue = uniquev[nTotalThresholdPixel-1];
int i = 0;
for (int row = 0; row < MaxHeight; row++)
{
for (int col = 0; col < MaxWidth; col++)
{
if(src_f.at<float>(row, col) <thresholdValue )
{src_f.at<float>(row, col) =0;}
}
}
Mat imgContours;
double thresh = 255;
try{
Canny(src_f,imgContours,0.6*thresh, thresh);
vector<Vec2f> lines;
HoughLines(src_f,lines,1,CV_PI/180,130);
Mat imgOutput;
cvtColor( src_f, imgOutput, CV_GRAY2BGR );
for( size_t i = 0; i < lines.size(); i++ )
{
float theta = lines[i][1];
float rho = lines[i][0];
double a = cos(theta), b = sin(theta);
double x0 = a*rho, y0 = b*rho;
Point pt1(cvRound(x0 + 1000*(-b)),cvRound(y0 + 1000*(a)));
Point pt2(cvRound(x0 - 1000*(-b)),
cvRound(y0 - 1000*(a)));
line( src_f, pt1, pt2, Scalar(0,0,255), 1, 8 );
}
}
catch(exception e ){
}
I think I've a problem in the output of gabor filter itself as it is float and -ve numbers
I got the exception here either in
Canny(src_f,imgContours,0.6*thresh, thresh);
or in if commented Canny
HoughLines(src_f,lines,1,CV_PI/180,130);
I'm not sure if it is possible in processing but I would like to be able to zoom in on the fractal without it being extremely laggy and buggy. What I currently have is:
int maxIter = 100;
float zoom = 1;
float x0 = width/2;
float y0 = height/2;
void setup(){
size(500,300);
noStroke();
smooth();
}
void draw(){
translate(x0, y0);
scale(zoom);
for(float Py = 0; Py < height; Py++){
for(float Px = 0; Px < width; Px++){
// scale pixel coordinates to Mandelbrot scale
float w = width;
float h = height;
float xScaled = (Px * (3.5/w)) - 2.5;
float yScaled = (Py * (2/h)) - 1;
float x = 0;
float y = 0;
int iter = 0;
while( x*x + y*y < 2*2 && iter < maxIter){
float tempX = x*x - y*y + xScaled;
y = 2*x*y + yScaled;
x = tempX;
iter += 1;
}
// color pixels
color c;
c = pickColor(iter);
rect(Px, Py,1,1);
fill(c);
}
}
}
// pick color based on time pixel took to escape (number of iterations through loop)
color pickColor(int iters){
color b = color(0,0,0);
if(iters == maxIter) return b;
int l = 1;
color[] colors = new color[maxIter];
for(int i = 0; i < colors.length; i++){
switch(l){
case 1 : colors[i] = color(255,0,0); break;
case 2 : colors[i] = color(0,0,255); break;
case 3 : colors[i] = color(0,255,0); break;
}
if(l == 1 || l == 2) l++;
else if(l == 3) l = 1;
else l--;
}
return colors[iters];
}
// allow zooming in and out
void mouseWheel(MouseEvent event){
float direction = event.getCount();
if(direction < 0) zoom += .02;
if(direction > 0) zoom -= .02;
}
// allow dragging back and forth to change view
void mouseDragged(){
x0+= mouseX-pmouseX;
y0+= mouseY-pmouseY;
}
but it doesn't work very well. It works alright at the size and max iteration I have it set to now (but still not well) and is completely unusable at larger sizes or higher maximum iterations.
The G4P library has an example that does exactly this. Download the library and go to the G4P_MandelBrot example. The example can be found online here.
Hope this helps!
Given a distance transform of a map with obstacles in it, how do I get the least cost path from a start pixel to a goal pixel? The distance transform image has the distance(euclidean) to the nearest obstacle of the original map, in each pixel i.e. if in the original map pixel (i,j) is 3 pixels away to the left and 2 pixels away to the down of an obstacle, then in the distance transform the pixel will have sqrt(4+9) = sqrt(13) as the pixel value. Thus darker pixels signify proximity to obstacles and lighter values signify that they are far from obstacles.
I want to plan a path from a given start to goal pixel using the information provided by this distance transform and optimize the cost of the path and also there is another constraint that the path should never reach a pixel which is less than 'x' pixels away from an obstacle.
How do I go about this?
P.S. A bit of description on the algorithm (or a bit of code) would be helpful as I am new to planning algorithms.
I found an algorithm in the appendix I of the chapter titled
JARVIS, Ray. Distance transform based path planning for robot navigation. Recent trends in mobile robots, 1993, 11: 3-31.
That chapter is fully visible to me in Google books and the book is
ZHENG, Yuang F. (ed.). Recent trends in mobile robots. World Scientific, 1993.
A C++ implementation of the algorithm follows:
#include <vector>
#include <iostream>
#include <cmath>
#include <algorithm>
#include <cassert>
#include <sstream>
/**
Algorithm in the appendix I of the chapter titled
JARVIS, Ray. Distance transform based path planning for robot navigation. *Recent trends in mobile robots*, 1993, 11: 3-31.
in the book
ZHENG, Yuang F. (ed.). *Recent trends in mobile robots*. World Scientific, 1993.
See also http://stackoverflow.com/questions/21215244/least-cost-path-using-a-given-distance-transform
*/
template < class T >
class Matrix
{
private:
int m_width;
int m_height;
std::vector<T> m_data;
public:
Matrix(int width, int height) :
m_width(width), m_height(height), m_data(width *height) {}
int width() const
{
return m_width;
}
int height() const
{
return m_height;
}
void set(int x, int y, const T &value)
{
m_data[x + y * m_width] = value;
}
const T &get(int x, int y) const
{
return m_data[x + y * m_width];
}
};
float distance( const Matrix< float > &a, const Matrix< float > &b )
{
assert(a.width() == b.width());
assert(a.height() == b.height());
float r = 0;
for ( int y = 0; y < a.height(); y++ )
{
for ( int x = 0; x < a.width(); x++ )
{
r += fabs(a.get(x, y) - b.get(x, y));
}
}
return r;
}
int PPMGammaEncode(float radiance, float d)
{
//return int(std::pow(std::min(1.0f, std::max(0.0f, radiance * d)),1.0f / 2.2f) * 255.0f);
return radiance;
}
void PPM_image_save(const Matrix<float> &img, const std::string &filename, float d = 15.0f)
{
FILE *file = fopen(filename.c_str(), "wt");
const int m_width = img.width();
const int m_height = img.height();
fprintf(file, "P3 %d %d 255\n", m_width, m_height);
for (int y = 0; y < m_height; ++y)
{
fprintf(file, "\n# y = %d\n", y);
for (int x = 0; x < m_width; ++x)
{
const float &c(img.get(x, y));
fprintf(file, "%d %d %d\n",
PPMGammaEncode(c, d),
PPMGammaEncode(c, d),
PPMGammaEncode(c, d));
}
}
fclose(file);
}
void PPM_image_save(const Matrix<bool> &img, const std::string &filename, float d = 15.0f)
{
FILE *file = fopen(filename.c_str(), "wt");
const int m_width = img.width();
const int m_height = img.height();
fprintf(file, "P3 %d %d 255\n", m_width, m_height);
for (int y = 0; y < m_height; ++y)
{
fprintf(file, "\n# y = %d\n", y);
for (int x = 0; x < m_width; ++x)
{
float v = img.get(x, y) ? 255 : 0;
fprintf(file, "%d %d %d\n",
PPMGammaEncode(v, d),
PPMGammaEncode(v, d),
PPMGammaEncode(v, d));
}
}
fclose(file);
}
void add_obstacles(Matrix<bool> &m, int n, int avg_lenght, int sd_lenght)
{
int side = std::max(3, std::min(m.width(), m.height()) / 10);
for ( int y = m.height() / 2 - side / 2; y < m.height() / 2 + side / 2; y++ )
{
for ( int x = m.width() / 2 - side / 2; x < m.width() / 2 + side / 2; x++ )
{
m.set(x, y, true);
}
}
/*
for ( int y = m.height()/2-side/2; y < m.height()/2+side/2; y++ ) {
for ( int x = 0; x < m.width()/2+side; x++ ) {
m.set(x,y,true);
}
}
*/
for ( int y = 0; y < m.height(); y++ )
{
m.set(0, y, true);
m.set(m.width() - 1, y, true);
}
for ( int x = 0; x < m.width(); x++ )
{
m.set(x, 0, true);
m.set(x, m.height() - 1, true);
}
}
class Info
{
public:
Info() {}
Info(float v, int x_o, int y_o): value(v), x_offset(x_o), y_offset(y_o) {}
float value;
int x_offset;
int y_offset;
bool operator<(const Info &rhs) const
{
return value < rhs.value;
}
};
void next(const Matrix<float> &m, const int &x, const int &y, int &x_n, int &y_n)
{
//todo: choose the diagonal adiacent in case of ties.
x_n = x;
y_n = y;
Info neighbours[8];
neighbours[0] = Info(m.get(x - 1, y - 1), -1, -1);
neighbours[1] = Info(m.get(x , y - 1), 0, -1);
neighbours[2] = Info(m.get(x + 1, y - 1), +1, -1);
neighbours[3] = Info(m.get(x - 1, y ), -1, 0);
neighbours[4] = Info(m.get(x + 1, y ), +1, 0);
neighbours[5] = Info(m.get(x - 1, y + 1), -1, +1);
neighbours[6] = Info(m.get(x , y + 1), 0, +1);
neighbours[7] = Info(m.get(x + 1, y + 1), +1, +1);
auto the_min = *std::min_element(neighbours, neighbours + 8);
x_n += the_min.x_offset;
y_n += the_min.y_offset;
}
int main(int, char **)
{
std::size_t xMax = 200;
std::size_t yMax = 150;
Matrix<float> cell(xMax + 2, yMax + 2);
Matrix<bool> start(xMax + 2, yMax + 2);
start.set(0.1 * xMax, 0.1 * yMax, true);
Matrix<bool> goal(xMax + 2, yMax + 2);
goal.set(0.9 * xMax, 0.9 * yMax, true);
Matrix<bool> blocked(xMax + 2, yMax + 2);
add_obstacles(blocked, 1, 1, 1);
PPM_image_save(blocked, "blocked.ppm");
PPM_image_save(start, "start.ppm");
PPM_image_save(goal, "goal.ppm");
for ( int y = 0; y <= yMax + 1; y++ )
{
for ( int x = 0; x <= xMax + 1; x++ )
{
if ( goal.get(x, y) )
{
cell.set(x, y, 0.);
}
else
{
cell.set(x, y, xMax * yMax);
}
}
}
Matrix<float> previous_cell = cell;
float values[5];
int cnt = 0;
do
{
std::ostringstream oss;
oss << "cell_" << cnt++ << ".ppm";
PPM_image_save(cell, oss.str());
previous_cell = cell;
for ( int y = 2; y <= yMax; y++ )
{
for ( int x = 2; x <= xMax; x++ )
{
if (!blocked.get(x, y))
{
values[0] = cell.get(x - 1, y ) + 1;
values[1] = cell.get(x - 1, y - 1) + 1;
values[2] = cell.get(x , y - 1) + 1;
values[3] = cell.get(x + 1, y - 1) + 1;
values[4] = cell.get(x , y );
cell.set(x, y, *std::min_element(values, values + 5));
}
}
}
for ( int y = yMax - 1; y >= 1; y-- )
{
for ( int x = xMax - 1; x >= 1; x-- )
{
if (!blocked.get(x, y))
{
values[0] = cell.get(x + 1, y ) + 1;
values[1] = cell.get(x + 1, y + 1) + 1;
values[2] = cell.get(x , y + 1) + 1;
values[3] = cell.get(x - 1, y + 1) + 1;
values[4] = cell.get(x , y );
cell.set(x, y, *std::min_element(values, values + 5));
}
}
}
}
while (distance(previous_cell, cell) > 0.);
PPM_image_save(cell, "cell.ppm");
Matrix<bool> path(xMax + 2, yMax + 2);
for ( int y_s = 1; y_s <= yMax; y_s++ )
{
for ( int x_s = 1; x_s <= xMax; x_s++ )
{
if ( start.get(x_s, y_s) )
{
int x = x_s;
int y = y_s;
while (!goal.get(x, y))
{
path.set(x, y, true);
int x_n, y_n;
next(cell, x, y, x_n, y_n);
x = x_n;
y = y_n;
}
}
}
}
PPM_image_save(path, "path.ppm");
return 0;
}
The algorithm uses the simple PPM image format explained for example in the Chapter 15 from the book Computer Graphics: Principles and Practice - Third Edition by Hughes et al. in order to save the images.
The algorithm starts from the image of the obstacles (blocked) and computes from it the distance transform (cell); then, starting from the distance transform, it computes the optimal path with a steepest descent method: it walks downhill in the transform distance potential field. So you can start from your own distance transform image.
Please note that it seems to me that the algorithm does not fulfill your additional constraint that:
the path should never reach a pixel which is less than 'x' pixels away
from an obstacle.
The following png image is the image of the obstacles, the program generated blocked.ppm image was exported as png via Gimp:
The following png image is the image of the start point, the program generated start.ppm image was exported as png via Gimp:
The following png image is the image of the end point, the program generated goal.ppm image was exported as png via Gimp:
The following png image is the image of the computed path, the program generated path.ppm image was exported as png via Gimp:
The following png image is the image of the distance transform, the program generated cell.ppm image was exported as png via Gimp:
I found the Jarvis' article after having a look at
CHIN, Yew Tuck, et al. Vision guided agv using distance transform. In: Proceedings of the 32nd ISR (International Symposium on Robotics). 2001. p. 21.
Update:
The Jarvis' algorithm is reviewed in the following paper where the authors state that:
Since the path is found by choosing locally only between neighbour
cells, the obtained path can be sub optimal
ELIZONDO-LEAL, Juan Carlos; PARRA-GONZÁLEZ, Ezra Federico; RAMÍREZ-TORRES, José Gabriel. The Exact Euclidean Distance Transform: A New Algorithm for Universal Path Planning. Int J Adv Robotic Sy, 2013, 10.266.
For a graph-based solution you can check for example chapter 15 of the book
DE BERG, Mark, et al. Computational geometry. Springer Berlin Heidelberg, 2008.
which has title "Visibility Graphs - Finding the Shortest Route" and it is freely available at the publisher site.
The chapter explains how to compute the Euclidean shortest path starting from the so-called visibility graph. The visibility graph is computed starting from the set of obstacles, each obstacle is described as a polygon.
The Euclidean shortest path is then found applying a shortest path algorithm such as Dijkstra's algorithm to the visibility graph.
In your distance transform image the obstacles are represented by pixels with zero value and so you can try to approximate them as polygons and after that apply the method described in the cited book.
In the distance transform map of pixels you choose your starting puxel and then select its neighbor with a lower value than your starting puxel - repeat the process until goal puxel is reached (a pixel with value zero).
Normally the goal pixel has value zero, the lowest number of any passable mode.
The problem of nor passing close to barriers is silver by generate the distance transform map so that barriers are enlarged. For example if you want a dustance if two pixels to any barrier - just add two pixels of barrier value. Normally the barriers wich could mot be passed is given a value of minus one.
Same value i used for the edges. An alternative approach ua to surround barriers with a very hifh starting value - the path is not guaranteed to get close, but the algorithm will try to avoid paths un the proximity of barriers.
I have a piece of processing code that I was given, which appears to be setting up a randomized Fourier series. Unfortunately, despite my efforts to improve my mathematical skills, I have no idea what it is doing and the articles I have found are not much help.
I'm trying to extend this code so that I can draw a line tangent to a point on the slope created by the code bellow. The closest I can find to answering this is in the mathematics forum. Unfortunately, I don't really understand what is being discussed or if it really is relevant to my situation.
Any assistance on how I would go about calculating a tangent line at a particular point on this curve would be much appreciated.
UPDATE As of 06/17/13
I've been trying to play around with this, but without much success. This is the best I can do, and I doubt that I'm applying the derivative correctly to find the tangent (or even if I have found the derivative at the point correctly). Also, I'm beginning to worry that I'm not drawing the line correctly even if I have everything else correct. If anyone can provide input on this I'd appreciate it.
final int w = 800;
final int h = 480;
double[] skyline;
PImage img;
int numOfDeriv = 800;
int derivModBy = 1; //Determines how many points will be checked
int time;
int timeDelay = 1000;
int iter;
double[] derivatives;
void setup() {
noStroke();
size(w, h);
fill(0,128,255);
rect(0,0,w,h);
int t[] = terrain(w,h);
fill(77,0,0);
for(int i=0; i < w; i++){
rect(i, h, 1, -1*t[i]);
}
time = millis();
timeDelay = 100;
iter =0;
img = get();
}
void draw() {
int dnum = 0; //Current position of derivatives
if(iter == numOfDeriv) iter = 0;
if (millis() > time + timeDelay){
image(img, 0, 0, width, height);
strokeWeight(4);
stroke(255,0,0);
point((float)iter*derivModBy, height-(float)skyline[iter*derivModBy]);
strokeWeight(1);
stroke(255,255,0);
print("At x = ");
print(iter);
print(", y = ");
print(skyline[iter]);
print(", derivative = ");
print((float)derivatives[iter]);
print('\n');
lineAngle(iter, (int)(height-skyline[iter]), (float)derivatives[iter], 100);
lineAngle(iter, (int)(height-skyline[iter]), (float)derivatives[iter], -100);
stroke(126);
time = millis();
iter += 1;
}
}
void lineAngle(int x, int y, float angle, float length)
{
line(x, y, x+cos(angle)*length, y-sin(angle)*length);
}
int[] terrain(int w, int h){
width = w;
height = h;
//min and max bracket the freq's of the sin/cos series
//The higher the max the hillier the environment
int min = 1, max = 6;
//allocating horizon for screen width
int[] horizon = new int[width];
skyline = new double[width];
derivatives = new double[numOfDeriv];
//ratio of amplitude of screen height to landscape variation
double r = (int) 2.0/5.0;
//number of terms to be used in sine/cosine series
int n = 4;
int[] f = new int[n*2];
//calculating omegas for sine series
for(int i = 0; i < n*2 ; i ++){
f[i] = (int) random(max - min + 1) + min;
}
//amp is the amplitude of the series
int amp = (int) (r*height);
int dnum = 0; //Current number of derivatives
for(int i = 0 ; i < width; i ++){
skyline[i] = 0;
double derivative = 0.0;
for(int j = 0; j < n; j++){
if(i % derivModBy == 0){
derivative += ( cos( (f[j]*PI*i/height) * f[j]*PI/height) -
sin(f[j+n]*PI*i/height) * f[j+n]*PI/height);
}
skyline[i] += ( sin( (f[j]*PI*i/height) ) + cos(f[j+n]*PI*i/height) );
}
skyline[i] *= amp/(n*2);
skyline[i] += (height/2);
skyline[i] = (int)skyline[i];
horizon[i] = (int)skyline[i];
derivative *= amp/(n*2);
if(i % derivModBy == 0){
derivatives[dnum++] = derivative;
derivative = 0;
}
}
return horizon;
}
void reset() {
time = millis();
}
Well it seems in this particular case that you don't need to understand much about the Fourier Series, just that it has the form:
A0 + A1*cos(x) + A2*cos(2*x) + A3*cos(3*x) +... + B1*sin(x) + B2*sin(x) +...
Normally you're given a function f(x) and you need to find the values of An and Bn such that the Fourier series converges to your function (as you add more terms) for some interval [a, b].
In this case however they want a random function that just looks like different lumps and pits (or hills and valleys as the context might suggest) so they choose random terms from the Fourier Series between min and max and set their coefficients to 1 (and conceptually 0 otherwise). They also satisfy themselves with a Fourier series of 4 sine terms and 4 cosine terms (which is certainly easier to manage than an infinite number of terms). This means that their Fourier Series ends up looking like different sine and cosine functions of different frequencies added together (and all have the same amplitude).
Finding the derivative of this is easy if you recall that:
sin(n*x)' = n * cos(x)
cos(n*x)' = -n * sin(x)
(f(x) + g(x))' = f'(x) + g'(x)
So the loop to calculate the the derivative would look like:
for(int j = 0; j < n; j++){
derivative += ( cos( (f[j]*PI*i/height) * f[j]*PI/height) - \
sin(f[j+n]*PI*i/height) * f[j+n]*PI/height);
}
At some point i (Note the derivative is being taken with respect to i since that is the variable that represents our x position here).
Hopefully with this you should be able to calculate the equation of the tangent line at a point i.
UPDATE
At the point where you do skyline[i] *= amp/(n*2); you must also adjust your derivative accordingly derivative *= amp/(n*2); however your derivative does not need adjusting when you do skyline[i] += height/2;
I received an answer to this problem via "quarks" on processing.org form. Essentially the problem is that I was taking the derivative of each term of the series instead of taking the derivative of the sum of the entire series. Also, I wasn't applying my result correctly anyway.
Here is the code that quarks provided that definitively solves this problem.
final int w = 800;
final int h = 480;
float[] skyline;
PImage img;
int numOfDeriv = 800;
int derivModBy = 1; //Determines how many points will be checked
int time;
int timeDelay = 1000;
int iter;
float[] tangents;
public void setup() {
noStroke();
size(w, h);
fill(0, 128, 255);
rect(0, 0, w, h);
terrain(w, h);
fill(77, 0, 0);
for (int i=0; i < w; i++) {
rect(i, h, 1, -1*(int)skyline[i]);
}
time = millis();
timeDelay = 100;
iter =0;
img = get();
}
public void draw() {
if (iter == numOfDeriv) iter = 0;
if (millis() > time + timeDelay) {
image(img, 0, 0, width, height);
strokeWeight(4);
stroke(255, 0, 0);
point((float)iter*derivModBy, height-(float)skyline[iter*derivModBy]);
strokeWeight(1);
stroke(255, 255, 0);
print("At x = ");
print(iter);
print(", y = ");
print(skyline[iter]);
print(", derivative = ");
print((float)tangents[iter]);
print('\n');
lineAngle(iter, (int)(height-skyline[iter]), (float)tangents[iter], 100);
lineAngle(iter, (int)(height-skyline[iter]), (float)tangents[iter], -100);
stroke(126);
time = millis();
iter += 1;
}
}
public void lineAngle(int x, int y, float angle, float length) {
line(x, y, x+cos(angle)*length, y-sin(angle)*length);
}
public void terrain(int w, int h) {
//min and max bracket the freq's of the sin/cos series
//The higher the max the hillier the environment
int min = 1, max = 6;
skyline = new float[w];
tangents = new float[w];
//ratio of amplitude of screen height to landscape variation
double r = (int) 2.0/5.0;
//number of terms to be used in sine/cosine series
int n = 4;
int[] f = new int[n*2];
//calculating omegas for sine series
for (int i = 0; i < n*2 ; i ++) {
f[i] = (int) random(max - min + 1) + min;
}
//amp is the amplitude of the series
int amp = (int) (r*h);
for (int i = 0 ; i < w; i ++) {
skyline[i] = 0;
for (int j = 0; j < n; j++) {
skyline[i] += ( sin( (f[j]*PI*i/h) ) + cos(f[j+n]*PI*i/h) );
}
skyline[i] *= amp/(n*2);
skyline[i] += (h/2);
}
for (int i = 1 ; i < w - 1; i ++) {
tangents[i] = atan2(skyline[i+1] - skyline[i-1], 2);
}
tangents[0] = atan2(skyline[1] - skyline[0], 1);
tangents[w-1] = atan2(skyline[w-2] - skyline[w-1], 1);
}
void reset() {
time = millis();
}