Esempio n. 1
0
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;
}
Esempio n. 2
0
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;
}