DBViewer: refactored how poses are aligned with GPS to be able to mak…

…e KMZ files (OBJ/DAE origin matching with first KML pose)
introlab · Feb 2, 2025 · ddaf2f5 · ddaf2f5
1 parent fcfadfe
commit ddaf2f5
Show file tree

Hide file tree

Showing 3 changed files with 186 additions and 160 deletions.
diff --git a/corelib/include/rtabmap/core/Graph.h b/corelib/include/rtabmap/core/Graph.h
@@ -115,7 +115,8 @@ Transform RTABMAP_CORE_EXPORT calcRMSE(
 		float & rotational_median,
 		float & rotational_std,
 		float & rotational_min,
-		float & rotational_max);
+		float & rotational_max,
+		bool align2D = false);
 
 void RTABMAP_CORE_EXPORT computeMaxGraphErrors(
 		const std::map<int, Transform> & poses,

diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp
@@ -783,7 +783,8 @@ Transform calcRMSE (
 		float & rotational_median,
 		float & rotational_std,
 		float & rotational_min,
-		float & rotational_max)
+		float & rotational_max,
+		bool align2D)
 {
 
 	translational_rmse = 0.0f;
@@ -815,8 +816,8 @@ Transform calcRMSE (
 			{
 				idFirst = iter->first;
 			}
-			cloud1[oi] = pcl::PointXYZ(jter->second.x(), jter->second.y(), jter->second.z());
-			cloud2[oi++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+			cloud1[oi] = pcl::PointXYZ(jter->second.x(), jter->second.y(), align2D?0:jter->second.z());
+			cloud2[oi++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), align2D?0:iter->second.z());
 		}
 	}