ExportDialog: added ground normals up option, added camera projection mask and decimation options. Export CLI: added --ground_normals_up and --cam_projection_mask options, changed --bin option by --ascii option (now binary by default). DBViewer: warn when scan from depth option is enabled but there is no depth.

2026-09-02 17:40:23 +08:00 · 2022-01-27 17:57:56 -05:00
parent a8e5bbf415
commit 402afc07ed
9 changed files with 626 additions and 352 deletions
--- a/corelib/src/util3d.cpp
+++ b/corelib/src/util3d.cpp
@@ -2848,6 +2848,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		float maxDistance,
 		float maxAngle,
 		const std::vector<float> & roiRatios,
+		const cv::Mat & projMask,
 		bool distanceToCamPolicy,
 		const ProgressState * state)
 {
@@ -2857,6 +2858,8 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 	UINFO("maxDistance=%f", maxDistance);
 	UINFO("maxAngle=%f", maxAngle);
 	UINFO("distanceToCamPolicy=%s", distanceToCamPolicy?"true":"false");
+	UINFO("roiRatios=%s", roiRatios.size() == 4?uFormat("%f %f %f %f", roiRatios[0], roiRatios[1], roiRatios[2], roiRatios[3]):"");
+	UINFO("projMask=%dx%d", projMask.cols, projMask.rows);
 	std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;

 	if (cloud.empty() || cameraPoses.empty() || cameraModels.empty())
@@ -2873,18 +2876,46 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera

 	std::vector<ProjectionInfo> invertedIndex(cloud.size()); // For each point: list of cameras
 	int cameraProcessed = 0;
+	bool wrongMaskFormatWarned = false;
 	for(std::map<int, Transform>::const_iterator pter = cameraPoses.lower_bound(0); pter!=cameraPoses.end(); ++pter)
 	{
 		std::map<int, std::vector<CameraModel> >::const_iterator iter=cameraModels.find(pter->first);
 		if(iter!=cameraModels.end() && !iter->second.empty())
 		{
-			for(size_t i=0; i<iter->second.size(); ++i)
+			cv::Mat validProjMask;
+			if(!projMask.empty())
 			{
-				Transform cameraTransform = (pter->second * iter->second[i].localTransform());
+				if(projMask.type() != CV_8UC1)
+				{
+					if(!wrongMaskFormatWarned)
+						UERROR("Wrong camera projection mask type %d, should be CV_8UC1", projMask.type());
+					wrongMaskFormatWarned = true;
+				}
+				else if(projMask.cols == iter->second[0].imageWidth() * (int)iter->second.size() &&
+				        projMask.rows == iter->second[0].imageHeight())
+				{
+					validProjMask = projMask;
+				}
+				else
+				{
+					UWARN("Camera projection mask (%dx%d) is not valid for current "
+						   "camera model(s) (count=%ld, image size=%dx%d). It will be "
+						   "ignored for node %d",
+						   projMask.cols, projMask.rows,
+						   iter->second.size(),
+						   iter->second[0].imageWidth(),
+						   iter->second[0].imageHeight(),
+						   pter->first);
+				}
+			}
+
+			for(size_t camIndex=0; camIndex<iter->second.size(); ++camIndex)
+			{
+				Transform cameraTransform = (pter->second * iter->second[camIndex].localTransform());
 				UASSERT(!cameraTransform.isNull());
-				cv::Mat cameraMatrixK = iter->second[i].K();
+				cv::Mat cameraMatrixK = iter->second[camIndex].K();
 				UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
-				const cv::Size & imageSize = iter->second[i].imageSize();
+				const cv::Size & imageSize = iter->second[camIndex].imageSize();

 				float fx = cameraMatrixK.at<double>(0,0);
 				float fy = cameraMatrixK.at<double>(1,1);
@@ -2921,7 +2952,8 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 						int dx_high = dx + 0.5f;
 						int dy_high = dy + 0.5f;
 						int zMM = z * 1000;
-						if(uIsInBounds(dx_low, roi.x, roi.x+roi.width) && uIsInBounds(dy_low, roi.y, roi.y+roi.height))
+						if(uIsInBounds(dx_low, roi.x, roi.x+roi.width) && uIsInBounds(dy_low, roi.y, roi.y+roi.height) &&
+						   (validProjMask.empty() || validProjMask.at<unsigned char>(dy_low, imageSize.width*camIndex+dx_low) > 0))
 						{
 							set = true;
 							cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_low, dx_low);
@@ -2932,7 +2964,8 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 							}
 						}
 						if((dx_low != dx_high || dy_low != dy_high) &&
-							uIsInBounds(dx_high, roi.x, roi.x+roi.width) && uIsInBounds(dy_high, roi.y, roi.y+roi.height))
+							uIsInBounds(dx_high, roi.x, roi.x+roi.width) && uIsInBounds(dy_high, roi.y, roi.y+roi.height) &&
+							(validProjMask.empty() || validProjMask.at<unsigned char>(dy_high, imageSize.width*camIndex+dx_high) > 0))
 						{
 							set = true;
 							cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_high, dx_high);
@@ -2951,11 +2984,11 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 				if(count == 0)
 				{
 					registered = cv::Mat();
-					UINFO("No points projected in camera %d/%d", pter->first, i);
+					UINFO("No points projected in camera %d/%d", pter->first, camIndex);
 				}
 				else
 				{
-					UDEBUG("%d points projected in camera %d/%d", count, pter->first, i);
+					UDEBUG("%d points projected in camera %d/%d", count, pter->first, camIndex);
 				}
 				for(int u=0; u<registered.cols; ++u)
 				{
@@ -2966,7 +2999,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 						{
 							ProjectionInfo info;
 							info.nodeID = pter->first;
-							info.cameraIndex = i;
+							info.cameraIndex = camIndex;
 							info.uv.x = float(u)/float(imageSize.width);
 							info.uv.y = float(v)/float(imageSize.height);
 							const Transform & cam = cameraPoses.at(info.nodeID);
@@ -3070,6 +3103,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		float maxDistance,
 		float maxAngle,
 		const std::vector<float> & roiRatios,
+		const cv::Mat & projMask,
 		bool distanceToCamPolicy,
 		const ProgressState * state)
 {
@@ -3079,6 +3113,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 			maxDistance,
 			maxAngle,
 			roiRatios,
+			projMask,
 			distanceToCamPolicy,
 			state);
 }
@@ -3090,6 +3125,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		float maxDistance,
 		float maxAngle,
 		const std::vector<float> & roiRatios,
+		const cv::Mat & projMask,
 		bool distanceToCamPolicy,
 		const ProgressState * state)
 {
@@ -3099,6 +3135,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 			maxDistance,
 			maxAngle,
 			roiRatios,
+			projMask,
 			distanceToCamPolicy,
 			state);
 }
