first normal estimation

CgMath/CgCovariance.h

+#ifndef CGCOVARIANCE_H
+#define CGCOVARIANCE_H
+
+#include <vector>
+#include "glm/glm.hpp"
+#include "Eigen/Dense"
+
+inline glm::vec3 getCentroid(const std::vector<glm::vec3> &points) {
+  glm::vec3 center(0.);
+  for(unsigned int i = 0; i < points.size(); i++) center += points[i];
+  center /= double(points.size());
+  return center;
+}
+
+inline glm::mat3 getCovarianceMatrix(const std::vector<glm::vec3> &points) {
+    unsigned int size = points.size();
+    Eigen::MatrixXd mat(size, 3);
+    glm::vec3 centroid = getCentroid(points);
+    for(unsigned int i = 0; i < size; i++) {
+        mat(i, 0) = points[i][0] - centroid[0];
+        mat(i, 1) = points[i][1] - centroid[1];
+        mat(i, 2) = points[i][2] - centroid[2];
+    }
+    Eigen::Matrix3d res = (1 / double(size)) * (mat.transpose() * mat);
+    glm::mat3 ret(
+      res(0, 0), res(0, 1), res(0, 2), 
+      res(1, 0), res(1, 1), res(1, 2), 
+      res(2, 0), res(2, 1), res(2, 2)
+    );
+    return ret;
+}
+
+#endif // CGCOVARIANCE_H
\ No newline at end of file

CgMath/CgEigenDecomposition3x3.cpp

 #include "CgEigenDecomposition3x3.h"
-#include "CgMath/Eigen/Dense"
-#include "CgMath/Eigen/Eigenvalues"
-#include "glm/glm.hpp"
-#include <iostream>
 
 using namespace Eigen;
 
