dense: estimate point-cloud scales

apps/DensifyPointCloud/DensifyPointCloud.cpp

@@ -62,6 +62,7 @@ float fBorderROI;
 bool bCrop2ROI;
 int nEstimateROI;
 int nFusionMode;
+float fEstimateScale;
 int thFilterPointCloud;
 int nExportNumViews;
 int nArchiveType;
@@ -140,6 +141,7 @@ bool Initialize(size_t argc, LPCTSTR* argv)
 		("geometric-iters", boost::program_options::value(&nEstimationGeometricIters)->default_value(2), "number of geometric consistent patch-match iterations (0 - disabled)")
 		("estimate-colors", boost::program_options::value(&nEstimateColors)->default_value(2), "estimate the colors for the dense point-cloud (0 - disabled, 1 - final, 2 - estimate)")
 		("estimate-normals", boost::program_options::value(&nEstimateNormals)->default_value(2), "estimate the normals for the dense point-cloud (0 - disabled, 1 - final, 2 - estimate)")
+		("estimate-scale", boost::program_options::value(&OPT::fEstimateScale)->default_value(0.f), "estimate the point-scale for the dense point-cloud (scale multiplier, 0 - disabled)")
 		("sub-scene-area", boost::program_options::value(&OPT::fMaxSubsceneArea)->default_value(0.f), "split the scene in sub-scenes such that each sub-scene surface does not exceed the given maximum sampling area (0 - disabled)")
 		("sample-mesh", boost::program_options::value(&OPT::fSampleMesh)->default_value(0.f), "uniformly samples points on a mesh (0 - disabled, <0 - number of points, >0 - sample density per square unit)")
 		("fusion-mode", boost::program_options::value(&OPT::nFusionMode)->default_value(0), "depth-maps fusion mode (-2 - fuse disparity-maps, -1 - export disparity-maps only, 0 - depth-maps & fusion, 1 - export depth-maps only)")
@@ -385,6 +387,20 @@ int main(int argc, LPCTSTR* argv)
 		Finalize();
 		return EXIT_SUCCESS;
 	}
+	if (OPT::fEstimateScale > 0 && !scene.pointcloud.IsEmpty() && !scene.images.empty()) {
+		// simply export existing point-cloud with scale
+		if (scene.pointcloud.normals.empty()) {
+			if (!scene.pointcloud.IsValid()) {
+				VERBOSE("error: can not estimate normals as the point-cloud is not valid");
+				return EXIT_FAILURE;
+			}
+			EstimatePointNormals(scene.images, scene.pointcloud);
+		}
+		const String baseFileName(MAKE_PATH_SAFE(Util::getFileFullName(OPT::strOutputFileName)));
+		scene.pointcloud.SaveWithScale(baseFileName+_T("_scale.ply"), scene.images, OPT::fEstimateScale);
+		Finalize();
+		return EXIT_SUCCESS;
+	}
 	if ((ARCHIVE_TYPE)OPT::nArchiveType != ARCHIVE_MVS) {
 		#if TD_VERBOSE != TD_VERBOSE_OFF
 		if (VERBOSITY_LEVEL > 1 && !scene.pointcloud.IsEmpty())

libs/MVS/Camera.h

@@ -402,6 +402,43 @@ class MVS_API Camera : public CameraIntern
 		return TransformPointOrthoC2I(TransformPointW2C(X));
 	}
 
