updated object detector

package object_detection;

import com.opencsv.CSVParser;

import com.opencsv.CSVParserBuilder;

import com.opencsv.CSVReader;

import com.opencsv.CSVReaderBuilder;

import com.opencsv.exceptions.CsvValidationException;

import object_detection.types.Point;

import java.io.FileReader;

import java.io.IOException;

import java.util.ArrayList;

import java.util.List;

public class Downsampler {

    /**

     * This function uses a voxel downsampling algorithm to take a dense pointcloud, and convert it to a voxel map of the world

     * @param pathname : path to the CSV file holding the xyz information

     * @param voxel_size : size of voxels (larger means less dense voxel map)

     * @return : an arraylist of points, this is the downsampled total

     * @throws IOException

     * @throws CsvValidationException

     */

    public static ArrayList<Point> get_voxels(String pathname, float voxel_size) throws IOException, CsvValidationException {

        // build CSVReader

        CSVParser csvParser = new CSVParserBuilder()

                .withSeparator(',')

                .withIgnoreQuotations(true)

                .build();

        CSVReader csvReader = new CSVReaderBuilder(new FileReader(pathname))

                .withCSVParser(csvParser)

                .build();

        // read CSV into float[][], just to build initial pointcloud

        // also, keep track of min/max of pointcloud

        List<double[]> allPoints = new ArrayList<>();

        double xmin = 999999; double xmax = -999999;

        double ymin = 999999; double ymax = -999999;

        double zmin = 999999; double zmax = -999999;

        String[] nextLine;

        while ((nextLine = csvReader.readNext()) != null) {

            double[] point = new double[6];

            point[0] = Double.parseDouble(nextLine[0]);

            point[1] = Double.parseDouble(nextLine[1]);

            point[2] = Double.parseDouble(nextLine[2]);

            point[3] = Double.parseDouble(nextLine[3]);

            point[4] = Double.parseDouble(nextLine[4]);

            point[5] = Double.parseDouble(nextLine[5]);

            allPoints.add(point);

            xmin = Math.min(xmin, point[0]);

            xmax = Math.max(xmax, point[0]);

            ymin = Math.min(ymin, point[1]);

            ymax = Math.max(ymax, point[1]);

            zmin = Math.min(zmin, point[2]);

            zmax = Math.max(zmax, point[2]);

        }

        System.out.println(" > ----------------------------");

        System.out.println("Starting with: " + allPoints.size() + " points");

        // create voxel matrix

        int num_vox_x = (int) Math.ceil(Math.abs(xmax - xmin) / voxel_size);

        int num_vox_y = (int) Math.ceil(Math.abs(ymax - ymin) / voxel_size);

        int num_vox_z = (int) Math.ceil(Math.abs(zmax - zmin) / voxel_size);

        // voxel = bucket, given 3 coordinates

        double[][][][] voxels = new double[num_vox_x][num_vox_y][num_vox_z][6];

        int[][][] count = new int[num_vox_x][num_vox_y][num_vox_z];

        // create shifts in points

        // put each point into a bucket (by summing)

        for(double[] point : allPoints){

            int x_floor = (int) Math.round(Math.floor((point[0] - xmin)/voxel_size));

            int y_floor = (int) Math.round(Math.floor((point[1] - ymin)/voxel_size));

            int z_floor = (int) Math.round(Math.floor((point[2] - zmin)/voxel_size));

            // increment count

            count[x_floor][y_floor][z_floor] += 1;

            // add point to sum

            voxels[x_floor][y_floor][z_floor][0] += point[0];

            voxels[x_floor][y_floor][z_floor][1] += point[1];

            voxels[x_floor][y_floor][z_floor][2] += point[2];

            voxels[x_floor][y_floor][z_floor][3] += point[3];

            voxels[x_floor][y_floor][z_floor][4] += point[4];

            voxels[x_floor][y_floor][z_floor][5] += point[5];

        }

        ArrayList<Point> res = new ArrayList<>();

        // average out voxels to get final pointcloud, and append to result if necessary

        for(int i = 0; i < num_vox_x; i++){

            for(int j = 0; j < num_vox_y; j++) {

                for (int k = 0; k < num_vox_z; k++) {

                    if (count[i][j][k] > 0) {

                        double x_avg = voxels[i][j][k][0] / count[i][j][k];

                        double y_avg = voxels[i][j][k][1] / count[i][j][k];

                        double z_avg = voxels[i][j][k][2] / count[i][j][k];

                        double r_avg = voxels[i][j][k][3] / count[i][j][k];

                        double g_avg = voxels[i][j][k][4] / count[i][j][k];

                        double b_avg = voxels[i][j][k][5] / count[i][j][k];

                        res.add(new Point((float) x_avg, (float) y_avg, (float) z_avg, (int) r_avg, (int) g_avg, (int) b_avg));

                    }

                }

            }

        }

        System.out.println("Downsampled to: " + res.size() + " points");

        System.out.println(" > ----------------------------");

        return res;

    }

