I am working on a project, trying to register point clouds extracted from consecutive video frames (rgb & depth images). The goal of this project is to align the point clouds of several frames, remove duplicate points and finally reconstruct the scene. The technology I decided to use is `opengl`

which may or may not be ideal for this type of problem but is the one I am the most familiar with.

The dataset I am using consists of 2 `640 x 480`

images per frame, a 3 channel rgb image and a grayscale depth image. So far, I have successfully converted these to 3D point clouds. The next step is to try and register 2 point clouds taken from consecutive video frames, by implementing the iterative closest point algorithm (ICP) and using singular value decomposition to obtain the rigid transformation that aligns the point clouds optimally for each step of the algorithm.

In order to achieve a viable nearest neighbor search time I must use an appropriate data structure. My 2 options were `kd-tree`

and `octree`

and I decided on the octree since kd-tree requires sorting to find the median and the amount of points in each cloud is large `640 x 480`

for a total of `307200`

points.

The basic structure of the `octree`

is as follows:

`class octnode { public: octnode(float xn, float xm, float yn, float ym, float zn, float zm, int l, octnode *par); ~octnode(); float xmin; float xmax; float ymin; float ymax; float zmin; float zmax; int level; bool isLeaf; octnode *parent; octnode* expand(glm::vec3 p); bool contains(glm::vec3 p); void addItem(int index); void setChildren(octnode *a = NULL, octnode *b = NULL, octnode *c = NULL, octnode *d = NULL, octnode *e = NULL, octnode *f = NULL, octnode *g = NULL, octnode *h = NULL); octnode** getChildren(); glm::vec3 getCentroid(); private: octnode **children; glm::vec3 centroid; std::vector<int> items; }; class octree { public: octree(float minX, float maxX, float minY, float maxY, float minZ, float maxZ, int partitions); ~octree(); void construct(octnode *root, int partitions); void addItem(glm::vec3 p, int index); std::vector<int> getSearchCandidates(glm::vec3 p); void destroy(octnode *n); void propagateItems(octnode *n, GLfloat *arr); std::vector<octnode *> leaves; private: octnode * root; int depth; }; `

The important methods that are used to create the tree are `construct`

and `propagateItems`

`void octree::construct(octnode *root, int partitions){ if (partitions == 0) { leaves.push_back(root); root->setChildren(); return; } float xmin, xmax, ymin, ymax, zmin, zmax; /* * ROOT: * E________________F * / /| * / / | * A/_____________B_/ | * | | | | * | | | | * G|_______|______| |H * | | | / * | | | / * C|_______|_____D|/ * * +x -> right * +y -> top * +z -> towards the screen */ //A xmin = root->xmin; xmax = (root->xmax + root->xmin) / 2; ymin = (root->ymax + root->ymin) / 2; ymax = root->ymax; zmin = root->zmin; zmax = (root->zmin + root->zmax) / 2; octnode* chA = new octnode(xmin, xmax, ymin, ymax, zmin, zmax, root->level + 1, root); construct(chA, partitions - 1); //B xmin = (root->xmin + root->xmax)/2; xmax = root->xmax; ymin = (root->ymax + root->ymin) / 2; ymax = root->ymax; zmin = root->zmin; zmax = (root->zmin + root->zmax) / 2; octnode* chB = new octnode(xmin, xmax, ymin, ymax, zmin, zmax, root->level + 1, root); construct(chB, partitions - 1); ... root->setChildren(chA, chB, chC, chD, chE, chF, chG, chH); } void octree::propagateItems(octnode * n, GLfloat *arr){ octnode** children = n->getChildren(); if (children[0] == NULL) { cout << "propagation attempted on null children." << endl; } //propagating each item to one of 8 children of this node for (int i = 0; i < n->getItems().size(); i++) { //acquiring index from the array int index = n->getItems().at(i); //creating the appropriate vec3 object glm::vec3 p(arr[3*index], arr[3*index + 1], arr[3*index + 2]); //checking which child it matches for (int k = 0; k < 8; k++) { if (children[k]->contains(p)) { children[k]->addItem(index); break; } } } //after propagating the items delete the "items" vector //from the current node. n->deleteItems(); } `

The idea is that once I obtain the bounding box of the point cloud, I then split it into 8 smaller equal boxes which are then split again until the proper number of partitions is reached. This would create a balanced octree. The “dense” aspect of the point cloud mentioned in the title comes from the fact that there are certain areas where the points are very close to each other. In these areas, when I created the octree, most of the nodes would have `~0 - 200`

points (items) each and one node woud have `95000`

points, making the nearest neighbor search impractical.

To solve that problem I decided to do more partitions in the nodes that had more items than a specified threshold:

`int partitions = 5; int threshold = 1000; //-> Creating only a single layer (8 leaf nodes) tree = new octree(minX-0.001, maxX+0.001, minY-0.001, maxY+0.001, minZ-0.001, maxZ+0.001, 1); //-> Adding the items for (int i = 0; i < height; i++) { for (int j = 0; j < width; j++) { int index = i * width + j; glm::vec3 p(vertexData[3 * index], vertexData[3 * index + 1], vertexData[3 * index + 2]); tree->addItem(p, index); } } //-> Keep expanding the leaf nodes till the number of //items in each leaf node drops below a threshold bool done = false; std::vector<octnode *> leavesCopy = tree->leaves; while (!done) { done = true; int sz = leavesCopy.size(); for (int i = 0; i < sz; i++) { octnode *currentLeaf = leavesCopy.at(i); //for each node whose items exceed the threshold if (currentLeaf->getItems().size() > threshold) { //create another layer below it tree->construct(currentLeaf, 1); //propagate its items to the new children tree->propagateItems(currentLeaf, vertexData); //if even a single leaf has been expanded, repeat the while //loop again until no changes are to be made. done = false; //adding the newly created children to the array std::vector<octnode *> ns = currentLeaf->getChildrenAsVector(); leavesCopy.insert( leavesCopy.end(), std::make_move_iterator(ns.begin()), std::make_move_iterator(ns.end()) ); } else { leavesCopy.push_back(currentLeaf); } } //reconstructing the leaves array leavesCopy.erase(leavesCopy.begin(), leavesCopy.begin() + sz); } `

Basically, I create an initial layer of 8 children and add the points to their corresponding child. After that every child is checked, and if it is found to have more items than the threshold allows, another layer is created and the items are propagated to that layer.

The problem is that this process takes approximately 6 minutes to finish for 1 point cloud, and I need 12 minutes to load 2 point clouds and try to align them. The aligning has several bugs in it which are really hard to find due to the large number of points available. I’ve already written more code than I can handle and the amount of time it takes to try to debug the application (>12 minutes) has become overwhelming.

Is there any way I could optimize the code to run faster? Am I even taking the right approach? I omitted some parts of the code, but I will include them if asked. Please forgive the long post and my inexperience on the subject.