[CC]平面拟合

　　常见的平面拟合方法一般是最小二乘法。当误差服从正态分布时，最小二乘方法的拟合效果还是很好的，可以转化成PCA问题。

　　当观测值的误差大于2倍中误差时，认为误差较大。采用最小二乘拟合时精度降低，不够稳健。

　　提出了一些稳健的方法：有移动最小二乘法（根据距离残差增加权重）；采用2倍距离残差的协方差剔除离群点；迭代重权重方法(选权迭代法)。

　　MainWindow中的平面拟合方法，调用了ccPlane的Fit方法。

  1 void MainWindow::doActionFitPlane()
  2 {
  3     doComputePlaneOrientation(false);
  4 }
  5 
  6 void MainWindow::doActionFitFacet()
  7 {
  8     doComputePlaneOrientation(true);
  9 }
 10 
 11 static double s_polygonMaxEdgeLength = 0;
 12 void MainWindow::doComputePlaneOrientation(bool fitFacet)
 13 {
 14     ccHObject::Container selectedEntities = m_selectedEntities;
 15     size_t selNum = selectedEntities.size();
 16     if (selNum < 1)
 17         return;
 18 
 19     double maxEdgeLength = 0;
 20     if (fitFacet)
 21     {
 22         bool ok = true;
 23         maxEdgeLength = QInputDialog::getDouble(this,"Fit facet", "Max edge length (0 = no limit)", s_polygonMaxEdgeLength, 0, 1.0e9, 8, &ok);
 24         if (!ok)
 25             return;
 26         s_polygonMaxEdgeLength = maxEdgeLength;
 27     }
 28 
 29     for (size_t i=0; i<selNum; ++i)
 30     {
 31         ccHObject* ent = selectedEntities[i];
 32         ccShiftedObject* shifted = 0;
 33         CCLib::GenericIndexedCloudPersist* cloud = 0;
 34 
 35         if (ent->isKindOf(CC_TYPES::POLY_LINE))
 36         {
 37             ccPolyline* poly = ccHObjectCaster::ToPolyline(ent);
 38             cloud = static_cast<CCLib::GenericIndexedCloudPersist*>(poly);
 39             shifted = poly;
 40         }
 41         else
 42         {
 43             ccGenericPointCloud* gencloud = ccHObjectCaster::ToGenericPointCloud(ent);
 44             if (gencloud)
 45             {
 46                 cloud = static_cast<CCLib::GenericIndexedCloudPersist*>(gencloud);
 47                 shifted = gencloud;
 48             }
 49         }
 50 
 51         if (cloud)
 52         {
 53             double rms = 0.0;
 54             CCVector3 C,N;
 55 
 56             ccHObject* plane = 0;
 57             if (fitFacet)
 58             {
 59                 ccFacet* facet = ccFacet::Create(cloud, static_cast<PointCoordinateType>(maxEdgeLength));
 60                 if (facet)
 61                 {
 62                     plane = static_cast<ccHObject*>(facet);
 63                     N = facet->getNormal();
 64                     C = facet->getCenter();
 65                     rms = facet->getRMS();
 66 
 67                     //manually copy shift & scale info!
 68                     if (shifted)
 69                     {
 70                         ccPolyline* contour = facet->getContour();
 71                         if (contour)
 72                         {
 73                             contour->setGlobalScale(shifted->getGlobalScale());
 74                             contour->setGlobalShift(shifted->getGlobalShift());
 75                         }
 76                     }
 77                 }
 78             }
 79             else
 80             {
 81                 ccPlane* pPlane = ccPlane::Fit(cloud, &rms);
 82                 if (pPlane)
 83                 {
 84                     plane = static_cast<ccHObject*>(pPlane);
 85                     N = pPlane->getNormal();
 86                     C = *CCLib::Neighbourhood(cloud).getGravityCenter();
 87                     pPlane->enableStippling(true);
 88                 }
 89             }
 90 
 91             //as all information appears in Console...
 92             forceConsoleDisplay();
 93 
 94             if (plane)
 95             {
 96                 ccConsole::Print(QString("[Orientation] Entity '%1'").arg(ent->getName()));
 97                 ccConsole::Print("\t- plane fitting RMS: %f",rms);
 98 
 99                 //We always consider the normal with a positive 'Z' by default!
100                 if (N.z < 0.0)
101                     N *= -1.0;
102                 ccConsole::Print("\t- normal: (%f,%f,%f)",N.x,N.y,N.z);
103 
104                 //we compute strike & dip by the way
105                 PointCoordinateType dip = 0, dipDir = 0;
106                 ccNormalVectors::ConvertNormalToDipAndDipDir(N,dip,dipDir);
107                 QString dipAndDipDirStr = ccNormalVectors::ConvertDipAndDipDirToString(dip,dipDir);
108                 ccConsole::Print(QString("\t- %1").arg(dipAndDipDirStr));
109 
110                 //hack: output the transformation matrix that would make this normal points towards +Z
111                 ccGLMatrix makeZPosMatrix = ccGLMatrix::FromToRotation(N,CCVector3(0,0,PC_ONE));
112                 CCVector3 Gt = C;
113                 makeZPosMatrix.applyRotation(Gt);
114                 makeZPosMatrix.setTranslation(C-Gt);
115                 ccConsole::Print("[Orientation] A matrix that would make this plane horizontal (normal towards Z+) is:");
116                 ccConsole::Print(makeZPosMatrix.toString(12,' ')); //full precision
117                 ccConsole::Print("[Orientation] You can copy this matrix values (CTRL+C) and paste them in the 'Apply transformation tool' dialog");
118 
119                 plane->setName(dipAndDipDirStr);
120                 plane->applyGLTransformation_recursive(); //not yet in DB
121                 plane->setVisible(true);
122                 plane->setSelectionBehavior(ccHObject::SELECTION_FIT_BBOX);
123 
124                 ent->addChild(plane);
125                 plane->setDisplay(ent->getDisplay());
126                 plane->prepareDisplayForRefresh_recursive();
127                 addToDB(plane);
128             }
129             else
130             {
131                 ccConsole::Warning(QString("Failed to fit a plane/facet on entity '%1'").arg(ent->getName()));
132             }
133         }
134     }
135 
136     refreshAll();
137     updateUI();
138 }

 ccPlane的fit方法：