    public static void main(String[] args) throws CsvValidationException, IOException {

        List<Point> result = Downsampler.get_voxels("src/main/java/vslam/pointcloud.csv", 0.05F);

    }

}

package object_detection;

import com.opencsv.exceptions.CsvValidationException;

import database.MongoDBInteraction;

import object_detection.types.ObjectSet;

import object_detection.types.Point;

import object_detection.types.*;

import java.io.File;

import java.io.FileNotFoundException;

import java.io.IOException;

import java.util.ArrayList;

import java.util.Arrays;

import java.util.List;

    /**

     * The starting function that creates an object set, compiles informatino, and returns it

     * TODO: need to add functionality to update Database

     * @throws FileNotFoundException

     */

    public static void startProcess() throws FileNotFoundException {

    public static void startProcess() throws IOException, CsvValidationException {

        // for now, we can just set paths to the directories that hold keyframes and featurepoint CSVs

        String feat_dir_pth = "src/main/java/vslam/KeyFramePoints";

        String keyframe_png_path = "src/main/java/vslam/KeyFrames";

        String bbox_dir_pth = "src/main/java/vslam/BoundedInfo";

        String pose_dir_path = "src/main/java/vslam/CameraPoses";

        // get files

        File[] feat_CSVs = getDirFiles(feat_dir_pth);

        File[] bbox_CSVs = getDirFiles(bbox_dir_pth);

        File[] pose_CSVs = getDirFiles(pose_dir_path);

        // sort to guarantee correct order for keyframes

        Arrays.sort(feat_CSVs);

        Arrays.sort(bbox_CSVs);

        // check to make sure both file arrays have same number of keyframes (for consistency)

        if(feat_CSVs.length != bbox_CSVs.length){

            System.err.println("ERROR: features and bounding box directories have differing number of Keyframes");

        }

        Arrays.sort(pose_CSVs);

        /* #################################################

        In the section below, we create a new ObjectSet, and iterate over each Keyframe

         ################################################## */

        System.out.println("====> Starting Object Mapping");

        // initialize ObjectSet

        ObjectSet objSet = new ObjectSet();

        // for each keyframe, process bounding box csv and feature csv

        int NUMKF = feat_CSVs.length;

        for(int i = 0; i < NUMKF; i++){

            // process the frame

            List<ArrayList<Point>> currFramePoints = objSet.processFrame(feat_CSVs[i], bbox_CSVs[i]);

            // update the current object set

            objSet.updateObjectSet(currFramePoints);

        CameraIntrinsics intrinsics = new CameraIntrinsics("src/main/java/vslam/CameraIntrinsics.csv");

        List<Point> pointCloud = Downsampler.get_voxels("src/main/java/vslam/pointcloud.csv", 0.05F);

        ObjectSet os = new ObjectSet(intrinsics, pointCloud);

        // iterate through each frame, create the frame, then process it

        for(int i = 0; i < pose_CSVs.length; i++){

            CameraPose cp = new CameraPose(pose_CSVs[i].getPath());

            Frame f = new Frame(bbox_CSVs[i].getPath(), cp);

            os.processFrame(f);

            System.out.println("Processed frame " + i);

            if(i == 5){

                break;

            }

        }

        // printing final object set for sanity

        System.out.println(objSet);

        System.out.println("====> Finished Object Mapping");

        Point[] pp = new Point[pointCloud.size()];

        int i = 0;

        for(Point p : pointCloud){

            pp[i] = p;

            i++;

        }

        os.objects.add(new PointSet(0, pp));

        //MongoDBInteraction mdbi = new MongoDBInteraction();

        //mdbi.updateObjectSet(5, objSet);

        // update MongoDB

        MongoDBInteraction mdbi = new MongoDBInteraction();

        mdbi.updateObjectSet(7, os);

    }

    /**

        return f_arr;

    }

    public static void main(String[] args) throws FileNotFoundException {

    public static void main(String[] args) throws IOException, CsvValidationException {

        startProcess();

    }

Point: single feature

- modify x,y,z coordinates

- pointer to rep(s)

PointSet: a single set of points that constitutes an object

- made of a set of points, and some k number of object reps

- object reps are chosen heuristically

  - current ideas are to get middle most points, or corner most points as rep

- also update reps when adding points to a PointSet

ObjectSet: group of objects <--> to/from database

- makeObject > instantiate HashSet of points

- find-set > return list of k reps

- union > combine two sets

- union-find > find and union together

ObjectMart: disjoint set operations, that gets coordinates from source

- just a main function?

- getObject > get new set of coordinates, make-set on them, try union-find

Input to System:

- pointcloud

- keyframes:

  - camera angles (x,y,z + angle ?)

  - 2D points captured

  - YOLO-captured bounding boxes

1) Take pointcloud, and downsample

2) For each frame:

  a. project downsampled pointcloud onto frame

  b. overlay 2D bounding boxes from YOLO

  c. create candidate objects based on points falling within boxes

  d. do overlap combinations based on thresholding voxels

3) Given final objectset, transmit corners to GUI

4) Display corners over original downsampled pointcloud to show objects

Output of System:

- pointcloud

- 3D bounding box of objects

Projecting 3D points onto 2D screen based on camera pose:

1) Calculate camera matrix = K * [R, t'] where R is rotation matrix, t is translation vector, and K is intrinsic of camera pose

2) Get projection by appling projPoints = [point, 1] * cameraMatrix' 

3) Divide projPoints[1:2] by projPoints[3] (i.e. divide x and y coordinates by z)

4) Return projPoints if z > 0 (infront of camera), or x,y fall into size of image (0-ImageSize.x, 0-ImageSize.y)

 No newline at end of file

package object_detection.types;

public class BoundingBox {

    public BoundingBox(Point2D tr, Point2D tl, Point2D br, Point2D bl, int idx, String predClass) {

        this.topRight = tr;

        this.topLeft = tl;

        this.botRight = br;

        this.botLeft = bl;

        this.idx = idx;

        this.predClass = predClass;

    }

    public boolean within(Point2D p){

        return p.getX() <= this.topRight.getX()

                && p.getX() >= this.topLeft.getX()

                && p.getY() >= this.topLeft.getY()

                && p.getY() <= this.botLeft.getY();

    }

    // members

    Point2D topRight;

    Point2D topLeft;

    Point2D botRight;

    Point2D botLeft;

    int idx;

    String predClass;

    @Override

    public String toString(){

        return "BBOX with corners:" +

                "\n TR: " + topRight +

                "\n TL: " + topLeft +

                "\n BR: " + botRight +

                "\n BL: " + botLeft + "\n";

    }

}