-CgEigenDecomposition3x3::CgEigenDecomposition3x3(glm::mat3 arg)
-{
+CgEigenDecomposition3x3::CgEigenDecomposition3x3(glm::mat3 arg) {
   Eigen::Matrix3d A;
   A(0,0) = arg[0][0]; A(0,1) = arg[0][1]; A(0,2) = arg[0][2];
   A(1,0) = arg[1][0]; A(1,1) = arg[1][1]; A(1,2) = arg[1][2];
   A(2,0) = arg[2][0]; A(2,1) = arg[2][1]; A(2,2) = arg[2][2];
 
-  Eigen::EigenSolver<Eigen::Matrix3d> es;
-  es.compute(A,true);
-
-
-  Eigen::MatrixXcd V = es.eigenvectors();
-  Eigen::VectorXcd res= es.eigenvalues();
-  std::cout << "The eigenvectors of A are: " <<  V.transpose()<< std::endl;
-  std::cout << "The eigenvalues of A are: " << res.transpose() << std::endl;
-
-  m_eigenvectors[0][0] = V(0,0).real();
-  m_eigenvectors[0][1] = V(0,1).real();
-  m_eigenvectors[0][2] = V(0,2).real();
-  m_eigenvectors[1][0] = V(1,0).real();
-  m_eigenvectors[1][1] = V(1,1).real();
-  m_eigenvectors[1][2] = V(1,2).real();
-  m_eigenvectors[2][0] = V(2,0).real();
-  m_eigenvectors[2][1] = V(2,1).real();
-  m_eigenvectors[2][2] = V(2,2).real();
-
-  m_eigenvalues[0] = res(0).real(); m_eigenvalues[1] = res(1).real(); m_eigenvalues[2] = res(2).real();
-
+  // the covariance matrix is a symmetric real matrix and therefore 
+  // self-adjoint
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es;
+  es.compute(A);
+
+  // The eigenvalues of a selfadjoint matrix are always real.
+  Eigen::Matrix3d V = es.eigenvectors();
+  Eigen::Vector3d res = es.eigenvalues();
+
+  // eigenvectors are returned in the COLUMNS of
+  // the Matrix
+  // V(0,1) is the second element of the first ROW
+  for(unsigned int row = 0; row < 3; row++) {
+    for(unsigned int col = 0; col < 3; col++) {
+      m_eigenvectors[col][row] = V(row, col);
+    }
+  }
 
+  m_eigenvalues[0] = res(0); 
+  m_eigenvalues[1] = res(1); 
+  m_eigenvalues[2] = res(2);
 }
 
 
-glm::mat3 CgEigenDecomposition3x3::getEigenvectors()
-{
+glm::mat3 CgEigenDecomposition3x3::getEigenvectors() {
   return m_eigenvectors;
 }
 
-glm::vec3 CgEigenDecomposition3x3::getEigenvalues()
-{
-
+glm::vec3 CgEigenDecomposition3x3::getEigenvalues() {
      return m_eigenvalues;
 }

CgMath/CgEigenDecomposition3x3.h

@@ -2,22 +2,19 @@
 #define CGEIGENDECOMPOSITION3X3_H
 
 #include "glm/glm.hpp"
+#include "CgMath/Eigen/Dense"
+#include "CgMath/Eigen/Eigenvalues"
+#include "glm/glm.hpp"
+#include <iostream>
 
-
-class CgEigenDecomposition3x3
-{
+class CgEigenDecomposition3x3 {
 public:
   CgEigenDecomposition3x3(glm::mat3 arg);
-
   glm::mat3 getEigenvectors();
   glm::vec3 getEigenvalues();
-
-
 private:
-
     glm::mat3 m_eigenvectors;
     glm::vec3 m_eigenvalues;
-
 };
 
 #endif // CGEIGENDECOMPOSITION3X3_H

CgSceneGraph/CgPointCloud.cpp

 #include "CgPointCloud.h"
-
+#include "CgUtils/Timer.h"
 
 // squared distance between a point and a ray
 float sqrPointRayDist(glm::vec3 point, glm::vec3 origin, glm::vec3 direction) {
@@ -61,22 +61,20 @@ void CgPointCloud::init( std::string filename, bool cheat_normals) {
     ObjLoader loader;
     loader.load(filename);
     loader.getPositionData(m_vertices);
-    std::cout << "got " << m_vertices.size() << " points" << std::endl;
+    m_centroid = getCentroid(m_vertices);
 
     buildKdTree(&m_vertices.front(), &(*(m_vertices.end() - 1)), 0);
     calculateAABB();
+    // run_timed(1, [&]() {estimateNormals();});
     estimateNormals();
-
-    // do this for cheating with the normals
-    // you need to replace this by a normal estimation algorithm
-    if(cheat_normals) loader.getNormalData(m_vertex_normals);
+    std::cout << "got " << m_vertices.size() << " points" << std::endl;
 
     // calculate local coordinate system for splats 
     // (arbitrary orientation of ellipse in plane)
     // replace this if you have the real coordinate 
     // system, use up vector = y-Axis of your local 
     // coordinate system instead of getPerpendicularVector(...)
-    calculateSplatOrientations();
+    //calculateSplatOrientations();
 
     //add a standard color for each point if lighting turned off
     for(unsigned int i = 0;i < m_vertices.size(); i++) {
@@ -87,7 +85,7 @@ void CgPointCloud::init( std::string filename, bool cheat_normals) {
     // "fast" if k is relatively small, if k is on the order 
     // of m_vertices.size(), this is slower than brute force.
     std::vector<unsigned int> neighbors = getNearestNeighborsFast(0, k);
-    setColors(neighbors, glm::vec3(0.0, 0.0, .75));
+    setColors(neighbors, glm::vec3(0.0, 0.0, 1.0));
 }
 
 void CgPointCloud::buildKdTree(glm::vec3* begin, glm::vec3* end, int depth) {
@@ -113,26 +111,45 @@ void CgPointCloud::buildKdTree(glm::vec3* begin, glm::vec3* end, int depth) {
 }
 
 void CgPointCloud::estimateNormals() {
+  unsigned int neighbourhood_size = 25;
+
   // find point with highest y-value
-  std::vector<unsigned int> spanning_tree(m_vertices.size());
-  float max_y =-std::numeric_limits<float>::infinity();
-  unsigned int max_y_index = 0;
-  for(int i = 0; i < m_vertices.size(); i++) {
-    float curr = m_vertices[i].y;
-    if(max_y < curr) {
-      max_y = curr;
-      max_y_index = i;
-    }
-  }
+  //std::vector<unsigned int> spanning_tree;
+  //spanning_tree.resize(m_vertices.size());
+  m_vertex_normals.clear();
+  m_vertex_normals.resize(m_vertices.size());
+  //float max_y =-std::numeric_limits<float>::infinity();
+  //unsigned int max_y_index = 0;
+  //for(int i = 0; i < m_vertices.size(); i++) {
+  //  float curr = m_vertices[i].y;
+  //  if(max_y < curr) {
+  //    max_y = curr;
+  //    max_y_index = i;
+  //  }
+  //}
+
   // the extremal point is selected as root (has no parent)
-  spanning_tree[max_y_index] = max_y_index;
+  //spanning_tree[max_y_index] = max_y_index;
 
   // build minimal spanning tree (for each index, record the index we came from)
   // traverse the tree, search neighborhoods
-  // estimate normal:
-    // calc centroid
-    // create covariance matrix
-    // select shortest eigenvector
+  for(unsigned int i = 0; i < m_vertices.size(); i++) {
+      //std::cout << "alive, now " << i << std::endl;
+      std::vector<unsigned int> nh_indices = getNearestNeighborsFast(i, neighbourhood_size);
+      std::vector<glm::vec3> nh;
+      nh.resize(neighbourhood_size);
+      for(unsigned int j = 0; j < neighbourhood_size; j++) {
+        nh[j] = m_vertices[nh_indices[j]];
+      }
+      glm::mat3 cov_mat = getCovarianceMatrix(nh);
+      CgEigenDecomposition3x3 eigen(cov_mat);
+      // The eigenvalues/vectors are sorted in increasing order, and we want the smallest one.
+      glm::vec3 normal = eigen.getEigenvectors()[0];
+      m_vertex_normals[i] = glm::dot(normal, m_vertices[i] - m_centroid) > 0.0 
+        ? normal
+        : -normal;
+  }
+
   // select normal pointing roughly in the same direction as parent's normal
 }
 
@@ -185,28 +202,21 @@ std::vector<unsigned int> CgPointCloud::getNearestNeighborsSlow(unsigned int cur
 
 std::vector<unsigned int> CgPointCloud::getNearestNeighborsFast(unsigned int current_point, unsigned int k) {
 
-  std::vector<unsigned int> erg;
-  if(k == 0 || m_vertices.size() == 0) return erg;
+  std::vector<unsigned int> erg(k);
+  if(k == 0 || m_vertices.size() < k) return erg;
   // pair of distance + index into backing vec3 array 
   using P = std::pair<double, unsigned int>;
 
   glm::vec3 query = m_vertices[current_point];
   glm::vec3* first = &m_vertices.front();
   glm::vec3* last = &(*(m_vertices.end() - 1));
-  auto cmp = [&](P left, P right) {
-    // std::set sees elements as equivalent if neither 
-    // compares less than the other
-    // -> needs to take index into consideration since 
-    // different points can have same distance.
-    return left.first != right.first
-      ? left.first < right.first
-      : left.second < right.second;
-  };
+  auto cmp = [&](const P &left, const P &right) { return (left.first < right.first);};
 
   // using an ordered set for simplicitly, this may be slow
   // because we frequently erase elements, causing the rb-tree
   // to be rebuilt.
-  std::set<P, decltype(cmp)> set(cmp);
+  std::vector<P> candidates;
+  candidates.reserve(k + 1);
 
   // stack of traversed nodes (start index, end index, aabb, depth)
   std::vector<std::tuple<glm::vec3*, glm::vec3*, CgAABB, unsigned int>> nodes;
@@ -225,9 +235,12 @@ std::vector<unsigned int> CgPointCloud::getNearestNeighborsFast(unsigned int cur
 
     glm::vec3 curr_point = *(begin + half_size);
     // insert current point into set
-    set.insert({glm::distance2(query, curr_point), begin + half_size - first});
-    // make sure our set only contains k elements at most
-    while(set.size() > k) set.erase(std::prev(set.end()));
+    candidates.push_back({glm::distance2(query, curr_point), begin + half_size - first});
+    std::push_heap(candidates.begin(), candidates.end(), cmp);
+    if(candidates.size() > k) {
+      std::pop_heap(candidates.begin(), candidates.end(), cmp);
+      candidates.pop_back();
+    }
 
     // the aabb of the sub-nodes
     unsigned int axis = depth % 3;
@@ -236,10 +249,10 @@ std::vector<unsigned int> CgPointCloud::getNearestNeighborsFast(unsigned int cur
     std::tie(lower_aabb, higher_aabb) = aabb.split(axis, curr_point[axis]);
     // only push lower child if its aabb intersects with the 
     // neighbourhood we found 
-    double neighbourhood_radius = (*std::prev(set.end())).first;
+    double neighbourhood_radius = (*candidates.begin()).first;
     if(
       begin != begin + half_size
-      && lower_aabb.position[axis] + lower_aabb.extent[axis] > query[axis] - neighbourhood_radius
+      && lower_aabb.position[axis] + lower_aabb.extent[axis] >= query[axis] - neighbourhood_radius
     ) {
       nodes.push_back({begin, begin + half_size, lower_aabb, depth + 1});
     }
@@ -247,7 +260,7 @@ std::vector<unsigned int> CgPointCloud::getNearestNeighborsFast(unsigned int cur
     // the neighbourhood we found.
     if(
       begin + half_size != end
-      && higher_aabb.position[axis] - higher_aabb.extent[axis] < query[axis] + neighbourhood_radius
+      && higher_aabb.position[axis] - higher_aabb.extent[axis] <= query[axis] + neighbourhood_radius
     ) {
       nodes.push_back({begin + half_size + 1, end, higher_aabb, depth + 1});
     }
@@ -255,9 +268,7 @@ std::vector<unsigned int> CgPointCloud::getNearestNeighborsFast(unsigned int cur
 
   // copy all nearest neighbour indices we found into
   // a vector to return
-  std::for_each(set.cbegin(), set.cend(), [&](P entry) {
-    erg.push_back(entry.second);
-  });
+  for(unsigned int i = 0; i < k; i++) erg[i] = candidates[i].second;
 
   return erg;
 }
@@ -338,12 +349,7 @@ glm::vec3 CgPointCloud::getPerpendicularVector(glm::vec3 arg) {
 }
 
 const glm::vec3 CgPointCloud::getCenter() const {
-  glm::vec3 center(0.);
-  for(unsigned int i=0;i<m_vertices.size();i++) {
-      center+=m_vertices[i];
-  }
-  center/=(double)m_vertices.size();
-  return center;
+  return m_centroid;
 }
 
 void CgPointCloud::calculateAABB() {

CgSceneGraph/CgPointCloud.h

@@ -16,6 +16,8 @@
 #include "CgSceneGraph/CgAABB.h"
 #include "CgBase/CgEnums.h"
 #include "CgUtils/ObjLoader.h"
+#include "CgMath/CgCovariance.h"
+#include "CgMath/CgEigenDecomposition3x3.h"
 #include <glm/glm.hpp>
 #include <glm/gtx/norm.hpp>
 #include <glm/gtc/matrix_transform.hpp>
@@ -92,6 +94,7 @@ private:
     std::vector<glm::vec3> m_vertices;
     std::vector<glm::vec3> m_vertex_normals;
     std::vector<glm::vec3> m_vertex_colors;
+    glm::vec3 m_centroid;
     CgAABB m_aabb;
 
     // indices of vertices for which a splat will be rendered

CgSceneGraph/CgSceneControl.cpp

@@ -121,15 +121,30 @@ void CgSceneControl::calculatePickRay(float x, float y) {
 
 
 void CgSceneControl::calculateEigenDecomposition3x3() {
-    glm::mat3 testmat = glm::mat3(3.,2.,4.,2.,0.,2.,4.,2.,3.);
-    std::cout << glm::to_string(testmat) << std::endl;
+
+    std::vector<glm::vec3> testvecs{
+      glm::vec3(1.0, 1.0, 1.0),
+      glm::vec3(1.0, 1.0, -1.0),
+      glm::vec3(1.0, -1.0, 1.0),
+      glm::vec3(-1.0, 4.3, 1.0),
+      glm::vec3(1.0, -2.15, -1.0),
+      glm::vec3(-.45, 1.0, -1.0),
+      glm::vec3(-1.0, -1.0, 1.0),
+      glm::vec3(-1.0, -2.0, -1.0),
+    };
+
+    glm::mat3 testmat = getCovarianceMatrix(testvecs);
+
+    std::cout << "CovMat is\n" << glm::to_string(testmat) << std::endl;
 
     CgEigenDecomposition3x3 eigen(testmat);
     glm::mat3 eigenvectors= eigen.getEigenvectors();
     glm::vec3 eigenvalues = eigen.getEigenvalues();
 
-    std::cout << glm::to_string(eigenvalues) << std::endl;
-    std::cout << glm::to_string(eigenvectors) << std::endl;
+    std::cout << "evec0: " << glm::to_string(eigenvectors[0]) << std::endl;
+    std::cout << "evec1: " << glm::to_string(eigenvectors[1]) << std::endl;
+    std::cout << "evec2: " << glm::to_string(eigenvectors[2]) << std::endl;
+    std::cout << "eval:" << glm::to_string(eigenvalues) << std::endl;
 }
 
 void CgSceneControl::calculateSingularValueDecomposition() {

ExerciseVC.pro

 QT       += core gui opengl widgets
 TEMPLATE = app
 TARGET = PointViewer
-QMAKE_CXXFLAGS += -g -std=c++11
-CONFIG += c++11
+QMAKE_CXXFLAGS += -g -std=c++20
+CONFIG += c++20
 
 SOURCES += main.cpp \
     CgEvents/CgLoadMeshEvent.cpp \
@@ -37,6 +37,7 @@ HEADERS += \
     CgEvents/CgKdTreeEvent.h \
     CgEvents/CgSplatEvent.h \
     CgMath/CgEigenDecomposition3x3.h \
+    CgMath/CgCovariance.h \
     CgMath/Eigen/Core \
     CgMath/Eigen/Eigen \
     CgMath/Eigen/SVD \