--- a/corelib/src/util3d_surface.cpp
+++ b/corelib/src/util3d_surface.cpp
@@ -3566,7 +3566,8 @@ void adjustNormalsToViewPointsImpl(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
 		const std::vector<int> & rawCameraIndices,
-		typename pcl::PointCloud<PointT>::Ptr & cloud)
+		typename pcl::PointCloud<PointT>::Ptr & cloud,
+		float groundNormalsUp)
 {
 	if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size())
 	{
@@ -3592,7 +3593,8 @@ void adjustNormalsToViewPointsImpl(
 					Eigen::Vector3f n(normal.x, normal.y, normal.z);

 					float result = v.dot(n);
-					if(result < 0)
+					if(result < 0 ||
+					   (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint.z)) // some far velodyne rays on road can have normals toward ground)
 					{
 						//reverse normal
 						cloud->points[i].normal_x *= -1.0f;
@@ -3613,34 +3615,38 @@ void adjustNormalsToViewPoints(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
 		const std::vector<int> & rawCameraIndices,
-		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud)
+		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		float groundNormalsUp)
 {
-	adjustNormalsToViewPointsImpl<pcl::PointNormal>(poses, rawCloud, rawCameraIndices, cloud);
+	adjustNormalsToViewPointsImpl<pcl::PointNormal>(poses, rawCloud, rawCameraIndices, cloud, groundNormalsUp);
 }

 void adjustNormalsToViewPoints(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
 		const std::vector<int> & rawCameraIndices,
-		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud)
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		float groundNormalsUp)
 {
-	adjustNormalsToViewPointsImpl<pcl::PointXYZRGBNormal>(poses, rawCloud, rawCameraIndices, cloud);
+	adjustNormalsToViewPointsImpl<pcl::PointXYZRGBNormal>(poses, rawCloud, rawCameraIndices, cloud, groundNormalsUp);
 }

 void adjustNormalsToViewPoints(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
 		const std::vector<int> & rawCameraIndices,
-		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud)
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		float groundNormalsUp)
 {
-	adjustNormalsToViewPointsImpl<pcl::PointXYZINormal>(poses, rawCloud, rawCameraIndices, cloud);
+	adjustNormalsToViewPointsImpl<pcl::PointXYZINormal>(poses, rawCloud, rawCameraIndices, cloud, groundNormalsUp);
 }

 void adjustNormalsToViewPoints(
 		const std::map<int, Transform> & viewpoints,
 		const LaserScan & rawScan,
 		const std::vector<int> & viewpointIds,
-		LaserScan & scan)
+		LaserScan & scan,
+		float groundNormalsUp)
 {
 	UDEBUG("poses=%d, rawCloud=%d, rawCameraIndices=%d, cloud=%d", (int)viewpoints.size(), (int)rawScan.size(), (int)viewpointIds.size(), (int)scan.size());
 	if(viewpoints.size() && rawScan.size() && rawScan.size() == (int)viewpointIds.size() && scan.size() && scan.hasNormals())
@@ -3669,7 +3675,8 @@ void adjustNormalsToViewPoints(
 					Eigen::Vector3f n(normal.x, normal.y, normal.z);

 					float result = v.dot(n);
-					if(result < 0)
+					if(result < 0 ||
+					   (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && point.z < viewpoint.z)) // some far velodyne rays on road can have normals toward ground))
 					{
 						//reverse normal
 						scan.field(i, scan.getNormalsOffset()) *= -1.0f;