ccPlane* ccPlane::Fit(CCLib::GenericIndexedCloudPersist *cloud, double* rms/*=0*/)
{
	//number of points
	unsigned count = cloud->size();
	if (count < 3)
	{
		ccLog::Warning("[ccPlane::Fit] Not enough points in input cloud to fit a plane!");
		return 0;
	}

	CCLib::Neighbourhood Yk(cloud);

	//plane equation
	const PointCoordinateType* theLSPlane = Yk.getLSPlane();
	if (!theLSPlane)
	{
		ccLog::Warning("[ccPlane::Fit] Not enough points to fit a plane!");
		return 0;
	}

	//get the centroid
	const CCVector3* G = Yk.getGravityCenter();
	assert(G);

	//and a local base
	CCVector3 N(theLSPlane);
	const CCVector3* X = Yk.getLSPlaneX(); //main direction
	assert(X);
	CCVector3 Y = N * (*X);

	//compute bounding box in 2D plane
	CCVector2 minXY(0,0), maxXY(0,0);
	cloud->placeIteratorAtBegining();
	for (unsigned k=0; k<count; ++k)
	{
		//projection into local 2D plane ref.
		CCVector3 P = *(cloud->getNextPoint()) - *G;

		CCVector2 P2D( P.dot(*X), P.dot(Y) );

		if (k != 0)
		{
			if (minXY.x > P2D.x)
				minXY.x = P2D.x;
			else if (maxXY.x < P2D.x)
				maxXY.x = P2D.x;
			if (minXY.y > P2D.y)
				minXY.y = P2D.y;
			else if (maxXY.y < P2D.y)
				maxXY.y = P2D.y;
		}
		else
		{
			minXY = maxXY = P2D;
		}
	}

	//we recenter the plane
	PointCoordinateType dX = maxXY.x-minXY.x;
	PointCoordinateType dY = maxXY.y-minXY.y;
	CCVector3 Gt = *G + *X * (minXY.x + dX / 2) + Y * (minXY.y + dY / 2);
	ccGLMatrix glMat(*X,Y,N,Gt);

	ccPlane* plane = new ccPlane(dX, dY, &glMat);

	//compute least-square fitting RMS if requested
	if (rms)
	{
		*rms = CCLib::DistanceComputationTools::computeCloud2PlaneDistanceRMS(cloud, theLSPlane);
		plane->setMetaData(QString("RMS"),QVariant(*rms));
	}


	return plane;
}

Efficient Ransac shape extract插件调用的模板类Plane实现，可以看到使用的Jacobi特征值分解的方法实现。

        template< class PointT >
	template< class PointsForwardIt, class WeightsForwardIt >
	bool Plane< PointT >::Fit(const PointType &origin, PointsForwardIt begin,PointsForwardIt end, WeightsForwardIt weights)
	{
		MatrixXX< PointType::Dim, PointType::Dim, ScalarType > c, v;
		CovarianceMatrix(origin, begin, end, weights, &c);
		VectorXD< PointType::Dim, ScalarType > d;
		if(!Jacobi(c, &d, &v))
		{
			//std::cout << "Jacobi failed:" << std::endl;
			//std::cout << "origin = " << origin[0] << "," << origin[1] << "," << origin[2] << std::endl
			//	<< "cov:" << std::endl
			//	<< c[0][0] << c[1][0] << c[2][0] << std::endl
			//	<< c[0][1] << c[1][1] << c[2][1] << std::endl
			//	<< c[0][2] << c[1][2] << c[2][2] << std::endl;
			//std::cout << "recomp origin:" << std::endl;
			//PointT com;
			//Mean(begin, end, weights, &com);
			//std::cout << "origin = " << origin[0] << "," << origin[1] << "," << origin[2] << std::endl;
			//std::cout << "recomp covariance:" << std::endl;
			//CovarianceMatrix(com, begin, end, weights, &c);
			//std::cout << "cov:" << std::endl
			//<< c[0][0] << c[1][0] << c[2][0] << std::endl
			//<< c[0][1] << c[1][1] << c[2][1] << std::endl
			//<< c[0][2] << c[1][2] << c[2][2] << std::endl;
			//std::cout << "weights and points:" << std::endl;
			//WeightsForwardIt w = weights;
			//for(PointsForwardIt i = begin; i != end; ++i, ++w)
			//	std::cout << (*i)[0] << "," << (*i)[1] << "," << (*i)[2]
			//		<< " weight=" << (*w) << std::endl;
			return false;
		}
		for(unsigned int i = 0; i < PointType::Dim; ++i)
			d[i] = Math< ScalarType >::Abs(d[i]);
		EigSortDesc(&d, &v);
		_normal = PointType(v[PointType::Dim - 1]);
		_d = -(_normal * origin);
		return true;
	}

　　

posted @ 2016-11-25 15:25  太一吾鱼水  阅读(2600)  评论(0)  编辑  收藏  举报