diff --git a/12thWeek/Canny/BlobDetection.pde b/12thWeek/Canny/BlobDetection.pde new file mode 100644 index 0000000..57b23ff --- /dev/null +++ b/12thWeek/Canny/BlobDetection.pde @@ -0,0 +1,110 @@ +import java.util.ArrayList; +import java.util.List; +import java.util.TreeSet; + +class BlobDetection { + PImage findConnectedComponents(PImage img, boolean onlyBiggest) { + PImage img0=img.copy(); + int [] labels= new int [img0.width*img0.height]; + List> labelsEquivalences= new ArrayList>(); + ArrayList count=new ArrayList(); + + int currentLabel=0; + + for (int line=0; line < img0.height; ++line) { + TreeSet adjColor=new TreeSet(); + for (int x=0; x < img0.width; x++) { + if (brightness(img0.pixels[line*img0.width+x])==255) { + adjColor.clear(); + + if (line>0) { + for (int i=x-1; i <= x+1; i++) {//check the three + if (0<=i && i(); + ts.add(++currentLabel); + labelsEquivalences.add(ts); + count.add(1); + labels[line*img0.width+x]=currentLabel; + } else { + if (adjColor.size()>1) { + for (Integer i : adjColor) { + labelsEquivalences.get(i-1).addAll(adjColor); + } + } + int first=adjColor.first(); + count.set(first-1, count.get(first-1)+1); + labels[line*img0.width+x]=first; + } + } + } + } + + //merge labelsEquivalences + for (int j=0; j ts=labelsEquivalences.get(j); + if (ts.size()>1) { + TreeSet acc=new TreeSet(); + for (Integer i : ts) { + TreeSet other=labelsEquivalences.get(i-1); + if(ts!=other){ acc.addAll(other);} + } + ts.addAll(acc); + for (Integer i : ts) { + labelsEquivalences.set(i-1,ts); + } + } + } + + //assess blob size + int[] blobSize=new int[labelsEquivalences.size()]; + for (int j=0; j ts=labelsEquivalences.get(j); + int sum=0; + for (Integer i : ts) { + sum+=count.get(i-1); + } + blobSize[j]=sum; + } + + //create ColorMap + int[] colorMap=new int[blobSize.length]; + if (onlyBiggest) { + int max =-1; + for(int i=0; i < blobSize.length; i++){ + max=max(max,blobSize[i]); + } + for(int i=0; i < blobSize.length; i++){ + colorMap[i]=blobSize[i]==max?color(255):color(0); + } + } else {//List> labelsEquivalences + for(TreeSet ts:labelsEquivalences){ + int rndColor=color(random(255),255,200); + for(Integer i:ts){ + colorMap[i-1]=rndColor; + } + } + } + + //colorize + for (int i=0; i < img0.width*img0.height; ++i) { + //int co=(labels[i])*256/7; + //labels[i]=color(co, 255, 200); + if(labels[i]!=0){ + img0.pixels[i]=colorMap[labels[i]-1]; + } + } + + return img0; + } +} diff --git a/8thWeek/Canny/Canny.pde b/12thWeek/Canny/Canny.pde similarity index 65% copy from 8thWeek/Canny/Canny.pde copy to 12thWeek/Canny/Canny.pde index 4a43051..60b3ab9 100644 --- a/8thWeek/Canny/Canny.pde +++ b/12thWeek/Canny/Canny.pde @@ -1,200 +1,236 @@ -import processing.video.*; - -PImage board1Thresholded, board1Blurred, board1Scharr; -int minHThreshold = 100, maxHThreshold =140; -int minSThreshold = 100, maxSThreshold = 255; -int minBThreshold = 45, maxBThreshold = 170; -//HScrollbar thresholdBar0; -//HScrollbar thresholdBar1; - -Capture cam; -PImage img; - -enum kernelType { - scale, blur, gaussian -}; - -void settings() { - //size(1600, 600); - size(640, 480); -} - -void setup() { - colorMode(HSB); - String[] cameras = Capture.list(); - if (cameras.length == 0) { - println("There are no cameras available for capture."); - exit(); - } else { - println("Available cameras:"); - for (int i = 0; i < cameras.length; i++) { - println(cameras[i]); - } - //If you're using gstreamer0.1 (Ubuntu 16.04 and earlier), - //select your predefined resolution from the list: - cam = new Capture(this, cameras[21]); - //If you're using gstreamer1.0 (Ubuntu 16.10 and later), - //select your resolution manually instead: - cam = new Capture(this, 640, 480, cameras[0]); - cam.start(); - } -} - -void draw() { - if (cam.available() == true) { - cam.read(); - } - img = cam.get(); - int I = 100; - img = thresholdHSB(img, minHThreshold, maxHThreshold, minSThreshold, maxSThreshold, minBThreshold, maxBThreshold ); - img = new BlobDetection().findConnectedComponents(img, true); - img = convolute(img, kernelType.gaussian); - img = scharr(img); - img = threshold(img, I, false); - - image(img, 0, 0); - - ArrayList lines=hough(img, 4); - List corners=new QuadGraph().findBestQuad(lines, 640, 480, 640* 480, 0, false); - - stroke(0); - fill(0x808080FF); - for (PVector pv : corners) { - ellipse(pv.x, pv.y, 30, 30); - } - noFill(); - stroke(255, 0, 0); - drawLines(lines, img); - - drawLines(hough(img, 4), img); -} - -//if inverted = false take values for 0 to threshol -PImage threshold(PImage img, int threshold, boolean inverted) { - PImage result = createImage(img.width, img.height, RGB); - for (int i = 0; i < img.width * img.height; i++) { - result.pixels[i]=brightness(img.pixels[i])>threshold^inverted?color(255):color(0); - } - return result; -} - -PImage thresholdHSB(PImage img, int minH, int maxH, int minS, int maxS, int minB, int maxB) { - PImage result = createImage(img.width, img.height, RGB); - for (int i = 0; i < img.width * img.height; i++) { - if (hue(img.pixels[i]) max )? value : max; - buffer[x+y*img.width] = value; - } - } - // ************************************* - - for (int y = 1; y < img.height - 1; y++) { // Skip top and bottom edges - for (int x = 1; x < img.width - 1; x++) { // Skip left and right - int val=(int) ((buffer[y * img.width + x] / max)*255); - result.pixels[y * img.width + x]=color(val); - } - } - return result; -} -PImage convolute(PImage img, kernelType kt) { - int N = 3; //kernel size - float[][] kernel; - float normFactor ; - if (kt == kernelType.scale ) { - float[][] kernel1 = {{ 0, 0, 0 }, - { 0, 2, 0 }, - { 0, 0, 0 }}; - kernel = kernel1; - normFactor = 1f; - } else if (kt == kernelType.blur) { - float[][] kernel2 = {{ 0, 1, 0 }, - { 1, 0, 1 }, - { 0, 1, 0 }}; - kernel = kernel2; - normFactor = 1f; - } else { - float[][] gaussianKernel = - {{ 9, 12, 9 }, - { 12, 15, 12 }, - { 9, 12, 9 }}; - kernel = gaussianKernel; - normFactor = 0f; - for (int i = 0; i< N; ++i) { - for (int j = 0; j lines=hough(img2, 4); + List corners=new QuadGraph().findBestQuad(lines, resizeX, resizeY, resizeX*resizeY, 0, false); + + stroke(0); + fill(0x808080FF); + for (PVector pv : corners) { + ellipse(pv.x, pv.y, 30, 30); + } + noFill(); + stroke(255, 0, 0); + drawLines(lines, dispImg); + + for(PVector c:corners){ + c.set(c.x,c.y,1); + } + PVector rotation=twoD3D.get3DRotations(corners); + rotation.set((rotation.x)/PI*180,(rotation.y)/PI*180,(rotation.z)/PI*180); + print(rotation); + + image(img2, resizeX, 0);//show image + image(img3, resizeX*2, 0);//show image + + IMAGE_NUMBER=(IMAGE_NUMBER)%4+1; +} + +//if inverted = false take values for 0 to threshol +PImage threshold(PImage img, int threshold, boolean inverted) { + PImage result = createImage(img.width, img.height, RGB); + for (int i = 0; i < img.width * img.height; i++) { + result.pixels[i]=brightness(img.pixels[i])>threshold^inverted?color(255):color(0); + } + return result; +} + +PImage thresholdHSB(PImage img, int minH, int maxH, int minS, int maxS, int minB, int maxB) { + PImage result = createImage(img.width, img.height, RGB); + for (int i = 0; i < img.width * img.height; i++) { + if (hue(img.pixels[i]) max )? value : max; + buffer[x+y*img.width] = value; + } + } + // ************************************* + + for (int y = 1; y < img.height - 1; y++) { // Skip top and bottom edges + for (int x = 1; x < img.width - 1; x++) { // Skip left and right + int val=(int) ((buffer[y * img.width + x] / max)*255); + result.pixels[y * img.width + x]=color(val); + } + } + return result; +} +PImage convolute(PImage img, kernelType kt) { + int N = 3; //kernel size + float[][] kernel; + float normFactor ; + if (kt == kernelType.scale ) { + float[][] kernel1 = {{ 0, 0, 0 }, + { 0, 2, 0 }, + { 0, 0, 0 }}; + kernel = kernel1; + normFactor = 1f; + } else if (kt == kernelType.blur) { + float[][] kernel2 = {{ 0, 1, 0 }, + { 1, 0, 1 }, + { 0, 1, 0 }}; + kernel = kernel2; + normFactor = 1f; + } else { + float[][] gaussianKernel = + {{ 9, 12, 9 }, + { 12, 15, 12 }, + { 9, 12, 9 }}; + kernel = gaussianKernel; + normFactor = 0f; + for (int i = 0; i< N; ++i) { + for (int j = 0; j 1) { + sliderPosition = sliderPosition + (newSliderPosition - sliderPosition); + } + } + + /** + * @brief Clamps the value into the interval + * + * @param val The value to be clamped + * @param minVal Smallest value possible + * @param maxVal Largest value possible + * + * @return val clamped into the interval [minVal, maxVal] + */ + float constrain(float val, float minVal, float maxVal) { + return min(max(val, minVal), maxVal); + } + + /** + * @brief Gets whether the mouse is hovering the scrollbar + * + * @return Whether the mouse is hovering the scrollbar + */ + boolean isMouseOver() { + if (mouseX > xPosition && mouseX < xPosition+barWidth && + mouseY > yPosition && mouseY < yPosition+barHeight) { + return true; + } else { + return false; + } + } + + /** + * @brief Draws the scrollbar in its current state + */ + void display() { + noStroke(); + fill(204); + rect(xPosition, yPosition, barWidth, barHeight); + if (mouseOver || locked) { + fill(0, 0, 0); + } else { + fill(102, 102, 102); + } + rect(sliderPosition, yPosition, barHeight, barHeight); + } + + /** + * @brief Gets the slider position + * + * @return The slider position in the interval [0,1] + * corresponding to [leftmost position, rightmost position] + */ + float getPos() { + return (sliderPosition - xPosition)/(barWidth - barHeight); + } +} diff --git a/12thWeek/Canny/HoughTransform.pde b/12thWeek/Canny/HoughTransform.pde new file mode 100644 index 0000000..2f292bd --- /dev/null +++ b/12thWeek/Canny/HoughTransform.pde @@ -0,0 +1,151 @@ +void drawLines(ArrayList lines, PImage edgeImg) { + + for (int idx = 0; idx < lines.size(); idx++) { + PVector line=lines.get(idx); + float r = line.x; + float phi = line.y; + // Cartesian equation of a line: y = ax + b + // in polar, y = (-cos(phi)/sin(phi))x + (r/sin(phi)) + // => y = 0 : x = r / cos(phi) + // => x = 0 : y = r / sin(phi) + // compute the intersection of this line with the 4 borders of + // the image + int x0 = 0; + int y0 = (int) (r / sin(phi)); + int x1 = (int) (r / cos(phi)); + int y1 = 0; + int x2 = edgeImg.width; + int y2 = (int) (-cos(phi) / sin(phi) * x2 + r / sin(phi)); + int y3 = edgeImg.width; + int x3 = (int) (-(y3 - r / sin(phi)) * (sin(phi) / cos(phi))); + // Finally, plot the lines + stroke(0, 255, 255); + if (y0 > 0) { + if (x1 > 0) + line(x0, y0, x1, y1); + else if (y2 > 0) + line(x0, y0, x2, y2); + else + line(x0, y0, x3, y3); + } else { + if (x1 > 0) { + if (y2 > 0) + line(x1, y1, x2, y2); + else + line(x1, y1, x3, y3); + } else + line(x2, y2, x3, y3); + } + } +} + +int minVotes=50; + +ArrayList hough(PImage edgeImg, int nLines) { + + float discretizationStepsPhi = 0.06f; + float discretizationStepsR = 2.5f; + + + ArrayList bestCandidates=new ArrayList(); + + // dimensions of the accumulator + int phiDim = (int) (Math.PI / discretizationStepsPhi +1); + //The max radius is the image diagonal, but it can be also negative + int rDim = (int) ((sqrt(edgeImg.width*edgeImg.width + + edgeImg.height*edgeImg.height) * 2) / discretizationStepsR +1); + // our accumulator + int[] accumulator = new int[phiDim * rDim]; + // Fill the accumulator: on edge points (ie, white pixels of the edge + // image), store all possible (r, phi) pairs describing lines going + // through the point. + + // pre-compute the sin and cos values + float[] tabSin = new float[phiDim]; + float[] tabCos = new float[phiDim]; + float ang = 0; + float inverseR = 1.f / discretizationStepsR; + for (int accPhi = 0; accPhi < phiDim; ang += discretizationStepsPhi, accPhi++) { + // we can also pre-multiply by (1/discretizationStepsR) since we need it in the Hough loop + tabSin[accPhi] = (float) (Math.sin(ang) * inverseR); + tabCos[accPhi] = (float) (Math.cos(ang) * inverseR); + } + + + for (int y = 0; y < edgeImg.height; y++) { + for (int x = 0; x < edgeImg.width; x++) { + // Are we on an edge? + if (brightness(edgeImg.pixels[y * edgeImg.width + x]) != 0) { + // ...determine here all the lines (r, phi) passing through + // pixel (x,y), convert (r,phi) to coordinates in the + // accumulator, and increment accordingly the accumulator. + // Be careful: r may be negative, so you may want to center onto + // the accumulator: r += rDim / 2 + for (int phi=0; phi minVotes && isMaxOverArea(accumulator, idx, REGION_SIZE, phiDim, rDim)) { + bestCandidates.add(idx); + } + } + + bestCandidates.sort(new HoughComparator(accumulator)); + ArrayList lines=new ArrayList(); + + for (int i=0; ithreshold) { + return false; + } + } + } + return true; +} + +class HoughComparator implements java.util.Comparator { + int[] accumulator; + public HoughComparator(int[] accumulator) { + this.accumulator = accumulator; + } + @Override + public int compare(Integer l1, Integer l2) { + if (accumulator[l1] > accumulator[l2] + || (accumulator[l1] == accumulator[l2] && l1 < l2)) return -1; + return 1; + } +} diff --git a/12thWeek/Canny/QuadGraph.pde b/12thWeek/Canny/QuadGraph.pde new file mode 100644 index 0000000..850fa29 --- /dev/null +++ b/12thWeek/Canny/QuadGraph.pde @@ -0,0 +1,384 @@ +import java.util.Collections; +import java.util.Comparator; +import java.util.List; +import java.util.ArrayList; +import java.util.Map; +class QuadGraph { + + boolean verbose=false; + + List cycles = new ArrayList(); + int[][] graph; + + List findBestQuad(List lines, int width, int height, int max_quad_area, int min_quad_area, boolean verbose) { + this.verbose=verbose; + build(lines, width, height); //<>// + findCycles(verbose); + ArrayList bestQuad=new ArrayList(); + float bestQuadArea=0; + for (int [] cy : cycles) { + ArrayList quad= new ArrayList(); + PVector l1 = lines.get(cy[0]); + PVector l2 = lines.get(cy[1]); + PVector l3 = lines.get(cy[2]); + PVector l4 = lines.get(cy[3]); + + quad.add(intersection(l1, l2)); + quad.add(intersection(l2, l3)); + quad.add(intersection(l3, l4)); + quad.add(intersection(l4, l1)); + quad=sortCorners(quad); + + PVector c1 = quad.get(0); + PVector c2 = quad.get(1); + PVector c3 = quad.get(2); + PVector c4 = quad.get(3); + + if (isConvex(c1, c2, c3, c4) && + nonFlatQuad(c1, c2, c3, c4)) { + float quadArea=validArea(c1, c2, c3, c4, max_quad_area, min_quad_area); + if (quadArea>0 && quadArea>bestQuadArea) { + bestQuadArea=quadArea; + bestQuad=quad; + } + } + } + + if (bestQuadArea>0) + return bestQuad; + else + return new ArrayList(); + } + + + void build(List lines, int width, int height) { + + int n = lines.size(); + + // The maximum possible number of edges is n * (n - 1)/2 + graph = new int[n * (n - 1)/2][2]; + + int idx =0; + + for (int i = 0; i < lines.size(); i++) { + for (int j = i + 1; j < lines.size(); j++) { + if (intersect(lines.get(i), lines.get(j), width, height)) { + + graph[idx][0]=i; + graph[idx][1]=j; + idx++; + } + } + } + } + + /** Returns true if polar lines 1 and 2 intersect + * inside an area of size (width, height) + */ + boolean intersect(PVector line1, PVector line2, int width, int height) { + + double sin_t1 = Math.sin(line1.y); + double sin_t2 = Math.sin(line2.y); + double cos_t1 = Math.cos(line1.y); + double cos_t2 = Math.cos(line2.y); + float r1 = line1.x; + float r2 = line2.x; + + double denom = cos_t2 * sin_t1 - cos_t1 * sin_t2; + + int x = (int) ((r2 * sin_t1 - r1 * sin_t2) / denom); + int y = (int) ((-r2 * cos_t1 + r1 * cos_t2) / denom); + + if (0 <= x && 0 <= y && width >= x && height >= y) + return true; + else + return false; + } + + PVector intersection(PVector line1, PVector line2) { + + double sin_t1 = Math.sin(line1.y); + double sin_t2 = Math.sin(line2.y); + double cos_t1 = Math.cos(line1.y); + double cos_t2 = Math.cos(line2.y); + float r1 = line1.x; + float r2 = line2.x; + + double denom = cos_t2 * sin_t1 - cos_t1 * sin_t2; + + int x = (int) ((r2 * sin_t1 - r1 * sin_t2) / denom); + int y = (int) ((-r2 * cos_t1 + r1 * cos_t2) / denom); + + return new PVector(x,y); + } + + void findCycles(boolean verbose) { + cycles.clear(); + for (int i = 0; i < graph.length; i++) { + for (int j = 0; j < graph[i].length; j++) { + findNewCycles(new int[] {graph[i][j]}); + } + } + if (verbose) { + for (int[] cy : cycles) { + String s = "" + cy[0]; + for (int i = 1; i < cy.length; i++) { + s += "," + cy[i]; + } + System.out.println(s); + } + } + } + + void findNewCycles(int[] path) + { + int n = path[0]; + int x; + int[] sub = new int[path.length + 1]; + + for (int i = 0; i < graph.length; i++) + for (int y = 0; y <= 1; y++) + if (graph[i][y] == n) + // edge refers to our current node + { + x = graph[i][(y + 1) % 2]; + if (!visited(x, path)) + // neighbor node not on path yet + { + sub[0] = x; + System.arraycopy(path, 0, sub, 1, path.length); + // explore extended path + findNewCycles(sub); + } else if ((path.length == 4) && (x == path[path.length - 1])) + // cycle found + { + int[] p = normalize(path); + int[] inv = invert(p); + if (isNew(p) && isNew(inv)) + { + cycles.add(p); + } + } + } + } + + // check of both arrays have same lengths and contents + Boolean equals(int[] a, int[] b) + { + Boolean ret = (a[0] == b[0]) && (a.length == b.length); + + for (int i = 1; ret && (i < a.length); i++) + { + if (a[i] != b[i]) + { + ret = false; + } + } + + return ret; + } + + // create a path array with reversed order + int[] invert(int[] path) + { + int[] p = new int[path.length]; + + for (int i = 0; i < path.length; i++) + { + p[i] = path[path.length - 1 - i]; + } + + return normalize(p); + } + + // rotate cycle path such that it begins with the smallest node + int[] normalize(int[] path) + { + int[] p = new int[path.length]; + int x = smallest(path); + int n; + + System.arraycopy(path, 0, p, 0, path.length); + + while (p[0] != x) + { + n = p[0]; + System.arraycopy(p, 1, p, 0, p.length - 1); + p[p.length - 1] = n; + } + + return p; + } + + // compare path against known cycles + // return true, iff path is not a known cycle + Boolean isNew(int[] path) + { + Boolean ret = true; + + for (int[] p : cycles) + { + if (equals(p, path)) + { + ret = false; + break; + } + } + + return ret; + } + + // return the int of the array which is the smallest + int smallest(int[] path) + { + int min = path[0]; + + for (int p : path) + { + if (p < min) + { + min = p; + } + } + + return min; + } + + // check if vertex n is contained in path + Boolean visited(int n, int[] path) + { + Boolean ret = false; + + for (int p : path) + { + if (p == n) + { + ret = true; + break; + } + } + + return ret; + } + + + + /** Check if a quad is convex or not. + * + * Algo: take two adjacent edges and compute their cross-product. + * The sign of the z-component of all the cross-products is the + * same for a convex polygon. + * + * See http://debian.fmi.uni-sofia.bg/~sergei/cgsr/docs/clockwise.htm + * for justification. + * + * @param c1 + */ + boolean isConvex(PVector c1, PVector c2, PVector c3, PVector c4) { + + PVector v21= PVector.sub(c1, c2); + PVector v32= PVector.sub(c2, c3); + PVector v43= PVector.sub(c3, c4); + PVector v14= PVector.sub(c4, c1); + + float i1=v21.cross(v32).z; + float i2=v32.cross(v43).z; + float i3=v43.cross(v14).z; + float i4=v14.cross(v21).z; + + if ( (i1>0 && i2>0 && i3>0 && i4>0) + || (i1<0 && i2<0 && i3<0 && i4<0)) + return true; + else if(verbose) + System.out.println("Eliminating non-convex quad"); + return false; + } + + /** Compute the area of a quad, and check it lays within a specific range + */ + float validArea(PVector c1, PVector c2, PVector c3, PVector c4, float max_area, float min_area) { + + float i1=c1.cross(c2).z; + float i2=c2.cross(c3).z; + float i3=c3.cross(c4).z; + float i4=c4.cross(c1).z; + + float area = Math.abs(0.5f * (i1 + i2 + i3 + i4)); + + if (area < max_area && area > min_area){ + return area; + } + return 0; + + } + + /** Compute the (cosine) of the four angles of the quad, and check they are all large enough + * (the quad representing our board should be close to a rectangle) + */ + boolean nonFlatQuad(PVector c1, PVector c2, PVector c3, PVector c4) { + + // cos(70deg) ~= 0.3 + float min_cos = 0.5f; + + PVector v21= PVector.sub(c1, c2); + PVector v32= PVector.sub(c2, c3); + PVector v43= PVector.sub(c3, c4); + PVector v14= PVector.sub(c4, c1); + + float cos1=Math.abs(v21.dot(v32) / (v21.mag() * v32.mag())); + float cos2=Math.abs(v32.dot(v43) / (v32.mag() * v43.mag())); + float cos3=Math.abs(v43.dot(v14) / (v43.mag() * v14.mag())); + float cos4=Math.abs(v14.dot(v21) / (v14.mag() * v21.mag())); + + if (cos1 < min_cos && cos2 < min_cos && cos3 < min_cos && cos4 < min_cos) + return true; + else { + if(verbose) + System.out.println("Flat quad"); + return false; + } + } + + + ArrayList sortCorners(ArrayList quad) { + + // 1 - Sort corners so that they are ordered clockwise + PVector a = quad.get(0); + PVector b = quad.get(2); + + PVector center = new PVector((a.x+b.x)/2, (a.y+b.y)/2); + + Collections.sort(quad, new CWComparator(center)); + + + + // 2 - Sort by upper left most corner + PVector origin = new PVector(0, 0); + float distToOrigin = 1000; + + for (PVector p : quad) { + if (p.dist(origin) < distToOrigin) distToOrigin = p.dist(origin); + } + + while (quad.get(0).dist(origin) != distToOrigin) + Collections.rotate(quad, 1); + + return quad; + } +} + +class CWComparator implements Comparator { + + PVector center; + + public CWComparator(PVector center) { + this.center = center; + } + + @Override + public int compare(PVector b, PVector d) { + if (Math.atan2(b.y-center.y, b.x-center.x)0) { + cutOffFreq=sampleRate/2; + alpha= (1/sampleRate)/(1/sampleRate + 1/cutOffFreq); + } + } + + PVector get3DRotations(List points2D) { + + // 1- Solve the extrinsic matrix from the projected 2D points + double[][] E = solveExtrinsicMatrix(points2D); + + + // 2 - Re-build a proper 3x3 rotation matrix from the camera's + // extrinsic matrix E + PVector firstColumn=new PVector((float)E[0][0], (float)E[1][0], (float)E[2][0]); + PVector secondColumn=new PVector((float)E[0][1], (float)E[1][1], (float)E[2][1]); + firstColumn.normalize(); + secondColumn.normalize(); + PVector thirdColumn=firstColumn.cross(secondColumn); + float [][] rotationMatrix={{firstColumn.x, secondColumn.x, thirdColumn.x}, + {firstColumn.y, secondColumn.y, thirdColumn.y}, + {firstColumn.z, secondColumn.z, thirdColumn.z}}; + + if (sampleRate>0) + filter(rotationMatrix, false); + + // 3 - Computes and returns Euler angles (rx, ry, rz) from this matrix + return rotationFromMatrix(rotationMatrix); + } + + + double[][] solveExtrinsicMatrix(List points2D) { + + // p ~= K · [R|t] · P + // with P the (3D) corners of the physical board, p the (2D) + // projected points onto the webcam image, K the intrinsic + // matrix and R and t the rotation and translation we want to + // compute. + // + // => We want to solve: (K^(-1) · p) X ([R|t] · P) = 0 + + float[][] projectedCorners = new float[4][3]; + + if(points2D.size() >= 4) + for (int i=0; i<4; i++) { + // TODO: + // store in projectedCorners the result of (K^(-1) · p), for each + // corner p found in the webcam image. + // You can use PVector dot function for computing dot product between K^(-1) lines and p. + //Do not forget to normalize the result + PVector point =points2D.get(i); + projectedCorners[i][0]=point.dot(invK_r1)/point.dot(invK_r3); + projectedCorners[i][1]=point.dot(invK_r2)/point.dot(invK_r3); + projectedCorners[i][2]=1; + } + + // 'A' contains the cross-product (K^(-1) · p) X P + float[][] A= new float[12][9]; + + for (int i=0; i<4; i++) { + A[i*3][0]=0; + A[i*3][1]=0; + A[i*3][2]=0; + + // note that we take physicalCorners[0,1,*3*]: we drop the Z + // coordinate and use the 2D homogenous coordinates of the physical + // corners + A[i*3][3]=-projectedCorners[i][2] * physicalCorners[i][0]; + A[i*3][4]=-projectedCorners[i][2] * physicalCorners[i][1]; + A[i*3][5]=-projectedCorners[i][2] * physicalCorners[i][3]; + + A[i*3][6]= projectedCorners[i][1] * physicalCorners[i][0]; + A[i*3][7]= projectedCorners[i][1] * physicalCorners[i][1]; + A[i*3][8]= projectedCorners[i][1] * physicalCorners[i][3]; + + A[i*3+1][0]= projectedCorners[i][2] * physicalCorners[i][0]; + A[i*3+1][1]= projectedCorners[i][2] * physicalCorners[i][1]; + A[i*3+1][2]= projectedCorners[i][2] * physicalCorners[i][3]; + + A[i*3+1][3]=0; + A[i*3+1][4]=0; + A[i*3+1][5]=0; + + A[i*3+1][6]=-projectedCorners[i][0] * physicalCorners[i][0]; + A[i*3+1][7]=-projectedCorners[i][0] * physicalCorners[i][1]; + A[i*3+1][8]=-projectedCorners[i][0] * physicalCorners[i][3]; + + A[i*3+2][0]=-projectedCorners[i][1] * physicalCorners[i][0]; + A[i*3+2][1]=-projectedCorners[i][1] * physicalCorners[i][1]; + A[i*3+2][2]=-projectedCorners[i][1] * physicalCorners[i][3]; + + A[i*3+2][3]= projectedCorners[i][0] * physicalCorners[i][0]; + A[i*3+2][4]= projectedCorners[i][0] * physicalCorners[i][1]; + A[i*3+2][5]= projectedCorners[i][0] * physicalCorners[i][3]; + + A[i*3+2][6]=0; + A[i*3+2][7]=0; + A[i*3+2][8]=0; + } + + for (int i=0; i<12; i++) + for (int j=0; j<9; j++) + opencv_A.put(i, j, A[i][j]); + + Core.SVDecomp(opencv_A, w, u, vt); + + for (int i=0; i<9; i++) + for (int j=0; j<9; j++) + V[j][i]=vt.get(i, j)[0]; + + double[][] E = new double[3][3]; + + //E is the last column of V + for (int i=0; i<9; i++) { + E[i/3][i%3] = V[i][V.length-1] / V[8][V.length-1]; + } + + return E; + } + + PVector rotationFromMatrix(float[][] mat) { + + // Assuming rotation order is around x,y,z + PVector rot = new PVector(); + + if (mat[1][0] > 0.998) { // singularity at north pole + rot.z = 0; + float delta = (float) Math.atan2(mat[0][1], mat[0][2]); + rot.y = -(float) Math.PI/2; + rot.x = -rot.z + delta; + return rot; + } + + if (mat[1][0] < -0.998) { // singularity at south pole + rot.z = 0; + float delta = (float) Math.atan2(mat[0][1], mat[0][2]); + rot.y = (float) Math.PI/2; + rot.x = rot.z + delta; + return rot; + } + + rot.y =-(float)Math.asin(mat[2][0]); + rot.x = (float)Math.atan2(mat[2][1]/Math.cos(rot.y), mat[2][2]/Math.cos(rot.y)); + rot.z = (float)Math.atan2(mat[1][0]/Math.cos(rot.y), mat[0][0]/Math.cos(rot.y)); + + return rot; + } + + int filter(float m[][], boolean reset) { + + float[] q= new float[4]; + float alpha, oneminusalpha, omega, cosomega, sinomega, s0, s1; + + mat2Quat(m, q); + if (nomalizeQuaternion(q)<0) return -1; + + if (reset) { + this.q[0] = q[0]; + this.q[1] = q[1]; + this.q[2] = q[2]; + this.q[3] = q[3]; + } else { + alpha = this.alpha; + + oneminusalpha = 1.0 - alpha; + + // SLERP for orientation. + cosomega = q[0]*this.q[0] + q[1]*this.q[1] + q[2]*this.q[2] + q[3]*this.q[3]; // cos of angle between vectors. + if (cosomega < 0.0) { + cosomega = -cosomega; + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } + if (cosomega > 0.9995) { + s0 = oneminusalpha; + s1 = alpha; + } else { + omega = acos(cosomega); + sinomega = sin(omega); + s0 = sin(oneminusalpha * omega) / sinomega; + s1 = sin(alpha * omega) / sinomega; + } + this.q[0] = q[0]*s1 + this.q[0]*s0; + this.q[1] = q[1]*s1 + this.q[1]*s0; + this.q[2] = q[2]*s1 + this.q[2]*s0; + this.q[3] = q[3]*s1 + this.q[3]*s0; + nomalizeQuaternion(this.q); + } + + if (quat2Mat(this.q, m) < 0) return (-2); + + return (0); + } + + + int nomalizeQuaternion(float[] q) {// Normalise quaternion. + float mag2 = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; + if (mag2==0) return (-1); + + float mag = sqrt(mag2); + + q[0] /= mag; + q[1] /= mag; + q[2] /= mag; + q[3] /= mag; + + return (0); + } + + int mat2Quat(float m[][], float q[]) { + float t, s; + t = m[0][0] + m[1][1] + m[2][2] + 1.0; + if (t > 0.0001) { + s = sqrt(t) * 2.0; + q[0] = (m[1][2] - m[2][1]) / s; + q[1] = (m[2][0] - m[0][2]) / s; + q[2] = (m[0][1] - m[1][0]) / s; + q[3] = 0.25 * s; + } else { + if (m[0][0] > m[1][1] && m[0][0] > m[2][2]) { // Column 0: + s = sqrt(1.0 + m[0][0] - m[1][1] - m[2][2]) * 2.0; + q[0] = 0.25 * s; + q[1] = (m[0][1] + m[1][0] ) / s; + q[2] = (m[2][0] + m[0][2] ) / s; + q[3] = (m[1][2] - m[2][1] ) / s; + } else if (m[1][1] > m[2][2]) { // Column 1: + s = sqrt(1.0 + m[1][1] - m[0][0] - m[2][2]) * 2.0; + q[0] = (m[0][1] + m[1][0] ) / s; + q[1] = 0.25 * s; + q[2] = (m[1][2] + m[2][1] ) / s; + q[3] = (m[2][0] - m[0][2] ) / s; + } else { // Column 2: + s = sqrt(1.0 + m[2][2] - m[0][0] - m[1][1]) * 2.0; + q[0] = (m[2][0] + m[0][2] ) / s; + q[1] = (m[1][2] + m[2][1] ) / s; + q[2] = 0.25 * s; + q[3] = (m[0][1] - m[1][0] ) / s; + } + } + return 0; + } + + int quat2Mat( float q[], float m[][] ) + { + float x2, y2, z2; + float xx, xy, xz; + float yy, yz, zz; + float wx, wy, wz; + + x2 = q[0] * 2.0; + y2 = q[1] * 2.0; + z2 = q[2] * 2.0; + + xx = q[0] * x2; + xy = q[0] * y2; + xz = q[0] * z2; + yy = q[1] * y2; + yz = q[1] * z2; + zz = q[2] * z2; + wx = q[3] * x2; + wy = q[3] * y2; + wz = q[3] * z2; + + m[0][0] = 1.0 - (yy + zz); + m[1][1] = 1.0 - (xx + zz); + m[2][2] = 1.0 - (xx + yy); + + m[1][0] = xy - wz; + m[0][1] = xy + wz; + m[2][0] = xz + wy; + m[0][2] = xz - wy; + m[2][1] = yz - wx; + m[1][2] = yz + wx; + + return 0; + } +} diff --git a/12thWeek/Canny/src/board1.jpg b/12thWeek/Canny/src/board1.jpg new file mode 100644 index 0000000..8766422 Binary files /dev/null and b/12thWeek/Canny/src/board1.jpg differ diff --git a/12thWeek/Canny/src/board1Blurred.bmp b/12thWeek/Canny/src/board1Blurred.bmp new file mode 100644 index 0000000..7eb8be8 Binary files /dev/null and b/12thWeek/Canny/src/board1Blurred.bmp differ diff --git a/12thWeek/Canny/src/board1Scharr.bmp b/12thWeek/Canny/src/board1Scharr.bmp new file mode 100644 index 0000000..688672d Binary files /dev/null and b/12thWeek/Canny/src/board1Scharr.bmp differ diff --git a/12thWeek/Canny/src/board1Thresholded.bmp b/12thWeek/Canny/src/board1Thresholded.bmp new file mode 100644 index 0000000..318f1de Binary files /dev/null and b/12thWeek/Canny/src/board1Thresholded.bmp differ diff --git a/12thWeek/Canny/src/board2.jpg b/12thWeek/Canny/src/board2.jpg new file mode 100644 index 0000000..dd326b2 Binary files /dev/null and b/12thWeek/Canny/src/board2.jpg differ diff --git a/12thWeek/Canny/src/board3.jpg b/12thWeek/Canny/src/board3.jpg new file mode 100644 index 0000000..8c1bbd3 Binary files /dev/null and b/12thWeek/Canny/src/board3.jpg differ diff --git a/12thWeek/Canny/src/board4.jpg b/12thWeek/Canny/src/board4.jpg new file mode 100644 index 0000000..34fa5cc Binary files /dev/null and b/12thWeek/Canny/src/board4.jpg differ diff --git a/12thWeek/Canny/src/nao.jpg b/12thWeek/Canny/src/nao.jpg new file mode 100644 index 0000000..1bce331 Binary files /dev/null and b/12thWeek/Canny/src/nao.jpg differ diff --git a/8thWeek/Canny/Canny.pde b/8thWeek/Canny/Canny.pde index 4a43051..91cbff5 100644 --- a/8thWeek/Canny/Canny.pde +++ b/8thWeek/Canny/Canny.pde @@ -1,200 +1,200 @@ import processing.video.*; PImage board1Thresholded, board1Blurred, board1Scharr; int minHThreshold = 100, maxHThreshold =140; int minSThreshold = 100, maxSThreshold = 255; -int minBThreshold = 45, maxBThreshold = 170; +int minBThreshold = 45, maxBThreshold = 200; //HScrollbar thresholdBar0; //HScrollbar thresholdBar1; Capture cam; PImage img; enum kernelType { scale, blur, gaussian }; void settings() { //size(1600, 600); size(640, 480); } void setup() { colorMode(HSB); String[] cameras = Capture.list(); if (cameras.length == 0) { println("There are no cameras available for capture."); exit(); } else { println("Available cameras:"); for (int i = 0; i < cameras.length; i++) { println(cameras[i]); } //If you're using gstreamer0.1 (Ubuntu 16.04 and earlier), //select your predefined resolution from the list: cam = new Capture(this, cameras[21]); //If you're using gstreamer1.0 (Ubuntu 16.10 and later), //select your resolution manually instead: cam = new Capture(this, 640, 480, cameras[0]); cam.start(); } } void draw() { if (cam.available() == true) { cam.read(); } - img = cam.get(); + PImage originalImage=cam.get(); + img = originalImage.copy(); int I = 100; img = thresholdHSB(img, minHThreshold, maxHThreshold, minSThreshold, maxSThreshold, minBThreshold, maxBThreshold ); img = new BlobDetection().findConnectedComponents(img, true); img = convolute(img, kernelType.gaussian); img = scharr(img); img = threshold(img, I, false); - image(img, 0, 0); + image(originalImage, 0, 0); ArrayList lines=hough(img, 4); List corners=new QuadGraph().findBestQuad(lines, 640, 480, 640* 480, 0, false); stroke(0); fill(0x808080FF); for (PVector pv : corners) { ellipse(pv.x, pv.y, 30, 30); } noFill(); stroke(255, 0, 0); drawLines(lines, img); - drawLines(hough(img, 4), img); } //if inverted = false take values for 0 to threshol PImage threshold(PImage img, int threshold, boolean inverted) { PImage result = createImage(img.width, img.height, RGB); for (int i = 0; i < img.width * img.height; i++) { result.pixels[i]=brightness(img.pixels[i])>threshold^inverted?color(255):color(0); } return result; } PImage thresholdHSB(PImage img, int minH, int maxH, int minS, int maxS, int minB, int maxB) { PImage result = createImage(img.width, img.height, RGB); for (int i = 0; i < img.width * img.height; i++) { if (hue(img.pixels[i]) max )? value : max; buffer[x+y*img.width] = value; } } // ************************************* for (int y = 1; y < img.height - 1; y++) { // Skip top and bottom edges for (int x = 1; x < img.width - 1; x++) { // Skip left and right int val=(int) ((buffer[y * img.width + x] / max)*255); result.pixels[y * img.width + x]=color(val); } } return result; } PImage convolute(PImage img, kernelType kt) { int N = 3; //kernel size float[][] kernel; float normFactor ; if (kt == kernelType.scale ) { float[][] kernel1 = {{ 0, 0, 0 }, { 0, 2, 0 }, { 0, 0, 0 }}; kernel = kernel1; normFactor = 1f; } else if (kt == kernelType.blur) { float[][] kernel2 = {{ 0, 1, 0 }, { 1, 0, 1 }, { 0, 1, 0 }}; kernel = kernel2; normFactor = 1f; } else { float[][] gaussianKernel = {{ 9, 12, 9 }, { 12, 15, 12 }, { 9, 12, 9 }}; kernel = gaussianKernel; normFactor = 0f; for (int i = 0; i< N; ++i) { for (int j = 0; j> labelsEquivalences= new ArrayList>(); + ArrayList count=new ArrayList(); + + int currentLabel=0; + + for (int line=0; line < img0.height; ++line) { + TreeSet adjColor=new TreeSet(); + for (int x=0; x < img0.width; x++) { + if (brightness(img0.pixels[line*img0.width+x])==255) { + adjColor.clear(); + + if (line>0) { + for (int i=x-1; i <= x+1; i++) {//check the three + if (0<=i && i(); + ts.add(++currentLabel); + labelsEquivalences.add(ts); + count.add(1); + labels[line*img0.width+x]=currentLabel; + } else { + if (adjColor.size()>1) { + for (Integer i : adjColor) { + labelsEquivalences.get(i-1).addAll(adjColor); + } + } + int first=adjColor.first(); + count.set(first-1, count.get(first-1)+1); + labels[line*img0.width+x]=first; + } + } + } + } + + //merge labelsEquivalences + for (int j=0; j ts=labelsEquivalences.get(j); + if (ts.size()>1) { + TreeSet acc=new TreeSet(); + for (Integer i : ts) { + TreeSet other=labelsEquivalences.get(i-1); + if(ts!=other){ acc.addAll(other);} + } + ts.addAll(acc); + for (Integer i : ts) { + labelsEquivalences.set(i-1,ts); + } + } + } + + //assess blob size + int[] blobSize=new int[labelsEquivalences.size()]; + for (int j=0; j ts=labelsEquivalences.get(j); + int sum=0; + for (Integer i : ts) { + sum+=count.get(i-1); + } + blobSize[j]=sum; + } + + //create ColorMap + int[] colorMap=new int[blobSize.length]; + if (onlyBiggest) { + int max =-1; + for(int i=0; i < blobSize.length; i++){ + max=max(max,blobSize[i]); + } + for(int i=0; i < blobSize.length; i++){ + colorMap[i]=blobSize[i]==max?color(255):color(0); + } + } else {//List> labelsEquivalences + for(TreeSet ts:labelsEquivalences){ + int rndColor=color(random(255),255,200); + for(Integer i:ts){ + colorMap[i-1]=rndColor; + } + } + } + + //colorize + for (int i=0; i < img0.width*img0.height; ++i) { + //int co=(labels[i])*256/7; + //labels[i]=color(co, 255, 200); + if(labels[i]!=0){ + img0.pixels[i]=colorMap[labels[i]-1]; + } + } + + return img0; + } +} diff --git a/Project/Game/Canny.pde b/Project/Game/Canny.pde new file mode 100644 index 0000000..39fdda1 --- /dev/null +++ b/Project/Game/Canny.pde @@ -0,0 +1,202 @@ +import gab.opencv.*; +import processing.video.*; + + +static enum kernelType { + scale, blur, gaussian +}; + +class Canny extends PApplet { + + static final int imageWidth=800; + static final int imageHeight=600; + + OpenCV opencv; + TwoDThreeD twoD3D; + + PImage img0; + + static final int minHThreshold = 100, maxHThreshold =140; + static final int minSThreshold = 80, maxSThreshold = 255; + static final int minBThreshold = 10, maxBThreshold = 170; + + Capture cam; + + Canny() { + String[] cameras = Capture.list(); + if (cameras.length == 0) { + println("There are no cameras available for capture."); + exit(); + } else { + println("Available cameras:"); + for (int i = 0; i < cameras.length; i++) { + println(cameras[i]); + } + //If you're using gstreamer0.1 (Ubuntu 16.04 and earlier), + //select your predefined resolution from the list: + //cam = new Capture(this, cameras[21]); + //If you're using gstreamer1.0 (Ubuntu 16.10 and later), + //select your resolution manually instead: + cam = new Capture(this, 640, 480, cameras[0]); + println(cam==null); + cam.start(); + } + opencv = new OpenCV(this, 100, 100); + twoD3D=new TwoDThreeD(imageWidth, imageHeight, 0); + } + + PVector update() { + if (cam.available() == true) { + cam.read(); + } + img0 = cam.get(); + int I = 100; + img0 = thresholdHSB(img0, minHThreshold, maxHThreshold, minSThreshold, maxSThreshold, minBThreshold, maxBThreshold ); + img0 = new BlobDetection().findConnectedComponents(img0, true); + img0 = convolute(img0, kernelType.gaussian); + img0 = scharr(img0); + img0 = threshold(img0, I, false); + + ArrayList lines=hough(img0, 4); + List corners=new QuadGraph().findBestQuad(lines, imageWidth, imageHeight, imageWidth*imageHeight, 0, false); + + + for (PVector c : corners) { + c.set(c.x, c.y, 1); + } + PVector rotation=twoD3D.get3DRotations(corners); + rotation.set((rotation.x)/PI*180, (rotation.y)/PI*180, (rotation.z)/PI*180); + + return rotation; + } + + //if inverted = false take values for 0 to threshol + PImage threshold(PImage img, int threshold, boolean inverted) { + PImage result = createImage(img.width, img.height, RGB); + for (int i = 0; i < img.width * img.height; i++) { + result.pixels[i]=brightness(img.pixels[i])>threshold^inverted?color(255):color(0); + } + return result; + } + + PImage thresholdHSB(PImage img, int minH, int maxH, int minS, int maxS, int minB, int maxB) { + PImage result = createImage(img.width, img.height, RGB); + for (int i = 0; i < img.width * img.height; i++) { + if (hue(img.pixels[i]) max )? value : max; + buffer[x+y*img.width] = value; + } + } + // ************************************* + + for (int y = 1; y < img.height - 1; y++) { // Skip top and bottom edges + for (int x = 1; x < img.width - 1; x++) { // Skip left and right + int val=(int) ((buffer[y * img.width + x] / max)*255); + result.pixels[y * img.width + x]=color(val); + } + } + return result; + } + PImage convolute(PImage img, kernelType kt) { + int N = 3; //kernel size + float[][] kernel; + float normFactor ; + if (kt == kernelType.scale ) { + float[][] kernel1 = {{ 0, 0, 0 }, + { 0, 2, 0 }, + { 0, 0, 0 }}; + kernel = kernel1; + normFactor = 1f; + } else if (kt == kernelType.blur) { + float[][] kernel2 = {{ 0, 1, 0 }, + { 1, 0, 1 }, + { 0, 1, 0 }}; + kernel = kernel2; + normFactor = 1f; + } else { + float[][] gaussianKernel = + {{ 9, 12, 9 }, + { 12, 15, 12 }, + { 9, 12, 9 }}; + kernel = gaussianKernel; + normFactor = 0f; + for (int i = 0; i< N; ++i) { + for (int j = 0; j scores; float score; float scoreMalus = -1; int scoreTimer = 0; int prVeloFreq = 20; //The frequency at which the velocity is printed float lastScore; float veloMag; float scoreFactor = 0.5; //lastScore = scoreFactor * lastVelocity; float VilainScoreMultiplier=10;//bonus score given when killing the vilain boolean win = false; -int minHistowidth = 1,maxHistowidth = 20, histowidth; +int minHistowidth = 1, maxHistowidth = 20, histowidth; float maxScore = 10, minScore=-10; float drawingFactor; // = histoHeight /(maxScore -minScore); int barChartWidth, barChartHeight, bcCubeHeight = 5; boolean mousePermittedToMove=true; //Plate size float plate_w=1000; float plate_t=plate_w/20; float x_coord, y_coord; float mX, mY; //mouse X position float xOnPlane, yOnPlane; //coordinate on 2D on the box float rz, rx; //rotate x and rotate z float cameraHeight; float speed=10; float radius=plate_t*4/5; DecimalFormat f = new DecimalFormat("#0.00"); DecimalFormat f2 = new DecimalFormat("#0"); boolean shiftMod=false; ParticleWin partW; ParticleSystem partSys=null; long previousFrame=0; float timeIntervalParticle=0.5;//seconds int margin = 15; //marge between surface int botPartHeight = 300; int topViewWidth = botPartHeight-0; PGraphics gameSurface; PGraphics statSurface; PGraphics topView; PGraphics scoreboard; PGraphics barChart; void settings() { size(displayWidth, displayHeight-60, P3D); fullScreen(); x_coord=width/2; y_coord=height/2; } void setup() { mover = new Mover(); partW=new ParticleWin(mover); cyl=new Cylinder(); scores = new ArrayList(); hs = new HScrollbar(750, height -50, 500, 20); img = loadImage("earth.jpg"); globe = createShape(SPHERE, radius); globe.setStroke(false); globe.setTexture(img); img = loadImage("robotnik.png"); robot = loadShape("robotnik.obj"); robot.setTexture(img); gameSurface=createGraphics(width, height-botPartHeight, P3D); topView=createGraphics(topViewWidth, topViewWidth, P2D); statSurface=createGraphics(3*botPartHeight, botPartHeight/3, P2D); - scoreboard = createGraphics(botPartHeight-2*margin,botPartHeight-2*margin, P2D); - + scoreboard = createGraphics(botPartHeight-2*margin, botPartHeight-2*margin, P2D); + barChartWidth = width - (botPartHeight+margin) *2; barChartHeight = botPartHeight-2*margin-50; barChart = createGraphics(barChartWidth, barChartHeight, P2D); + + + canny = new Canny(); + String []args = {"Image processing window"}; + PApplet.runSketch(args, canny); } void draw() { - background(181,65,2); - + background(181, 65, 2); + drawGame(); image(gameSurface, 0, 0); - + drawTopView(); - image(topView, margin , height-botPartHeight); + image(topView, margin, height-botPartHeight); drawStat(); image(statSurface, 0, 0); - + drawScoreboard(); image(scoreboard, botPartHeight+2*margin, height-botPartHeight+margin); - + drawBarChart(); image(barChart, 2*botPartHeight+margin, height-botPartHeight+margin); - + + println(canny.update()); + hs.update(); hs.display(); } -void drawBarChart(){ +void drawBarChart() { barChart.beginDraw(); - barChart.background(242,86,2); + barChart.background(242, 86, 2); barChart.fill(40, 10, 200); histowidth = (int)(hs.getPos()*(maxHistowidth-minHistowidth)+minHistowidth); int maxRect = barChartWidth / histowidth; int start = (scores.size() < maxRect)? 0 : scores.size()-maxRect; - for(int i = start ; i < scores.size(); ++i){ - float zeroInHisto = drawingFactor * (maxScore); - float posInHisto = drawingFactor * (-scores.get(i)+maxScore) ; - int nbOfCube ; - if(zeroInHisto>posInHisto){ //positif - nbOfCube = (int)(( zeroInHisto - posInHisto)/bcCubeHeight); - for(int j = 0 ; j < nbOfCube; ++j){ - barChart.rect((i-start)*histowidth, posInHisto + j * bcCubeHeight, histowidth, bcCubeHeight); - } - }else{ - nbOfCube = (int)((posInHisto - zeroInHisto)/bcCubeHeight); - for(int j = 0 ; j < nbOfCube; ++j){ - barChart.rect((i-start)*histowidth, zeroInHisto + j * bcCubeHeight, histowidth, bcCubeHeight); - } + for (int i = start; i < scores.size(); ++i) { + float zeroInHisto = drawingFactor * (maxScore); + float posInHisto = drawingFactor * (-scores.get(i)+maxScore) ; + int nbOfCube ; + if (zeroInHisto>posInHisto) { //positif + nbOfCube = (int)(( zeroInHisto - posInHisto)/bcCubeHeight); + for (int j = 0; j < nbOfCube; ++j) { + barChart.rect((i-start)*histowidth, posInHisto + j * bcCubeHeight, histowidth, bcCubeHeight); } - //float top = (zeroInHistoposInHisto)?zeroInHisto:posInHisto)-top; - //barChart.rect((i-start)*histowidth, top, histowidth, bot); + } else { + nbOfCube = (int)((posInHisto - zeroInHisto)/bcCubeHeight); + for (int j = 0; j < nbOfCube; ++j) { + barChart.rect((i-start)*histowidth, zeroInHisto + j * bcCubeHeight, histowidth, bcCubeHeight); + } + } + //float top = (zeroInHistoposInHisto)?zeroInHisto:posInHisto)-top; + //barChart.rect((i-start)*histowidth, top, histowidth, bot); } barChart.endDraw(); } -void updateScore(float points){ +void updateScore(float points) { score += points; scores.add(score); minScore = (scoremaxScore)? score : maxScore; drawingFactor = barChartHeight/(maxScore-minScore+2); } -void drawScoreboard(){ +void drawScoreboard() { scoreboard.beginDraw(); - scoreboard.background(242,86,2); + scoreboard.background(242, 86, 2); scoreTimer = (scoreTimer%prVeloFreq == prVeloFreq-1) ? 0 : ++scoreTimer; veloMag = (scoreTimer == 0)? mover.velocity.mag():veloMag; scoreboard.fill(0); scoreboard.textSize(25); - scoreboard.text("Total Score : \n "+f.format(score), 15,40); - scoreboard.text("Velocity : \n "+f.format(veloMag),15,130); - scoreboard.text("Last score : \n "+f.format(lastScore),15,220); - + scoreboard.text("Total Score : \n "+f.format(score), 15, 40); + scoreboard.text("Velocity : \n "+f.format(veloMag), 15, 130); + scoreboard.text("Last score : \n "+f.format(lastScore), 15, 220); + scoreboard.endDraw(); } void drawTopView() { topView.beginDraw(); - topView.background(181,65,2); + topView.background(181, 65, 2); topView.fill(0, 150, 150); topView.rect(0, 0, topViewWidth, topViewWidth); topView.fill(255, 255, 255); float topViewCylRad=map(cyl.cylinderBaseSize, 0, plate_w/2, 0, topViewWidth); if (partSys!=null) { for (PVector pv : partSys.cylinders) { float topViewCylX=map(pv.x, -plate_w/2, plate_w/2, 0, topViewWidth); float topViewCylY=map(pv.y, -plate_w/2, plate_w/2, 0, topViewWidth); topView.ellipse(topViewCylX, topViewCylY, topViewCylRad, topViewCylRad); } if (partSys.cylinders.contains(partSys.mainCyl)) { topView.fill(255, 0, 0); float topViewCylX=map(partSys.mainCyl.x, -plate_w/2, plate_w/2, 0, topViewWidth); float topViewCylY=map(partSys.mainCyl.y, -plate_w/2, plate_w/2, 0, topViewWidth); topView.ellipse(topViewCylX, topViewCylY, topViewCylRad, topViewCylRad); } } - topView.fill(0,0,255); + topView.fill(0, 0, 255); float topViewBallRad=map(radius, 0, plate_w/2, 0, topViewWidth); float topViewBallX=map(mover.location.x, -plate_w/2, plate_w/2, 0, topViewWidth); float topViewBallY=map(mover.location.y, -plate_w/2, plate_w/2, 0, topViewWidth); topView.ellipse(topViewBallX, topViewBallY, topViewBallRad, topViewBallRad); topView.endDraw(); } void drawStat() { statSurface.beginDraw(); statSurface.background(0, 0, 0, 0); statSurface.fill(0); statSurface.textSize(25); statSurface.text("Rotation X : "+f.format(rx/Math.PI*180)+"° Rotation Z : "+f.format(rz/Math.PI*180)+"° speed: "+f.format(speed/10), 5, 25); statSurface.text("Ball location : ("+f2.format(mover.location.x)+","+f2.format(mover.location.y)+")", 5, 50); statSurface.text("Ball velocity : ("+f2.format(mover.velocity.x)+","+f2.format(mover.velocity.y)+")", 5, 75); statSurface.endDraw(); } void drawGame() { gameSurface.beginDraw(); - if(GameOver){gameSurface.background(50,65,200); - }else{gameSurface.background(100, 0, 0);} - + if (GameOver) { + gameSurface.background(50, 65, 200); + } else { + gameSurface.background(100, 0, 0); + } + gameSurface.directionalLight(50, 100, 125, 0, 1, 0); gameSurface.ambientLight(102, 102, 102); gameSurface.pointLight(150, 150, 150, 0, -200, 0); if (!shiftMod) { mover.update(); mover.checkEdges(); if (partSys!=null) { - GameOver|=mover.checkCylinderCollision(partSys.cylinders, radius,partSys.mainCyl); + GameOver|=mover.checkCylinderCollision(partSys.cylinders, radius, partSys.mainCyl); } rz = map(x_coord, 0, width, -PI/3, PI/3); rx = map(y_coord, 0, height, -PI/3, PI/3); cameraHeight=400; } else { rz=0; rx=-PI/2; cameraHeight=0; } gameSurface.pushMatrix(); gameSurface.camera(0, -cameraHeight, depth, 0, 0, 0, 0, 1, 0); gameSurface.rotateZ(rz); gameSurface.rotateX(rx); gameSurface.shininess(100); gameSurface.box(plate_w, plate_t, plate_w); //particles if (partSys!=null) { if (frameCount-previousFrame>=timeIntervalParticle*frameRate && !shiftMod) { timeIntervalParticle=random(0.2, 1.2); previousFrame=frameCount; partSys.addParticle(); } partSys.run(-(plate_t/2), PI/2); } if (partW!=null && GameOver) { partW.updateNdraw(); } if (shiftMod) { - xOnPlane=map(mouseX,gameSurface.screenX(-plate_w/2,0,0),gameSurface.screenX(plate_w/2,0,0),-plate_w/2,plate_w/2); - yOnPlane=map(mouseY,gameSurface.screenY(0,0,-plate_w/2),gameSurface.screenY(0,0,plate_w/2),-plate_w/2,plate_w/2); + xOnPlane=map(mouseX, gameSurface.screenX(-plate_w/2, 0, 0), gameSurface.screenX(plate_w/2, 0, 0), -plate_w/2, plate_w/2); + yOnPlane=map(mouseY, gameSurface.screenY(0, 0, -plate_w/2), gameSurface.screenY(0, 0, plate_w/2), -plate_w/2, plate_w/2); cyl.draw(xOnPlane, yOnPlane, -(plate_t/2), PI/2); } gameSurface.translate(mover.getX(), -(plate_t/2+radius), mover.getY()); gameSurface.fill(255, 100, 100); gameSurface.rotateX(mover.getRotX()); gameSurface.rotateZ(mover.getRotZ()); gameSurface.shape(globe); gameSurface.popMatrix(); //Draw the text gameSurface.pushMatrix(); - + if (shiftMod) { gameSurface.textSize(50); gameSurface.text("[EDIT MOD ON]", 50, 500); } gameSurface.fill(255); gameSurface.popMatrix(); gameSurface.endDraw(); } void mouseWheel(MouseEvent event) { float e = event.getCount(); if (e<0) { speed-=speed<=2?0:1; } else { speed+=speed>=15?0:1; } } void mouseDragged() { - if (!shiftMod && mousePermittedToMove) { - float d=-(mX-mouseX)*speed/10; - x_coord=x_coord+d<=0?0:x_coord+d>=width?width:x_coord+d; - mX=mouseX; - d=(mY-mouseY)*speed/10; - y_coord=y_coord+d<=0?0:y_coord+d>=height?height:y_coord+d; - mY=mouseY; - } + /*if (!shiftMod && mousePermittedToMove) { + float d=-(mX-mouseX)*speed/10; + x_coord=x_coord+d<=0?0:x_coord+d>=width?width:x_coord+d; + mX=mouseX; + d=(mY-mouseY)*speed/10; + y_coord=y_coord+d<=0?0:y_coord+d>=height?height:y_coord+d; + mY=mouseY; + }*/ } void mousePressed() { - if(mouseY>height-botPartHeight){ + if (mouseY>height-botPartHeight) { mousePermittedToMove=false; - }else{ + } else { mousePermittedToMove=true; } mX = mouseX; mY = mouseY; if (shiftMod && -plate_w/2 2*radius || Math.abs(yOnPlane-mover.location.y) > 2*radius)) { partSys=new ParticleSystem(new PVector(xOnPlane, yOnPlane), mover, radius); GameOver=false; } } void keyPressed() { if (keyCode == SHIFT) { shiftMod=true; } } void keyReleased() { if (keyCode == SHIFT) { shiftMod=false; } } diff --git a/Project/Game/HoughTransform.pde b/Project/Game/HoughTransform.pde new file mode 100644 index 0000000..2f292bd --- /dev/null +++ b/Project/Game/HoughTransform.pde @@ -0,0 +1,151 @@ +void drawLines(ArrayList lines, PImage edgeImg) { + + for (int idx = 0; idx < lines.size(); idx++) { + PVector line=lines.get(idx); + float r = line.x; + float phi = line.y; + // Cartesian equation of a line: y = ax + b + // in polar, y = (-cos(phi)/sin(phi))x + (r/sin(phi)) + // => y = 0 : x = r / cos(phi) + // => x = 0 : y = r / sin(phi) + // compute the intersection of this line with the 4 borders of + // the image + int x0 = 0; + int y0 = (int) (r / sin(phi)); + int x1 = (int) (r / cos(phi)); + int y1 = 0; + int x2 = edgeImg.width; + int y2 = (int) (-cos(phi) / sin(phi) * x2 + r / sin(phi)); + int y3 = edgeImg.width; + int x3 = (int) (-(y3 - r / sin(phi)) * (sin(phi) / cos(phi))); + // Finally, plot the lines + stroke(0, 255, 255); + if (y0 > 0) { + if (x1 > 0) + line(x0, y0, x1, y1); + else if (y2 > 0) + line(x0, y0, x2, y2); + else + line(x0, y0, x3, y3); + } else { + if (x1 > 0) { + if (y2 > 0) + line(x1, y1, x2, y2); + else + line(x1, y1, x3, y3); + } else + line(x2, y2, x3, y3); + } + } +} + +int minVotes=50; + +ArrayList hough(PImage edgeImg, int nLines) { + + float discretizationStepsPhi = 0.06f; + float discretizationStepsR = 2.5f; + + + ArrayList bestCandidates=new ArrayList(); + + // dimensions of the accumulator + int phiDim = (int) (Math.PI / discretizationStepsPhi +1); + //The max radius is the image diagonal, but it can be also negative + int rDim = (int) ((sqrt(edgeImg.width*edgeImg.width + + edgeImg.height*edgeImg.height) * 2) / discretizationStepsR +1); + // our accumulator + int[] accumulator = new int[phiDim * rDim]; + // Fill the accumulator: on edge points (ie, white pixels of the edge + // image), store all possible (r, phi) pairs describing lines going + // through the point. + + // pre-compute the sin and cos values + float[] tabSin = new float[phiDim]; + float[] tabCos = new float[phiDim]; + float ang = 0; + float inverseR = 1.f / discretizationStepsR; + for (int accPhi = 0; accPhi < phiDim; ang += discretizationStepsPhi, accPhi++) { + // we can also pre-multiply by (1/discretizationStepsR) since we need it in the Hough loop + tabSin[accPhi] = (float) (Math.sin(ang) * inverseR); + tabCos[accPhi] = (float) (Math.cos(ang) * inverseR); + } + + + for (int y = 0; y < edgeImg.height; y++) { + for (int x = 0; x < edgeImg.width; x++) { + // Are we on an edge? + if (brightness(edgeImg.pixels[y * edgeImg.width + x]) != 0) { + // ...determine here all the lines (r, phi) passing through + // pixel (x,y), convert (r,phi) to coordinates in the + // accumulator, and increment accordingly the accumulator. + // Be careful: r may be negative, so you may want to center onto + // the accumulator: r += rDim / 2 + for (int phi=0; phi minVotes && isMaxOverArea(accumulator, idx, REGION_SIZE, phiDim, rDim)) { + bestCandidates.add(idx); + } + } + + bestCandidates.sort(new HoughComparator(accumulator)); + ArrayList lines=new ArrayList(); + + for (int i=0; ithreshold) { + return false; + } + } + } + return true; +} + +class HoughComparator implements java.util.Comparator { + int[] accumulator; + public HoughComparator(int[] accumulator) { + this.accumulator = accumulator; + } + @Override + public int compare(Integer l1, Integer l2) { + if (accumulator[l1] > accumulator[l2] + || (accumulator[l1] == accumulator[l2] && l1 < l2)) return -1; + return 1; + } +} diff --git a/Project/Game/QuadGraph.pde b/Project/Game/QuadGraph.pde new file mode 100644 index 0000000..850fa29 --- /dev/null +++ b/Project/Game/QuadGraph.pde @@ -0,0 +1,384 @@ +import java.util.Collections; +import java.util.Comparator; +import java.util.List; +import java.util.ArrayList; +import java.util.Map; +class QuadGraph { + + boolean verbose=false; + + List cycles = new ArrayList(); + int[][] graph; + + List findBestQuad(List lines, int width, int height, int max_quad_area, int min_quad_area, boolean verbose) { + this.verbose=verbose; + build(lines, width, height); //<>// + findCycles(verbose); + ArrayList bestQuad=new ArrayList(); + float bestQuadArea=0; + for (int [] cy : cycles) { + ArrayList quad= new ArrayList(); + PVector l1 = lines.get(cy[0]); + PVector l2 = lines.get(cy[1]); + PVector l3 = lines.get(cy[2]); + PVector l4 = lines.get(cy[3]); + + quad.add(intersection(l1, l2)); + quad.add(intersection(l2, l3)); + quad.add(intersection(l3, l4)); + quad.add(intersection(l4, l1)); + quad=sortCorners(quad); + + PVector c1 = quad.get(0); + PVector c2 = quad.get(1); + PVector c3 = quad.get(2); + PVector c4 = quad.get(3); + + if (isConvex(c1, c2, c3, c4) && + nonFlatQuad(c1, c2, c3, c4)) { + float quadArea=validArea(c1, c2, c3, c4, max_quad_area, min_quad_area); + if (quadArea>0 && quadArea>bestQuadArea) { + bestQuadArea=quadArea; + bestQuad=quad; + } + } + } + + if (bestQuadArea>0) + return bestQuad; + else + return new ArrayList(); + } + + + void build(List lines, int width, int height) { + + int n = lines.size(); + + // The maximum possible number of edges is n * (n - 1)/2 + graph = new int[n * (n - 1)/2][2]; + + int idx =0; + + for (int i = 0; i < lines.size(); i++) { + for (int j = i + 1; j < lines.size(); j++) { + if (intersect(lines.get(i), lines.get(j), width, height)) { + + graph[idx][0]=i; + graph[idx][1]=j; + idx++; + } + } + } + } + + /** Returns true if polar lines 1 and 2 intersect + * inside an area of size (width, height) + */ + boolean intersect(PVector line1, PVector line2, int width, int height) { + + double sin_t1 = Math.sin(line1.y); + double sin_t2 = Math.sin(line2.y); + double cos_t1 = Math.cos(line1.y); + double cos_t2 = Math.cos(line2.y); + float r1 = line1.x; + float r2 = line2.x; + + double denom = cos_t2 * sin_t1 - cos_t1 * sin_t2; + + int x = (int) ((r2 * sin_t1 - r1 * sin_t2) / denom); + int y = (int) ((-r2 * cos_t1 + r1 * cos_t2) / denom); + + if (0 <= x && 0 <= y && width >= x && height >= y) + return true; + else + return false; + } + + PVector intersection(PVector line1, PVector line2) { + + double sin_t1 = Math.sin(line1.y); + double sin_t2 = Math.sin(line2.y); + double cos_t1 = Math.cos(line1.y); + double cos_t2 = Math.cos(line2.y); + float r1 = line1.x; + float r2 = line2.x; + + double denom = cos_t2 * sin_t1 - cos_t1 * sin_t2; + + int x = (int) ((r2 * sin_t1 - r1 * sin_t2) / denom); + int y = (int) ((-r2 * cos_t1 + r1 * cos_t2) / denom); + + return new PVector(x,y); + } + + void findCycles(boolean verbose) { + cycles.clear(); + for (int i = 0; i < graph.length; i++) { + for (int j = 0; j < graph[i].length; j++) { + findNewCycles(new int[] {graph[i][j]}); + } + } + if (verbose) { + for (int[] cy : cycles) { + String s = "" + cy[0]; + for (int i = 1; i < cy.length; i++) { + s += "," + cy[i]; + } + System.out.println(s); + } + } + } + + void findNewCycles(int[] path) + { + int n = path[0]; + int x; + int[] sub = new int[path.length + 1]; + + for (int i = 0; i < graph.length; i++) + for (int y = 0; y <= 1; y++) + if (graph[i][y] == n) + // edge refers to our current node + { + x = graph[i][(y + 1) % 2]; + if (!visited(x, path)) + // neighbor node not on path yet + { + sub[0] = x; + System.arraycopy(path, 0, sub, 1, path.length); + // explore extended path + findNewCycles(sub); + } else if ((path.length == 4) && (x == path[path.length - 1])) + // cycle found + { + int[] p = normalize(path); + int[] inv = invert(p); + if (isNew(p) && isNew(inv)) + { + cycles.add(p); + } + } + } + } + + // check of both arrays have same lengths and contents + Boolean equals(int[] a, int[] b) + { + Boolean ret = (a[0] == b[0]) && (a.length == b.length); + + for (int i = 1; ret && (i < a.length); i++) + { + if (a[i] != b[i]) + { + ret = false; + } + } + + return ret; + } + + // create a path array with reversed order + int[] invert(int[] path) + { + int[] p = new int[path.length]; + + for (int i = 0; i < path.length; i++) + { + p[i] = path[path.length - 1 - i]; + } + + return normalize(p); + } + + // rotate cycle path such that it begins with the smallest node + int[] normalize(int[] path) + { + int[] p = new int[path.length]; + int x = smallest(path); + int n; + + System.arraycopy(path, 0, p, 0, path.length); + + while (p[0] != x) + { + n = p[0]; + System.arraycopy(p, 1, p, 0, p.length - 1); + p[p.length - 1] = n; + } + + return p; + } + + // compare path against known cycles + // return true, iff path is not a known cycle + Boolean isNew(int[] path) + { + Boolean ret = true; + + for (int[] p : cycles) + { + if (equals(p, path)) + { + ret = false; + break; + } + } + + return ret; + } + + // return the int of the array which is the smallest + int smallest(int[] path) + { + int min = path[0]; + + for (int p : path) + { + if (p < min) + { + min = p; + } + } + + return min; + } + + // check if vertex n is contained in path + Boolean visited(int n, int[] path) + { + Boolean ret = false; + + for (int p : path) + { + if (p == n) + { + ret = true; + break; + } + } + + return ret; + } + + + + /** Check if a quad is convex or not. + * + * Algo: take two adjacent edges and compute their cross-product. + * The sign of the z-component of all the cross-products is the + * same for a convex polygon. + * + * See http://debian.fmi.uni-sofia.bg/~sergei/cgsr/docs/clockwise.htm + * for justification. + * + * @param c1 + */ + boolean isConvex(PVector c1, PVector c2, PVector c3, PVector c4) { + + PVector v21= PVector.sub(c1, c2); + PVector v32= PVector.sub(c2, c3); + PVector v43= PVector.sub(c3, c4); + PVector v14= PVector.sub(c4, c1); + + float i1=v21.cross(v32).z; + float i2=v32.cross(v43).z; + float i3=v43.cross(v14).z; + float i4=v14.cross(v21).z; + + if ( (i1>0 && i2>0 && i3>0 && i4>0) + || (i1<0 && i2<0 && i3<0 && i4<0)) + return true; + else if(verbose) + System.out.println("Eliminating non-convex quad"); + return false; + } + + /** Compute the area of a quad, and check it lays within a specific range + */ + float validArea(PVector c1, PVector c2, PVector c3, PVector c4, float max_area, float min_area) { + + float i1=c1.cross(c2).z; + float i2=c2.cross(c3).z; + float i3=c3.cross(c4).z; + float i4=c4.cross(c1).z; + + float area = Math.abs(0.5f * (i1 + i2 + i3 + i4)); + + if (area < max_area && area > min_area){ + return area; + } + return 0; + + } + + /** Compute the (cosine) of the four angles of the quad, and check they are all large enough + * (the quad representing our board should be close to a rectangle) + */ + boolean nonFlatQuad(PVector c1, PVector c2, PVector c3, PVector c4) { + + // cos(70deg) ~= 0.3 + float min_cos = 0.5f; + + PVector v21= PVector.sub(c1, c2); + PVector v32= PVector.sub(c2, c3); + PVector v43= PVector.sub(c3, c4); + PVector v14= PVector.sub(c4, c1); + + float cos1=Math.abs(v21.dot(v32) / (v21.mag() * v32.mag())); + float cos2=Math.abs(v32.dot(v43) / (v32.mag() * v43.mag())); + float cos3=Math.abs(v43.dot(v14) / (v43.mag() * v14.mag())); + float cos4=Math.abs(v14.dot(v21) / (v14.mag() * v21.mag())); + + if (cos1 < min_cos && cos2 < min_cos && cos3 < min_cos && cos4 < min_cos) + return true; + else { + if(verbose) + System.out.println("Flat quad"); + return false; + } + } + + + ArrayList sortCorners(ArrayList quad) { + + // 1 - Sort corners so that they are ordered clockwise + PVector a = quad.get(0); + PVector b = quad.get(2); + + PVector center = new PVector((a.x+b.x)/2, (a.y+b.y)/2); + + Collections.sort(quad, new CWComparator(center)); + + + + // 2 - Sort by upper left most corner + PVector origin = new PVector(0, 0); + float distToOrigin = 1000; + + for (PVector p : quad) { + if (p.dist(origin) < distToOrigin) distToOrigin = p.dist(origin); + } + + while (quad.get(0).dist(origin) != distToOrigin) + Collections.rotate(quad, 1); + + return quad; + } +} + +class CWComparator implements Comparator { + + PVector center; + + public CWComparator(PVector center) { + this.center = center; + } + + @Override + public int compare(PVector b, PVector d) { + if (Math.atan2(b.y-center.y, b.x-center.x)0) { + cutOffFreq=sampleRate/2; + alpha= (1/sampleRate)/(1/sampleRate + 1/cutOffFreq); + } + } + + PVector get3DRotations(List points2D) { + + // 1- Solve the extrinsic matrix from the projected 2D points + double[][] E = solveExtrinsicMatrix(points2D); + + + // 2 - Re-build a proper 3x3 rotation matrix from the camera's + // extrinsic matrix E + PVector firstColumn=new PVector((float)E[0][0], (float)E[1][0], (float)E[2][0]); + PVector secondColumn=new PVector((float)E[0][1], (float)E[1][1], (float)E[2][1]); + firstColumn.normalize(); + secondColumn.normalize(); + PVector thirdColumn=firstColumn.cross(secondColumn); + float [][] rotationMatrix={{firstColumn.x, secondColumn.x, thirdColumn.x}, + {firstColumn.y, secondColumn.y, thirdColumn.y}, + {firstColumn.z, secondColumn.z, thirdColumn.z}}; + + if (sampleRate>0) + filter(rotationMatrix, false); + + // 3 - Computes and returns Euler angles (rx, ry, rz) from this matrix + return rotationFromMatrix(rotationMatrix); + } + + + double[][] solveExtrinsicMatrix(List points2D) { + + // p ~= K · [R|t] · P + // with P the (3D) corners of the physical board, p the (2D) + // projected points onto the webcam image, K the intrinsic + // matrix and R and t the rotation and translation we want to + // compute. + // + // => We want to solve: (K^(-1) · p) X ([R|t] · P) = 0 + + float[][] projectedCorners = new float[4][3]; + + if(points2D.size() >= 4) + for (int i=0; i<4; i++) { + // TODO: + // store in projectedCorners the result of (K^(-1) · p), for each + // corner p found in the webcam image. + // You can use PVector dot function for computing dot product between K^(-1) lines and p. + //Do not forget to normalize the result + PVector point =points2D.get(i); + projectedCorners[i][0]=point.dot(invK_r1)/point.dot(invK_r3); + projectedCorners[i][1]=point.dot(invK_r2)/point.dot(invK_r3); + projectedCorners[i][2]=1; + } + + // 'A' contains the cross-product (K^(-1) · p) X P + float[][] A= new float[12][9]; + + for (int i=0; i<4; i++) { + A[i*3][0]=0; + A[i*3][1]=0; + A[i*3][2]=0; + + // note that we take physicalCorners[0,1,*3*]: we drop the Z + // coordinate and use the 2D homogenous coordinates of the physical + // corners + A[i*3][3]=-projectedCorners[i][2] * physicalCorners[i][0]; + A[i*3][4]=-projectedCorners[i][2] * physicalCorners[i][1]; + A[i*3][5]=-projectedCorners[i][2] * physicalCorners[i][3]; + + A[i*3][6]= projectedCorners[i][1] * physicalCorners[i][0]; + A[i*3][7]= projectedCorners[i][1] * physicalCorners[i][1]; + A[i*3][8]= projectedCorners[i][1] * physicalCorners[i][3]; + + A[i*3+1][0]= projectedCorners[i][2] * physicalCorners[i][0]; + A[i*3+1][1]= projectedCorners[i][2] * physicalCorners[i][1]; + A[i*3+1][2]= projectedCorners[i][2] * physicalCorners[i][3]; + + A[i*3+1][3]=0; + A[i*3+1][4]=0; + A[i*3+1][5]=0; + + A[i*3+1][6]=-projectedCorners[i][0] * physicalCorners[i][0]; + A[i*3+1][7]=-projectedCorners[i][0] * physicalCorners[i][1]; + A[i*3+1][8]=-projectedCorners[i][0] * physicalCorners[i][3]; + + A[i*3+2][0]=-projectedCorners[i][1] * physicalCorners[i][0]; + A[i*3+2][1]=-projectedCorners[i][1] * physicalCorners[i][1]; + A[i*3+2][2]=-projectedCorners[i][1] * physicalCorners[i][3]; + + A[i*3+2][3]= projectedCorners[i][0] * physicalCorners[i][0]; + A[i*3+2][4]= projectedCorners[i][0] * physicalCorners[i][1]; + A[i*3+2][5]= projectedCorners[i][0] * physicalCorners[i][3]; + + A[i*3+2][6]=0; + A[i*3+2][7]=0; + A[i*3+2][8]=0; + } + + for (int i=0; i<12; i++) + for (int j=0; j<9; j++) + opencv_A.put(i, j, A[i][j]); + + Core.SVDecomp(opencv_A, w, u, vt); + + for (int i=0; i<9; i++) + for (int j=0; j<9; j++) + V[j][i]=vt.get(i, j)[0]; + + double[][] E = new double[3][3]; + + //E is the last column of V + for (int i=0; i<9; i++) { + E[i/3][i%3] = V[i][V.length-1] / V[8][V.length-1]; + } + + return E; + } + + PVector rotationFromMatrix(float[][] mat) { + + // Assuming rotation order is around x,y,z + PVector rot = new PVector(); + + if (mat[1][0] > 0.998) { // singularity at north pole + rot.z = 0; + float delta = (float) Math.atan2(mat[0][1], mat[0][2]); + rot.y = -(float) Math.PI/2; + rot.x = -rot.z + delta; + return rot; + } + + if (mat[1][0] < -0.998) { // singularity at south pole + rot.z = 0; + float delta = (float) Math.atan2(mat[0][1], mat[0][2]); + rot.y = (float) Math.PI/2; + rot.x = rot.z + delta; + return rot; + } + + rot.y =-(float)Math.asin(mat[2][0]); + rot.x = (float)Math.atan2(mat[2][1]/Math.cos(rot.y), mat[2][2]/Math.cos(rot.y)); + rot.z = (float)Math.atan2(mat[1][0]/Math.cos(rot.y), mat[0][0]/Math.cos(rot.y)); + + return rot; + } + + int filter(float m[][], boolean reset) { + + float[] q= new float[4]; + float alpha, oneminusalpha, omega, cosomega, sinomega, s0, s1; + + mat2Quat(m, q); + if (nomalizeQuaternion(q)<0) return -1; + + if (reset) { + this.q[0] = q[0]; + this.q[1] = q[1]; + this.q[2] = q[2]; + this.q[3] = q[3]; + } else { + alpha = this.alpha; + + oneminusalpha = 1.0 - alpha; + + // SLERP for orientation. + cosomega = q[0]*this.q[0] + q[1]*this.q[1] + q[2]*this.q[2] + q[3]*this.q[3]; // cos of angle between vectors. + if (cosomega < 0.0) { + cosomega = -cosomega; + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } + if (cosomega > 0.9995) { + s0 = oneminusalpha; + s1 = alpha; + } else { + omega = acos(cosomega); + sinomega = sin(omega); + s0 = sin(oneminusalpha * omega) / sinomega; + s1 = sin(alpha * omega) / sinomega; + } + this.q[0] = q[0]*s1 + this.q[0]*s0; + this.q[1] = q[1]*s1 + this.q[1]*s0; + this.q[2] = q[2]*s1 + this.q[2]*s0; + this.q[3] = q[3]*s1 + this.q[3]*s0; + nomalizeQuaternion(this.q); + } + + if (quat2Mat(this.q, m) < 0) return (-2); + + return (0); + } + + + int nomalizeQuaternion(float[] q) {// Normalise quaternion. + float mag2 = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; + if (mag2==0) return (-1); + + float mag = sqrt(mag2); + + q[0] /= mag; + q[1] /= mag; + q[2] /= mag; + q[3] /= mag; + + return (0); + } + + int mat2Quat(float m[][], float q[]) { + float t, s; + t = m[0][0] + m[1][1] + m[2][2] + 1.0; + if (t > 0.0001) { + s = sqrt(t) * 2.0; + q[0] = (m[1][2] - m[2][1]) / s; + q[1] = (m[2][0] - m[0][2]) / s; + q[2] = (m[0][1] - m[1][0]) / s; + q[3] = 0.25 * s; + } else { + if (m[0][0] > m[1][1] && m[0][0] > m[2][2]) { // Column 0: + s = sqrt(1.0 + m[0][0] - m[1][1] - m[2][2]) * 2.0; + q[0] = 0.25 * s; + q[1] = (m[0][1] + m[1][0] ) / s; + q[2] = (m[2][0] + m[0][2] ) / s; + q[3] = (m[1][2] - m[2][1] ) / s; + } else if (m[1][1] > m[2][2]) { // Column 1: + s = sqrt(1.0 + m[1][1] - m[0][0] - m[2][2]) * 2.0; + q[0] = (m[0][1] + m[1][0] ) / s; + q[1] = 0.25 * s; + q[2] = (m[1][2] + m[2][1] ) / s; + q[3] = (m[2][0] - m[0][2] ) / s; + } else { // Column 2: + s = sqrt(1.0 + m[2][2] - m[0][0] - m[1][1]) * 2.0; + q[0] = (m[2][0] + m[0][2] ) / s; + q[1] = (m[1][2] + m[2][1] ) / s; + q[2] = 0.25 * s; + q[3] = (m[0][1] - m[1][0] ) / s; + } + } + return 0; + } + + int quat2Mat( float q[], float m[][] ) + { + float x2, y2, z2; + float xx, xy, xz; + float yy, yz, zz; + float wx, wy, wz; + + x2 = q[0] * 2.0; + y2 = q[1] * 2.0; + z2 = q[2] * 2.0; + + xx = q[0] * x2; + xy = q[0] * y2; + xz = q[0] * z2; + yy = q[1] * y2; + yz = q[1] * z2; + zz = q[2] * z2; + wx = q[3] * x2; + wy = q[3] * y2; + wz = q[3] * z2; + + m[0][0] = 1.0 - (yy + zz); + m[1][1] = 1.0 - (xx + zz); + m[2][2] = 1.0 - (xx + yy); + + m[1][0] = xy - wz; + m[0][1] = xy + wz; + m[2][0] = xz + wy; + m[0][2] = xz - wy; + m[2][1] = yz - wx; + m[1][2] = yz + wx; + + return 0; + } +}