AprilTagReceive.java

import java.io.*;
import java.net.*;

public  class AprilTagReceive
{
    public static void main(String args[]) throws IOException
    {
        DatagramSocket sock = new DatagramSocket(7709);

        byte buf[] = new byte[65536];
        DatagramPacket packet = new DatagramPacket(buf, buf.length);

        while (true) {
            sock.receive(packet);

            DataInputStream ins = new DataInputStream(new ByteArrayInputStream(buf));
            int MAGIC1 = ins.readInt();
            int MAGIC2 = ins.readInt();
            assert(MAGIC1 == 0x41505249);
            assert(MAGIC2 == 0x4c544147);

            int version = ins.readInt();
            assert(version == 0x00010001);

            int ndets = ins.readInt();
            long utime = (((long)ins.readInt())<<32) + ins.readInt();

            System.out.printf("%13.6f: %d tags\n", utime/1000000.0, ndets);
            for (int i = 0; i < ndets; i++) {
                int id = ins.readInt();
                int hamming = ins.readInt();
                float goodness = ins.readFloat();

                double c[] = new double[2];
                for (int j = 0; j < 2; j++)
                    c[j] = ins.readFloat();

                double p[][] = new double[4][2];
                for (int j = 0; j < 4; j++) {
                    p[j][0] = ins.readFloat();
                    p[j][1] = ins.readFloat();
                }

                double H[] = new double[9];
                for (int j = 0; j < 9; j++)
                    H[j] = ins.readFloat();

                System.out.printf(" id=%3d, err=%d at (%8.2f,%8.2f)\n",
                                  id, hamming, c[0], c[1]);
            }
        }
    }
}