bool Neighbourhood::computeQuadric() { //invalidate previous quadric (if any) m_structuresValidity &= (~FLAG_QUADRIC); assert(m_associatedCloud); if (!m_associatedCloud) return false; unsigned count = m_associatedCloud->size(); assert(CC_LOCAL_MODEL_MIN_SIZE[QUADRIC] >= 5); if (count < CC_LOCAL_MODEL_MIN_SIZE[QUADRIC]) return false; const PointCoordinateType* lsPlane = getLSPlane(); if (!lsPlane) return false; //we get centroid (should already be up-to-date - see computeCovarianceMatrix) const CCVector3* G = getGravityCenter(); assert(G); //get the best projection axis Tuple3ub idx(0/*x*/,1/*y*/,2/*z*/); //default configuration: z is the "normal" direction, we use (x,y) as the base plane PointCoordinateType nxx = lsPlane[0]*lsPlane[0]; PointCoordinateType nyy = lsPlane[1]*lsPlane[1]; PointCoordinateType nzz = lsPlane[2]*lsPlane[2]; if (nxx > nyy) { if (nxx > nzz) { //as x is the "normal" direction, we use (y,z) as the base plane idx.x = 1/*y*/; idx.y = 2/*z*/; idx.z = 0/*x*/; } } else { if (nyy > nzz) { //as y is the "normal" direction, we use (z,x) as the base plane idx.x = 2/*z*/; idx.y = 0/*x*/; idx.z = 1/*y*/; } } //compute the A matrix and b vector std::vector<float> A; std::vector<float> b; try { A.resize(6*count); b.resize(count); } catch (std::bad_alloc) { //not enough memory return false; } float lmax2 = 0; //max (squared) dimension //for all points { float* _A = &(A[0]); float* _b = &(b[0]); for (unsigned i=0; i<count; ++i) { CCVector3 P = *m_associatedCloud->getPoint(i) - *G; float lX = static_cast<float>(P.u[idx.x]); float lY = static_cast<float>(P.u[idx.y]); float lZ = static_cast<float>(P.u[idx.z]); *_A++ = 1.0f; *_A++ = lX; *_A++ = lY; *_A = lX*lX; //by the way, we track the max 'X' squared dimension if (*_A > lmax2) lmax2 = *_A; ++_A; *_A++ = lX*lY; *_A = lY*lY; //by the way, we track the max 'Y' squared dimension if (*_A > lmax2) lmax2 = *_A; ++_A; *_b++ = lZ; lZ *= lZ; //and don't forget to track the max 'Z' squared dimension as well if (lZ > lmax2) lmax2 = lZ; } } //conjugate gradient initialization //we solve tA.A.X=tA.b ConjugateGradient<6,double> cg; CCLib::SquareMatrixd& tAA = cg.A(); double* tAb = cg.b(); //compute tA.A and tA.b { for (unsigned i=0; i<6; ++i) { //tA.A part for (unsigned j=i; j<6; ++j) { double tmp = 0; float* _Ai = &(A[i]); float* _Aj = &(A[j]); for (unsigned k=0; k<count; ++k) { //tmp += A[(6*k)+i] * A[(6*k)+j]; tmp += static_cast<double>(*_Ai) * static_cast<double>(*_Aj); _Ai += 6; _Aj += 6; } tAA.m_values[j][i] = tAA.m_values[i][j] = tmp; } //tA.b part { double tmp = 0; float* _Ai = &(A[i]); float* _b = &(b[i]); for (unsigned k=0; k<count; ++k) { //tmp += A[(6*k)+i]*b[k]; tmp += static_cast<double>(*_Ai) * static_cast<double>(*_b++); _Ai += 6; } tAb[i] = tmp; } } } //first guess for X: plane equation (a0.x+a1.y+a2.z=a3 --> z = a3/a2 - a0/a2.x - a1/a2.y) double X0[6] = {static_cast<double>(/*lsPlane[3]/lsPlane[idx.z]*/0), //DGM: warning, points have already been recentred around the gravity center! So forget about a3 static_cast<double>(-lsPlane[idx.x]/lsPlane[idx.z]), static_cast<double>(-lsPlane[idx.y]/lsPlane[idx.z]), 0, 0, 0 }; //special case: a0 = a1 = a2 = 0! //happens for perfectly flat surfaces! if (X0[1] == 0 && X0[2] == 0) X0[0] = 1.0; //init. conjugate gradient cg.initConjugateGradient(X0); //conjugate gradient iterations { double convergenceThreshold = lmax2 * 1.0e-8; //max. error for convergence = 1e-8 of largest cloud dimension (empirical!) for (unsigned i=0; i<1500; ++i) { double lastError = cg.iterConjugateGradient(X0); if (lastError < convergenceThreshold) //converged break; } } //fprintf(fp,"X%i=(%f,%f,%f,%f,%f,%f)\n",i,X0[0],X0[1],X0[2],X0[3],X0[4],X0[5]); //fprintf(fp,"lastError=%E/%E\n",lastError,convergenceThreshold); //fclose(fp); //output { for (unsigned i=0; i<6; ++i) { m_quadricEquation[i] = static_cast<PointCoordinateType>(X0[i]); } m_quadricEquationDirections = idx; m_structuresValidity |= FLAG_QUADRIC; } return true; }
bool Neighbourhood::computeHeightFunction() { //invalidate previous quadric (if any) structuresValidity &= (~HEIGHT_FUNCTION); assert(m_associatedCloud); if (!m_associatedCloud) return false; unsigned count = m_associatedCloud->size(); assert(CC_LOCAL_MODEL_MIN_SIZE[HF] >= 5); if (count < CC_LOCAL_MODEL_MIN_SIZE[HF]) return false; const PointCoordinateType* lsq = getLSQPlane(); if (!lsq) return false; //we get centroid (should already be up-to-date - see computeCovarianceMatrix) const CCVector3* G = getGravityCenter(); assert(G); //get the best projection axis uchar idx_X=0/*x*/,idx_Y=1/*y*/,idx_Z=2/*z*/; //default configuration: z is the "normal" direction, we use (x,y) as the base plane PointCoordinateType nxx = lsq[0]*lsq[0]; PointCoordinateType nyy = lsq[1]*lsq[1]; PointCoordinateType nzz = lsq[2]*lsq[2]; if (nxx > nyy) { if (nxx > nzz) { //as x is the "normal" direction, we use (y,z) as the base plane idx_X=1/*y*/; idx_Y=2/*z*/; idx_Z=0/*x*/; } } else { if (nyy > nzz) { //as y is the "normal" direction, we use (z,x) as the base plane idx_X=2/*z*/; idx_Y=0/*x*/; idx_Z=1/*y*/; } } //compute the A matrix and b vector float *A = new float[6*count]; float *b = new float[count]; float lmax2=0; //max (squared) dimension //for all points { float* _A=A; float* _b=b; for (unsigned i=0;i<count;++i) { CCVector3 P = *m_associatedCloud->getPoint(i) - *G; float lX = (float)P.u[idx_X]; float lY = (float)P.u[idx_Y]; float lZ = (float)P.u[idx_Z]; *_A++ = 1.0; *_A++ = lX; *_A++ = lY; *_A = lX*lX; //by the way, we track the max 'X' squared dimension if (*_A>lmax2) lmax2=*_A; ++_A; *_A++ = lX*lY; *_A = lY*lY; //by the way, we track the max 'Y' squared dimension if (*_A>lmax2) lmax2=*_A; ++_A; *_b++ = lZ; lZ *= lZ; //and don't forget to track the max 'Z' squared dimension as well if (lZ>lmax2) lmax2=lZ; } } //conjugate gradient initialization //we solve tA.A.X=tA.b ConjugateGradient<6,double> cg; CCLib::SquareMatrixd& tAA = cg.A(); double* tAb = cg.b(); //compute tA.A and tA.b { for (unsigned i=0; i<6; ++i) { //tA.A part for (unsigned j=i; j<6; ++j) { double tmp = 0; float* _Ai = A+i; float* _Aj = A+j; for (unsigned k=0; k<count; ++k) { //tmp += A[(6*k)+i]*A[(6*k)+j]; tmp += (double)((*_Ai) * (*_Aj)); _Ai += 6; _Aj += 6; } tAA.m_values[j][i] = tAA.m_values[i][j] = tmp; } //tA.b part { double tmp = 0; float* _Ai = A+i; float* _b = b; for (unsigned k=0; k<count; ++k) { //tmp += A[(6*k)+i]*b[k]; tmp += (double)((*_Ai) * (*_b++)); _Ai += 6; } tAb[i] = tmp; } } } //first guess for X: plane equation (a0.x+a1.y+a2.z=a3 --> z = a3/a2 - a0/a2.x - a1/a2.y) double X0[6]; X0[0] = (double)/*lsq[3]/lsq[idx_Z]*/0; //DGM: warning, points have already been recentred around the gravity center! So forget about a3 X0[1] = (double)(-lsq[idx_X]/lsq[idx_Z]); X0[2] = (double)(-lsq[idx_Y]/lsq[idx_Z]); X0[3] = 0; X0[4] = 0; X0[5] = 0; //special case: a0 = a1 = a2 = 0! //happens for perfectly flat surfaces! if (X0[1] == 0.0 && X0[2] == 0.0) X0[0] = 1.0; //init. conjugate gradient cg.initConjugateGradient(X0); //conjugate gradient iterations { double convergenceThreshold = (double)lmax2 * 1e-8; //max. error for convergence = 1e-8 of largest cloud dimension (empirical!) for (unsigned i=0; i<1500; ++i) { double lastError = cg.iterConjugateGradient(X0); if (lastError < convergenceThreshold) //converged break; } } //fprintf(fp,"X%i=(%f,%f,%f,%f,%f,%f)\n",i,X0[0],X0[1],X0[2],X0[3],X0[4],X0[5]); //fprintf(fp,"lastError=%E/%E\n",lastError,convergenceThreshold); //fclose(fp); delete[] A; A=0; delete[] b; b=0; //output { for (unsigned i=0;i<6;++i) theHeightFunction[i]=(PointCoordinateType)X0[i]; theHeightFunctionDirections[0]=idx_X; theHeightFunctionDirections[1]=idx_Y; theHeightFunctionDirections[2]=idx_Z; structuresValidity |= HEIGHT_FUNCTION; } return true; }