Added RGBD/ProximityMergedScanCovFactor parameter. Localization: fixed output height when Reg/Force3DoF=true but input poses are 6DoF. Transform: added is3DoF() and is4DoF() functions. GTSAM: when Reg/Force3DoF=true, copy input roll,pitch,z values for output poses. RegIcp: fixed working memory dir '~' conversion. RGBD/ProximityGlobalScanMap: fixed map::at error when some nodes don't have scans.

2026-09-11 14:00:20 +08:00 · 2022-01-12 16:14:42 -05:00
parent a4ec95963e
commit fcec98105d
14 changed files with 122 additions and 43 deletions
@@ -221,6 +221,20 @@ Transform Transform::to4DoF() const
 	return Transform(x,y,z, 0,0,yaw);
 }

+bool Transform::is3DoF() const
+{
+	return is4DoF() && z() == 0.0;
+}
+
+bool Transform::is4DoF() const
+{
+	return r13() == 0.0 &&
+		   r23() == 0.0 &&
+		   r31() == 0.0 &&
+		   r32() == 0.0 &&
+		   r33() == 0.0;
+}
+
 cv::Mat Transform::rotationMatrix() const
 {
 	return data_.colRange(0, 3).clone();