+	// compute the projection scale in this camera of the given world point
+	template <typename TYPE>
+	inline TYPE GetFootprintImage(const TPoint3<TYPE>& X) const {
+		#if 0
+		const TYPE fSphereRadius(1);
+		const TPoint3<TYPE> camX(TransformPointW2C(X));
+		return norm(TransformPointC2I(TPoint3<TYPE>(camX.x+fSphereRadius,camX.y,camX.z))-TransformPointC2I(camX));
+		#else
+		return GetFocalLength() / PointDepth(X);
+		#endif
+	}
+	// compute the surface the projected pixel covers at the given depth
+	template <typename TYPE>
+	inline TYPE GetFootprintWorldSq(const TPoint2<TYPE>& x, TYPE depth) const {
+		#if 0
+		return SQUARE(GetFocalLength());
+		#else
+		// improved version of the above
+		return SQUARE(depth) / (SQUARE(GetFocalLength()) + normSq(TransformPointI2V(x)));
+		#endif
+	}
+	template <typename TYPE>
+	inline TYPE GetFootprintWorld(const TPoint2<TYPE>& x, TYPE depth) const {
+		return depth / SQRT(SQUARE(GetFocalLength()) + normSq(TransformPointI2V(x)));
+	}
+	// same as above, but the 3D point is given
+	template <typename TYPE>
+	inline TYPE GetFootprintWorldSq(const TPoint3<TYPE>& X) const {
+		const TPoint3<TYPE> camX(TransformPointW2C(X));
+		return GetFootprintWorldSq(TPoint2<TYPE>(camX.x/camX.z,camX.y/camX.z), camX.z);
+	}
+	template <typename TYPE>
+	inline TYPE GetFootprintWorld(const TPoint3<TYPE>& X) const {
+		const TPoint3<TYPE> camX(TransformPointW2C(X));
+		return GetFootprintWorld(TPoint2<TYPE>(camX.x/camX.z,camX.y/camX.z), camX.z);
+	}
+
 	#ifdef _USE_BOOST
 	// implement BOOST serialization
 	template<class Archive>

libs/MVS/DepthMap.cpp

