Refactored polynomial calculations and removed memory leak

PointCloudLibrary · Oct 1, 2019 · 790df76 · 790df76
1 parent 0deb728
commit 790df76
Showing 1 changed file with 41 additions and 76 deletions.
diff --git a/common/include/pcl/common/impl/polynomial_calculations.hpp b/common/include/pcl/common/impl/polynomial_calculations.hpp
@@ -426,14 +426,10 @@ inline bool
       std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >& samplePoints, unsigned int polynomial_degree,
       pcl::BivariatePolynomialT<real>& ret) const
 {
-  //MEASURE_FUNCTION_TIME;
-  unsigned int parameters_size = BivariatePolynomialT<real>::getNoOfParametersFromDegree (polynomial_degree);
-  //std::cout << PVARN (parameters_size);
+  const auto parameters_size = BivariatePolynomialT<real>::getNoOfParametersFromDegree (polynomial_degree);
 
-  //std::cout << "Searching for the "<<parameters_size<<" parameters for the bivariate polynom of degree "
-  //     << polynomial_degree<<" using "<<samplePoints.size ()<<" points.\n";
-
-  if (parameters_size > samplePoints.size ()) // Too many parameters for this number of equations (points)?
+  // Too many parameters for this number of equations (points)?
+  if (parameters_size > samplePoints.size ())
   {
     return false;
     // Reduce degree of polynomial
@@ -445,88 +441,58 @@ inline bool
 
   ret.setDegree (polynomial_degree);
 
-  //double coeffStuffStartTime=-get_time ();
+  // A is a symmetric matrix
   Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> A (parameters_size, parameters_size);
   A.setZero();
   Eigen::Matrix<real, Eigen::Dynamic, 1> b (parameters_size);
   b.setZero();
-  real currentX, currentY, currentZ;
-  real tmpX, tmpY;
-  real *tmpC = new real[parameters_size];
-  real* tmpCEndPtr = &tmpC[parameters_size-1];
-  for (const auto& point: samplePoints)
+
   {
-    currentX= point[0]; currentY= point[1]; currentZ= point[2];
-    //std::cout << "current point: "<<currentX<<","<<currentY<<" => "<<currentZ<<"\n";
-    //unsigned int posInC = parameters_size-1;
-    real* tmpCPtr = tmpCEndPtr;
-    tmpX = 1.0;
-    for (unsigned int xDegree=0; xDegree<=polynomial_degree; ++xDegree)
+    std::vector<real> C (parameters_size);
+    for (const auto& point: samplePoints)
     {
-      tmpY = 1.0;
-      for (unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; ++yDegree)
+      real currentX = point[0], currentY = point[1], currentZ = point[2];
+
       {
-        * (tmpCPtr--) = tmpX*tmpY;
-        //std::cout << "x="<<currentX<<", y="<<currentY<<", Pos "<<posInC--<<": "<<tmpX<<"*"<<tmpY<<"="<<tmpC[posInC]<<"\n";
-        tmpY *= currentY;
+        auto CRevPtr = C.rbegin ();
+        real tmpX = 1.0;
+        for (unsigned int xDegree=0; xDegree<=polynomial_degree; ++xDegree)
+        {
+          real tmpY = 1.0;
+          for (unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; ++yDegree)
+          {
+            *CRevPtr = tmpX*tmpY;
+            CRevPtr = std::next(CRevPtr);
+            tmpY *= currentY;
+          }
+          tmpX *= currentX;
+        }
       }
-      tmpX *= currentX;
-    }
-
-    real* APtr = &A(0,0);
-    real* bPtr = &b[0];
-    real* tmpCPtr1=tmpC;
-    for (unsigned int i=0; i<parameters_size; ++i)
-    {
-      * (bPtr++) += currentZ * *tmpCPtr1;
 
-      real* tmpCPtr2=tmpC;
-      for (unsigned int j=0; j<parameters_size; ++j)
+      for (std::size_t i=0; i<parameters_size; ++i)
       {
-        * (APtr++) += *tmpCPtr1 * * (tmpCPtr2++);
+        b[i] += currentZ * C[i];
+        // fill the upper right triangular matrix
+        for (std::size_t j=i; j<parameters_size; ++j)
+        {
+          A (i, j) += C[i] * C[j];
+        }
       }
+    //A += DMatrix<real>::outProd (C);
+    //b += currentZ * C;
+    }
+  }
 
-      ++tmpCPtr1;
+  // The Eigen only solution is slow for small matrices. Maybe file a bug
+  // A.traingularView<Eigen::StrictlyLower> = A.transpose();
+  // copy upper-right elements to lower-left
+  for (std::size_t i = 0; i < parameters_size; ++i)
+  {
+    for (std::size_t j = 0; j < i; ++j)
+    {
+        A (i, j) = A (j, i);
     }
-    //A += DMatrix<real>::outProd (tmpC);
-    //b += currentZ * tmpC;
   }
-  //std::cout << "Calculating matrix A and vector b (size "<<b.size ()<<") from "<<samplePoints.size ()<<" points took "
-       //<< (coeffStuffStartTime+get_time ())*1000<<"ms using constant memory.\n";
-    //std::cout << PVARC (A)<<PVARN (b);
-
-
-  //double coeffStuffStartTime=-get_time ();
-  //DMatrix<real> A (parameters_size, parameters_size);
-  //DVector<real> b (parameters_size);
-  //real currentX, currentY, currentZ;
-  //unsigned int posInC;
-  //real tmpX, tmpY;
-  //DVector<real> tmpC (parameters_size);
-  //for (typename std::vector<Eigen::Matrix<real, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<real, 3, 1> > >::const_iterator it=samplePoints.begin ();
-  //     it!=samplePoints.end (); ++it)
-  //{
-    //currentX= (*it)[0]; currentY= (*it)[1]; currentZ= (*it)[2];
-    ////std::cout << "x="<<currentX<<", y="<<currentY<<"\n";
-    //posInC = parameters_size-1;
-    //tmpX = 1.0;
-    //for (unsigned int xDegree=0; xDegree<=polynomial_degree; xDegree++)
-    //{
-      //tmpY = 1.0;
-      //for (unsigned int yDegree=0; yDegree<=polynomial_degree-xDegree; yDegree++)
-      //{
-        //tmpC[posInC] = tmpX*tmpY;
-        ////std::cout << "x="<<currentX<<", y="<<currentY<<", Pos "<<posInC<<": "<<tmpX<<"*"<<tmpY<<"="<<tmpC[posInC]<<"\n";
-        //tmpY *= currentY;
-        //posInC--;
-      //}
-      //tmpX *= currentX;
-    //}
-    //A += DMatrix<real>::outProd (tmpC);
-    //b += currentZ * tmpC;
-  //}
-  //std::cout << "Calculating matrix A and vector b (size "<<b.size ()<<") from "<<samplePoints.size ()<<" points took "
-       //<< (coeffStuffStartTime+get_time ())*1000<<"ms.\n";
 
   Eigen::Matrix<real, Eigen::Dynamic, 1> parameters;
   //double choleskyStartTime=-get_time ();
@@ -552,7 +518,6 @@ inline bool
 
   //Test of gradient: ret.calculateGradient ();
 
-  delete [] tmpC;
   return true;
 }