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.hardware; 031 032import java.awt.event.KeyEvent; 033import java.awt.event.KeyListener; 034import java.io.File; 035import java.io.FileNotFoundException; 036import java.io.PrintWriter; 037import java.util.ArrayList; 038import java.util.Arrays; 039import java.util.List; 040 041import javax.swing.JFrame; 042import javax.swing.JOptionPane; 043import javax.swing.SwingUtilities; 044 045import org.openimaj.demos.Demo; 046import org.openimaj.hardware.kinect.KinectController; 047import org.openimaj.hardware.kinect.KinectException; 048import org.openimaj.image.DisplayUtilities; 049import org.openimaj.image.FImage; 050import org.openimaj.image.Image; 051import org.openimaj.image.MBFImage; 052import org.openimaj.image.colour.ColourMap; 053import org.openimaj.image.colour.ColourSpace; 054import org.openimaj.image.colour.RGBColour; 055import org.openimaj.image.renderer.MBFImageRenderer; 056import org.openimaj.image.renderer.RenderHints; 057import org.openimaj.image.typography.hershey.HersheyFont; 058import org.openimaj.video.Video; 059import org.openimaj.video.VideoDisplay; 060 061/** 062 * Kinect integration demo. Shows video and depth. Press t to toggle between rgb 063 * and ir mode. Pressing w and x moves the device up or down. Pressing s levels 064 * the device. 065 * 066 * @author Jonathon Hare (jsh2@ecs.soton.ac.uk) 067 */ 068@Demo( 069 author = "Jonathon Hare", 070 description = "Kinect integration demo. Shows video and depth. Press t " + 071 "to toggle between rgb and ir mode. Pressing w and x moves the device " + 072 "up or down. Pressing s levels the device.", 073 keywords = { "kinect", "video" }, 074 title = "Kinect Integration", 075 screenshot = "/org/openimaj/demos/screens/hardware/kinect.png", 076 icon = "/org/openimaj/demos/icons/hardware/kinect.png") 077public class KinectDemo extends Video<MBFImage> implements KeyListener { 078 MBFImage currentFrame; 079 KinectController controller; 080 JFrame frame; 081 private double tilt = 0; 082 private boolean irmode = false; 083 private final MBFImageRenderer renderer; 084 private String accel; 085 private final VideoDisplay<MBFImage> videoFrame; 086 private boolean rdepth = true; 087 private boolean printCloud = false; 088 private MBFImage v3d; 089 090 /** 091 * Default constructor 092 * 093 * @param id 094 * of kinect controller 095 * @throws KinectException 096 */ 097 public KinectDemo(int id) throws KinectException { 098 controller = new KinectController(id, irmode, rdepth); 099 currentFrame = new MBFImage(640 * 2, 480, ColourSpace.RGB); 100 renderer = currentFrame.createRenderer(RenderHints.ANTI_ALIASED); 101 102 videoFrame = VideoDisplay.createVideoDisplay(this); 103 ((JFrame) SwingUtilities.getRoot(videoFrame.getScreen())).setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE); 104 SwingUtilities.getRoot(videoFrame.getScreen()).addKeyListener(this); 105 106 v3d = new MBFImage(640, 480); 107 } 108 109 @Override 110 public MBFImage getNextFrame() { 111 MBFImage vid; 112 Image<?, ?> tmp = controller.videoStream.getNextFrame(); 113 114 if (tmp instanceof MBFImage) { 115 vid = (MBFImage) tmp; 116 } else { 117 vid = new MBFImage((FImage) tmp, (FImage) tmp, (FImage) tmp); 118 } 119 120 renderer.drawImage(vid, 0, 0); 121 122 tmp = controller.depthStream.getNextFrame(); 123 drawPointCloud((FImage) tmp, vid, 0, 0, 640, 440, 100, 80); 124 125 MBFImage depth = null; 126 if (this.rdepth) { 127 final FImage fdepth = ((FImage) tmp).clone(); 128 if (printCloud) { 129 printCloud = false; 130 try { 131 pointCloudOut(fdepth, "pointcloud.txt", 0, 0, 640, 440, 400, 320); 132 System.out.println("Point cloud written!"); 133 } catch (final FileNotFoundException e) { 134 System.err.println("failed to write pointcloud"); 135 } 136 } 137 final int pixToDraw = (int) fdepth.pixels[100][100]; 138 fdepth.normalise(); 139 depth = fdepth.toRGB(); 140 depth.drawText("Camera: " + Arrays.toString(new int[] { 100, 100, pixToDraw }), 0, 460, 141 HersheyFont.TIMES_MEDIUM, 16, RGBColour.WHITE); 142 depth.drawText("World: " + Arrays.toString(controller.cameraToWorld(100, 100, pixToDraw)), 0, 480, 143 HersheyFont.TIMES_MEDIUM, 16, RGBColour.WHITE); 144 } else { 145 depth = ColourMap.Jet.apply((FImage) tmp); 146 } 147 148 renderer.drawImage(depth, 640, 0); 149 150 if (super.currentFrame % 30 == 0) 151 accel = controller.getAcceleration() + ""; 152 renderer.drawText(accel, 0, 480, HersheyFont.TIMES_MEDIUM, 16, RGBColour.WHITE); 153 154 super.currentFrame++; 155 156 return currentFrame; 157 } 158 159 private void pointCloudOut(FImage depth, String out, int xmin, int ymin, int xmax, int ymax, float xdiv, float ydiv) 160 throws FileNotFoundException 161 { 162 final PrintWriter writer = new PrintWriter(new File(out)); 163 final float stepx = (xmax - xmin) / xdiv; 164 final float stepy = (ymax - ymin) / ydiv; 165 166 final float[] xyz = new float[3]; 167 final double factor = controller.computeScalingFactor(); 168 for (int y = ymin; y < ymax; y += stepy) { 169 for (int x = xmin; x < xmax; x += stepx) { 170 final int d = (int) depth.pixels[y][x]; 171 if (d > 0) { 172 // double[] xyz = controller.cameraToWorld(x, y, d); 173 controller.cameraToWorld(x, y, d, factor, xyz); 174 writer.printf("%4.2f %4.2f %4.2f\n", xyz[0], xyz[1], xyz[2]); 175 } 176 } 177 writer.flush(); 178 } 179 writer.close(); 180 } 181 182 private void drawPointCloud(FImage depth, MBFImage frame, int xmin, int ymin, int xmax, int ymax, float xdiv, 183 float ydiv) 184 { 185 v3d.fill(RGBColour.BLACK); 186 final List<Simple3D.Primative> points = new ArrayList<Simple3D.Primative>(); 187 188 final float stepx = 1;// (xmax - xmin) / xdiv; 189 final float stepy = 1;// (ymax - ymin) / ydiv; 190 191 float meanDepth = 0; 192 int count = 0; 193 194 final float[] xyz = new float[3]; 195 final double factor = controller.computeScalingFactor(); 196 for (int y = ymin; y < ymax; y += stepy) { 197 for (int x = xmin; x < xmax; x += stepx) { 198 final int d = (int) depth.pixels[y][x]; 199 if (d > 0) { 200 // double[] xyz = controller.cameraToWorld(x, y, d); 201 controller.cameraToWorld(x, y, d, factor, xyz); 202 203 // writer.printf("%4.2f %4.2f %4.2f\n", xyz[0], xyz[1], 204 // xyz[2]); 205 points.add(new Simple3D.Point3D(xyz[0], -xyz[1], -xyz[2], frame.getPixel(x, y), 1)); 206 meanDepth -= xyz[2]; 207 count++; 208 } 209 } 210 } 211 212 meanDepth /= count; 213 214 final double ax = Math.PI / 4; 215 final Simple3D.Scene scene = new Simple3D.Scene(points); 216 scene.translate(0, (int) (Math.tan(ax) * meanDepth), 0); 217 scene.renderOrtho(Simple3D.euler2Rot(ax, 0, 0), v3d); 218 DisplayUtilities.displayName(v3d, "3d"); 219 } 220 221 @Override 222 public MBFImage getCurrentFrame() { 223 return currentFrame; 224 } 225 226 @Override 227 public int getWidth() { 228 return currentFrame.getWidth(); 229 } 230 231 @Override 232 public int getHeight() { 233 return currentFrame.getHeight(); 234 } 235 236 @Override 237 public boolean hasNextFrame() { 238 return true; 239 } 240 241 @Override 242 public long countFrames() { 243 return -1; 244 } 245 246 @Override 247 public void reset() { 248 // do nothing 249 } 250 251 @Override 252 public void keyTyped(KeyEvent e) { 253 254 } 255 256 @Override 257 public void keyPressed(KeyEvent e) { 258 if (e.getKeyChar() == 'w') { 259 controller.setTilt(tilt += 1); 260 } else if (e.getKeyChar() == 'x') { 261 controller.setTilt(tilt -= 1); 262 } else if (e.getKeyChar() == 's') { 263 controller.setTilt(tilt = 0); 264 } else if (e.getKeyChar() == 't') { 265 controller.setIRMode(irmode = !irmode); 266 } else if (e.getKeyChar() == 'y') { 267 controller.setRegisteredDepth(rdepth = !rdepth); 268 } else if (e.getKeyChar() == 'p') { 269 printCloud = true; 270 } 271 } 272 273 @Override 274 public void keyReleased(KeyEvent e) { 275 276 } 277 278 /** 279 * Default main 280 * 281 * @param args 282 * Command-line arguments 283 */ 284 public static void main(String[] args) { 285 try { 286 new KinectDemo(0); 287 } catch (final KinectException e) { 288 JOptionPane.showMessageDialog(null, "No available Kinect device found!"); 289 } 290 } 291 292 @Override 293 public long getTimeStamp() { 294 return (long) (super.currentFrame * 1000 / getFPS()); 295 } 296 297 @Override 298 public double getFPS() { 299 return 30; 300 } 301 302 /** 303 * Get the display showing the kinect video 304 * 305 * @return The video display 306 */ 307 public VideoDisplay<MBFImage> getDisplay() { 308 return videoFrame; 309 } 310}