@@ -1540,6 +1540,8 @@ void MVS::EstimatePointNormals(const ImageArr& images, PointCloud& pointcloud, i
 {
 	TD_TIMER_START();
 
+	ASSERT(pointcloud.IsValid());
+
 	typedef CGAL::Simple_cartesian<double> kernel_t;
 	typedef kernel_t::Point_3 point_t;
 	typedef kernel_t::Vector_3 vector_t;

libs/MVS/PointCloud.cpp

@@ -223,6 +223,8 @@ namespace BasicPLY {
 		Point p;
 		Color c;
 		Normal n;
+		float scale;
+		float confidence;
 	};
 	static const PLY::PlyProperty vert_props[] = {
 		{"x",     PLY::Float32, PLY::Float32, offsetof(PointColNormal,p.x), 0, 0, 0, 0},
@@ -233,7 +235,9 @@ namespace BasicPLY {
 		{"blue",  PLY::Uint8,   PLY::Uint8,   offsetof(PointColNormal,c.b), 0, 0, 0, 0},
 		{"nx",    PLY::Float32, PLY::Float32, offsetof(PointColNormal,n.x), 0, 0, 0, 0},
 		{"ny",    PLY::Float32, PLY::Float32, offsetof(PointColNormal,n.y), 0, 0, 0, 0},
-		{"nz",    PLY::Float32, PLY::Float32, offsetof(PointColNormal,n.z), 0, 0, 0, 0}
+		{"nz",    PLY::Float32, PLY::Float32, offsetof(PointColNormal,n.z), 0, 0, 0, 0},
+		{"scale", PLY::Float32, PLY::Float32, offsetof(PointColNormal,scale), 0, 0, 0, 0},
+		{"confidence", PLY::Float32, PLY::Float32, offsetof(PointColNormal,confidence), 0, 0, 0, 0}
 	};
 	// list of the kinds of elements in the PLY
 	static const char* elem_names[] = {
@@ -296,7 +300,7 @@ bool PointCloud::Load(const String& fileName)
 } // Load
 
 // save the dense point cloud as PLY file
-bool PointCloud::Save(const String& fileName, bool bLegacyTypes) const
+bool PointCloud::Save(const String& fileName, bool bLegacyTypes, bool bBinary) const
 {
 	if (points.IsEmpty())
 		return false;
@@ -308,7 +312,7 @@ bool PointCloud::Save(const String& fileName, bool bLegacyTypes) const
 	PLY ply;
 	if (bLegacyTypes)
 		ply.set_legacy_type_names();
-	if (!ply.write(fileName, 1, BasicPLY::elem_names, PLY::BINARY_LE, 64*1024))
+	if (!ply.write(fileName, 1, BasicPLY::elem_names, bBinary?PLY::BINARY_LE:PLY::ASCII, 64*1024))
 		return false;
 
 	if (normals.IsEmpty()) {
@@ -353,7 +357,7 @@ bool PointCloud::Save(const String& fileName, bool bLegacyTypes) const
 } // Save
 
 // save the dense point cloud having >=N views as PLY file
-bool PointCloud::SaveNViews(const String& fileName, uint32_t minViews, bool bLegacyTypes) const
+bool PointCloud::SaveNViews(const String& fileName, uint32_t minViews, bool bLegacyTypes, bool bBinary) const
 {
 	if (points.IsEmpty())
 		return false;
@@ -365,7 +369,7 @@ bool PointCloud::SaveNViews(const String& fileName, uint32_t minViews, bool bLeg
 	PLY ply;
 	if (bLegacyTypes)
 		ply.set_legacy_type_names();
-	if (!ply.write(fileName, 1, BasicPLY::elem_names, PLY::BINARY_LE, 64*1024))
+	if (!ply.write(fileName, 1, BasicPLY::elem_names, bBinary?PLY::BINARY_LE:PLY::ASCII, 64*1024))
 		return false;
 
 	if (normals.IsEmpty()) {
@@ -407,6 +411,90 @@ bool PointCloud::SaveNViews(const String& fileName, uint32_t minViews, bool bLeg
 	DEBUG_EXTRA("Point-cloud saved: %u points with at least %u views each (%s)", numPoints, minViews, TD_TIMER_GET_FMT().c_str());
 	return true;
 } // SaveNViews
+
+// save the dense point cloud + scale as PLY file
+bool PointCloud::SaveWithScale(const String& fileName, const ImageArr& images, float scaleMult, bool bLegacyTypes, bool bBinary) const
+{
+	if (points.empty())
+		return false;
+
+	TD_TIMER_STARTD();
+
+	// create PLY object
+	ASSERT(!fileName.empty());
+	Util::ensureFolder(fileName);
+	PLY ply;
+	if (bLegacyTypes)
+		ply.set_legacy_type_names();
+	if (!ply.write(fileName, 1, BasicPLY::elem_names, bBinary?PLY::BINARY_LE:PLY::ASCII, 64*1024))
+		return false;
+
+	// describe what properties go into the vertex elements
+	ply.describe_property(BasicPLY::elem_names[0], 11, BasicPLY::vert_props);
+
+	// export the array of 3D points
+	BasicPLY::PointColNormal vertex;
+	FOREACH(i, points) {
+		// export the vertex position, normal and scale
+		vertex.p = points[i];
+		if (colors.empty())
+			vertex.c = Color::WHITE;
+		else
+			vertex.c = colors[i];
+		if (normals.empty())
+			vertex.n = Vec3f::ZERO;
+		else
+			vertex.n = normals[i];
+		#if 0
+		// one sample per view
+		vertex.confidence = 1;
+		for (IIndex idxView: pointViews[i]) {
+			const float scale((float)images[idxView].camera.GetFootprintWorld(Cast<REAL>(vertex.p)));
+			ASSERT(scale > 0);
+			vertex.scale = scale*scaleMult;
+			ply.put_element(&vertex);
+		}
+		#else
+		// one sample per point
+		vertex.scale = FLT_MAX;
+		if (pointWeights.empty()) {
+			vertex.confidence = (float)pointViews[i].size();
+			for (IIndex idxView: pointViews[i]) {
+				const float scale((float)images[idxView].camera.GetFootprintWorld(Cast<REAL>(vertex.p)));
+				ASSERT(scale > 0);
+				if (vertex.scale > scale)
+					vertex.scale = scale;
+			}
+		} else {
+			vertex.confidence = 0;
+			float scaleWeightBest = FLT_MAX;
+			FOREACH(j, pointViews[i]) {
+				const IIndex idxView = pointViews[i][j];
+				const float scale((float)images[idxView].camera.GetFootprintWorld(Cast<REAL>(vertex.p)));
+				ASSERT(scale > 0);
+				const float conf(pointWeights[i][j]);
+				const float scaleWeight(scale/conf);
+				if (scaleWeightBest > scaleWeight) {
+					scaleWeightBest = scaleWeight;
+					vertex.scale = scale;
+					vertex.confidence = conf;
+				}
+			}
+		}
+		ASSERT(vertex.scale != FLT_MAX);
+		vertex.scale *= scaleMult;
+		ply.put_element(&vertex);
+		#endif
+	}
+	ASSERT(ply.get_current_element_count() == (int)points.size());
+
+	// write the header
+	if (!ply.header_complete())
+		return false;
+
+	DEBUG_EXTRA("Point-cloud saved: %u points with scale (%s)", points.size(), TD_TIMER_GET_FMT().c_str());
+	return true;
+} // SaveWithScale
 /*----------------------------------------------------------------*/
 
 

libs/MVS/PointCloud.h

@@ -102,8 +102,9 @@ class MVS_API PointCloud
 	Planef EstimateGroundPlane(const ImageArr& images, float planeThreshold=0, const String& fileExportPlane="") const;
 
 	bool Load(const String& fileName);
-	bool Save(const String& fileName, bool bLegacyTypes=false) const;
-	bool SaveNViews(const String& fileName, uint32_t minViews, bool bLegacyTypes=false) const;
+	bool Save(const String& fileName, bool bLegacyTypes=false, bool bBinary=true) const;
+	bool SaveNViews(const String& fileName, uint32_t minViews, bool bLegacyTypes=false, bool bBinary=true) const;
+	bool SaveWithScale(const String& fileName, const ImageArr& images, float scaleMult, bool bLegacyTypes=false, bool bBinary=true) const;
 
 	void PrintStatistics(const Image* pImages = NULL, const OBB3f* pObb = NULL) const;
 

libs/MVS/Scene.cpp

@@ -786,16 +786,6 @@ bool Scene::EstimateNeighborViewsPointCloud(unsigned maxResolution)
 } // EstimateNeighborViewsPointCloud
 /*----------------------------------------------------------------*/
 
-inline float Footprint(const Camera& camera, const Point3f& X) {
-	#if 0
-	const REAL fSphereRadius(1);
-	const Point3 cX(camera.TransformPointW2C(Cast<REAL>(X)));
-	return (float)norm(camera.TransformPointC2I(Point3(cX.x+fSphereRadius,cX.y,cX.z))-camera.TransformPointC2I(cX))+std::numeric_limits<float>::epsilon();
-	#else
-	return (float)(camera.GetFocalLength()/camera.PointDepth(X));
-	#endif
-}
-
 // compute visibility for the reference image
 // and select the best views for reconstructing the dense point-cloud;
 // extract also all 3D points seen by the reference image;
@@ -848,15 +838,15 @@ bool Scene::SelectNeighborViews(uint32_t ID, IndexArr& points, unsigned nMinView
 		++nPoints;
 		// score shared views
 		const Point3f V1(imageData.camera.C - Cast<REAL>(point));
-		const float footprint1(Footprint(imageData.camera, point));
+		const float footprint1(imageData.camera.GetFootprintImage(point));
 		for (const PointCloud::View& view: views) {
 			if (view == ID)
 				continue;
 			const Image& imageData2 = images[view];
 			const Point3f V2(imageData2.camera.C - Cast<REAL>(point));
 			const float fAngle(ACOS(ComputeAngle(V1.ptr(), V2.ptr())));
 			const float wAngle(EXP(SQUARE(fAngle-fOptimAngle)*(fAngle<fOptimAngle?sigmaAngleSmall:sigmaAngleLarge)));
-			const float footprint2(Footprint(imageData2.camera, point));
+			const float footprint2(imageData2.camera.GetFootprintImage(point));
 			const float fScaleRatio(footprint1/footprint2);
 			float wScale;
 			if (fScaleRatio > 1.6f)
@@ -1060,7 +1050,7 @@ unsigned Scene::Split(ImagesChunkArr& chunks, float maxArea, int depthMapStep) c
 					const Point3f X(Cast<float>(camera.TransformPointI2W(Point3(c,r,depth))));
 					if (IsBounded() && !obb.Intersects(X))
 						continue;
-					areas.emplace_back(Footprint(camera, X)*areaScale);
+					areas.emplace_back(camera.GetFootprintImage(X)*areaScale);
 					visibility.emplace_back(idxImage);
 					samples.emplace_back(X);
 				}