Java floor floorPlaneToPlanView(int[][] frameCells, final int h)

Description

Project floorplane from depth view into plan view

License

Open Source License

Parameter

Parameter	Description
frameCells	two dimensional array with floorplane highlighted by 5th bit

Return

two dimensional byte array 00=unknown, 01=floor plane, 11=not floor plane

Declaration

public static byte[][] floorPlaneToPlanView(int[][] frameCells, final int h) 

Method Source Code

//package com.java2s;
//License from project: Open Source License 

public class Main {
    final static int camFOVx = 58;
    public final static int maxDepthFPTV = 3500;

    /**/*from  ww  w  .jav a2  s  .  c  om*/
     * Project floorplane from depth view into plan view
     * @param frameCells two dimensional array with floorplane highlighted by 5th bit
     * @return two dimensional byte array 00=unknown, 01=floor plane, 11=not floor plane
        
     */
    public static byte[][] floorPlaneToPlanView(int[][] frameCells, final int h) {
        //      final int yres =(int) ((double) maxDepth /h);
        //      final int xres = yres;
        //      final int h = (int) ((double)maxDepth /yres);
        final int w = (int) (Math.sin((camFOVx / 2) * Math.PI / 180) * h) * 2;
        //      System.out.println("width: "+w+", height: "+h);
        final int cwidth = frameCells.length;
        final int cheight = frameCells[0].length;
        final double angle = (double) camFOVx / 2 * Math.PI / 180; // 0.392699082; // 29 deg in radians from ctr, or half included view angle

        byte[][] result = new byte[w][h];

        final int xdctr = cwidth / 2;

        for (int y = 0; y < cheight; y++) {
            for (int x = 0; x < cwidth; x++) {
                int d = frameCells[x][y]; // depth

                byte b = 0;
                if (y > cheight / 2 - 3 && y < cheight / 2 + 3)
                    b = 0b11; // horiz
                else if ((d & 0xf0000) >> 16 == 1) { // 17th bit set at 1, is floor plane
                    d = d & 0xffff;
                    b = 0b01;
                }
                //            int y = (int) ((float)xdepth[xd]/(float)maxDepthInMM*(float)h);
                int ry = (int) ((double) d / (double) maxDepthFPTV * (double) h);
                double xdratio = (double) (x - xdctr) / (double) xdctr;
                int rx = (w / 2) - ((int) (Math.tan(angle) * (double) ry * xdratio));

                //               System.out.println("x: "+x+", y: "+y+", d: "+d+", rx:"+rx+", ry: "+ry);

                if (ry < h && ry >= 0 && rx >= 0 && rx < w) {
                    result[rx][h - ry - 1] = b;
                }

            }
        }

        //      result[3][3]=true;
        return result;
    }
}

Related

1. floorMod(long x, long y)
2. floorMod(long x, long y)
3. floorModExt(int dividend, int divisor)
4. floorModulo(int value, int divisor)
5. floorPlaneAndHorizToPlanView(final int[][] frameCells, final short frame[], final int h)
6. floorPositive(float value)
7. floorPot(float value)
8. floorPOT(int n)
9. floorPowerOf2(final int n)