001/** 002 * Copyright (c) 2011, The University of Southampton and the individual contributors. 003 * All rights reserved. 004 * 005 * Redistribution and use in source and binary forms, with or without modification, 006 * are permitted provided that the following conditions are met: 007 * 008 * * Redistributions of source code must retain the above copyright notice, 009 * this list of conditions and the following disclaimer. 010 * 011 * * Redistributions in binary form must reproduce the above copyright notice, 012 * this list of conditions and the following disclaimer in the documentation 013 * and/or other materials provided with the distribution. 014 * 015 * * Neither the name of the University of Southampton nor the names of its 016 * contributors may be used to endorse or promote products derived from this 017 * software without specific prior written permission. 018 * 019 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 020 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 021 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 022 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 023 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 024 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 025 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 026 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 027 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 028 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 029 */ 030package org.openimaj.demos.touchtable; 031 032import java.io.IOException; 033import java.io.PrintWriter; 034import java.util.Scanner; 035 036import org.openimaj.math.geometry.point.Point2d; 037 038import Jama.Matrix; 039 040public class HomographyCameraConfig implements CameraConfig { 041 042 Matrix cameraMatrix; 043 float[] distortion; 044 Matrix homography; 045 046 public HomographyCameraConfig(){ 047 this.cameraMatrix = new Matrix(3,3); 048 this.homography = new Matrix(3,3); 049 this.distortion = new float[8]; 050 } 051 052 public HomographyCameraConfig(float f, float g){ 053 this(); 054 this.cameraMatrix.set(0, 2, f); 055 this.cameraMatrix.set(1, 2, g); 056 this.cameraMatrix.set(0, 0, 1.0f); 057 this.cameraMatrix.set(1, 1, 1.0f); 058 } 059 060 public HomographyCameraConfig(float fx, float fy,float cx, float cy, float k1, float k2, float p1, float p2){ 061 this(); 062 this.cameraMatrix.set(0, 2, cx); 063 this.cameraMatrix.set(1, 2, cy); 064 this.cameraMatrix.set(0, 0, fx); 065 this.cameraMatrix.set(1, 1, fy); 066 this.distortion[0] = k1; 067 this.distortion[1] = k2; 068 this.distortion[2] = p1; 069 this.distortion[3] = p2; 070 this.distortion[4] = 0; 071 this.distortion[5] = 0; 072 this.distortion[6] = 0; 073 this.distortion[7] = 0; 074 } 075 076 @Override 077 public Touch transformTouch(Touch point){ 078 int distortion_iterations = 5; // From OpenCV 079 double x, y, x0, y0; 080 081 double cx = cameraMatrix.get(0,2); 082 double cy = cameraMatrix.get(1,2); 083 084 double fx = cameraMatrix.get(0,0) / 10; 085 double fy = cameraMatrix.get(1,1) / 10; 086 087 double ifx = 1/fx; 088 double ify = 1/fy; 089 090 float k1 = distortion[0] ; 091 float k2 = distortion[1]; 092 float p1 = distortion[2]; 093 float p2 = distortion[3]; 094 float k3 = distortion[4]; 095 float k4 = distortion[5]; 096 float k5 = distortion[6]; 097 float k6 = distortion[7]; 098 099 x = point.getX(); 100 y = point.getY(); 101 102 x0 = x = (x - cx)*ifx; 103 y0 = y = (y - cy)*ify; 104 105 106 // compensate distortion iteratively 107 for( int j = 0; j < distortion_iterations; j++ ) 108 { 109 double r2 = x*x + y*y; 110 double icdist = (1 + ((k6*r2 + k5)*r2 + k4)*r2)/(1 + ((k3*r2 + k2)*r2 + k1)*r2); 111 double deltaX = 2*p1*x*y + p2*(r2 + 2*x*x); 112 double deltaY = p1*(r2 + 2*y*y) + 2*p2*x*y; 113 x = (x0 - deltaX)*icdist; 114 y = (y0 - deltaY)*icdist; 115 } 116 117 point.setX((float) x); 118 point.setY((float) y); 119 point.translate((float)cx, (float)cy); 120 121 Point2d newc = point.calculateCentroid().transform(homography); 122 123 return new Touch(newc.getX(),newc.getY(),point.getRadius(), point.touchID, point.motionVector); 124 125 } 126 127 @Override 128 public String asciiHeader() { 129 return ""; 130 } 131 132 @Override 133 public void readASCII(Scanner in) throws IOException { 134 135 cameraMatrix.set(0,0,in.nextFloat()); 136 cameraMatrix.set(1,1,in.nextFloat()); 137 cameraMatrix.set(0,2,in.nextFloat()); 138 cameraMatrix.set(1,2,in.nextFloat()); 139 140 homography.set(0,0,in.nextFloat());homography.set(0,1,in.nextFloat());homography.set(0,2,in.nextFloat()); 141 homography.set(1,0,in.nextFloat());homography.set(1,1,in.nextFloat());homography.set(1,2,in.nextFloat()); 142 homography.set(2,0,in.nextFloat());homography.set(2,1,in.nextFloat());homography.set(2,2,in.nextFloat()); 143 144 for (int i = 0; i < distortion.length; i++) { 145 distortion[i] = in.nextFloat(); 146 } 147 } 148 149 @Override 150 public void writeASCII(PrintWriter out) throws IOException { 151 out.format("%f %f %f %f\n",cameraMatrix.get(0,0),cameraMatrix.get(1,1),cameraMatrix.get(0,2),cameraMatrix.get(1,2)); 152 out.format("%f %f %f %f %f %f %f %f %f\n", 153 homography.get(0,0),homography.get(0,1),homography.get(0,2), 154 homography.get(1,0),homography.get(1,1),homography.get(1,2), 155 homography.get(2,0),homography.get(2,1),homography.get(2,2) 156 ); 157 158 out.format("%f %f %f %f %f %f %f %f\n", 159 distortion[0],distortion[1],distortion[2],distortion[3], 160 distortion[4],distortion[5],distortion[6],distortion[7] 161 ); 162 } 163 164}