3d code opencv

Checking for co-planarity of 3D points in OpenCV [w/code]

Just catching up on some bloggin, and I wanna share a snippet of OpenCV code on how to check if your (badly) triangulated 3D points came up co-planar, and therefore a botch triangulation. It’s a very simplistic method, only a few lines, and it also is part of my Structure from Motion Toy Library project.

vetor<Point3d> pcloud = GetCloud();
//check for coplanarity of points
	cv::Mat_<double> cldm(pcloud.size(),3);
	for(unsigned int i=0;i<pcloud.size();i++) {
		cldm.row(i)(0) = pcloud[i].x;
		cldm.row(i)(1) = pcloud[i].y;
		cldm.row(i)(2) = pcloud[i].z;
	cv::Mat_<double> mean;
	cv::PCA pca(cldm,mean,CV_PCA_DATA_AS_ROW);
	double p_to_plane_thresh =<double>(2);
	int num_inliers = 0;
	cv::Vec3d nrm = pca.eigenvectors.row(2); nrm = nrm / norm(nrm);
	cv::Vec3d x0 = pca.mean;
	for (int i=0; i<pcloud.size(); i++) {
		Vec3d w = Vec3d(pcloud[i].pt) - x0;
		double D = fabs(;
		if(D < p_to_plane_thresh) num_inliers++;
	cout << num_inliers << "/" << pcloud.size() << " are coplanar" << endl;
	if((double)num_inliers / (double)(pcloud.size()) > 0.9)
		return false;

As you can see I’m making some assumptions here, but they are mild.
I start from a PCA on the 3D point cloud. Now if the points were co-planar the 2 fist eigenvectors should lie on the plane, while the 3rd eigenvector would be the normal to the plane. “Very useful PCA is,” says Yoda.
Wise Yoda also notes that the eigenvalue corresponding to the 3rd eigenvector gives us the “thickness” of the plane, and can be used as the threshold value for telling if a point is part of the plane or not.
Not only that, but PCA will give us a mean point of the cloud, again if the most of the cloud is on a plane (like Nirvana) that mean point will be a good measure of a point that is on the plane (we call it x0). We need a point on the plane for the next part.
Then we calculate the distance of each point in the cloud from the plane, using the normal and x0.
If you think about it, you just take the vector from x0 to your point (my_p – x0) and project it on the normal. If you look at the length of that projection you will get the distance from the plane. Here I am only calculating the distance of the projection (the normal is already normalized, so it equals 1 and therefor dropped). Everything is much better explained here.
Enjoy your non-coplanar 3D point clouds!

One reply on “Checking for co-planarity of 3D points in OpenCV [w/code]”

Kudos for the (elegant) method and the joke about Nirvana (18 years have already gone 🙁 )
Maybe a small typo in line 25 of the code: I assume you want to return true if more than 90% of the points are inliers, right ?

Leave a Reply

Your email address will not be published. Required fields are marked *