Fixed ProximityByTime wrong guess transform. Fixed Icp correspondence ratio computation when using voxel filter. OdometryResetEvent: added pose to constructor.

2026-09-13 23:10:20 +08:00 · 2017-08-15 15:26:03 -04:00
parent 1e335e53ba
commit b5b96a3edb
6 changed files with 66 additions and 80 deletions
@@ -97,9 +97,12 @@ private:
 class OdometryResetEvent : public UEvent
 {
 public:
-	OdometryResetEvent(){}
+	OdometryResetEvent(const Transform & pose = Transform::getIdentity()){_pose = pose;}
 	virtual ~OdometryResetEvent() {}
 	virtual std::string getClassName() const {return "OdometryResetEvent";}
+	const Transform & getPose() const {return _pose;}
+private:
+	Transform _pose;
 };

 }
@@ -65,6 +65,7 @@ private:
 	Odometry * _odometry;
 	unsigned int _dataBufferMaxSize;
 	bool _resetOdometry;
+	Transform _resetPose;
 };

 } // namespace rtabmap
@@ -39,7 +39,8 @@ namespace rtabmap {
 OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize) :
 	_odometry(odometry),
 	_dataBufferMaxSize(dataBufferMaxSize),
-	_resetOdometry(false)
+	_resetOdometry(false),
+	_resetPose(Transform::getIdentity())
 {
 	UASSERT(_odometry != 0);
 }
@@ -67,10 +68,16 @@ bool OdometryThread::handleEvent(UEvent * event)
 				this->addData(cameraEvent->data());
 			}
 		}
-		else if(event->getClassName().compare("OdometryResetEvent") == 0)
+	}
+	if(event->getClassName().compare("OdometryResetEvent") == 0)
+	{
+		OdometryResetEvent * odomEvent = (OdometryResetEvent*)event;
+		_resetPose.setIdentity();
+		if(!odomEvent->getPose().isNull())
 		{
-			_resetOdometry = true;
+			_resetPose = odomEvent->getPose();
 		}
+		_resetOdometry = true;
 	}
 	return false;
 }
@@ -92,7 +99,7 @@ void OdometryThread::mainLoop()
 {
 	if(_resetOdometry)
 	{
-		_odometry->reset();
+		_odometry->reset(_resetPose);
 		_resetOdometry = false;
 	}

@@ -104,11 +104,13 @@ Transform RegistrationIcp::computeTransformationImpl(
 	SensorData & dataFrom = fromSignature.sensorData();
 	SensorData & dataTo = toSignature.sensorData();

-	UDEBUG("size from=%d (channels=%d) to=%d (channels=%d)",
+	UDEBUG("size from=%d (channels=%d, max pts=%d) to=%d (channels=%d, max pts=%d)",
 			dataFrom.laserScanRaw().cols,
 			dataFrom.laserScanRaw().channels(),
+			dataFrom.laserScanInfo().maxPoints(),
 			dataTo.laserScanRaw().cols,
-			dataTo.laserScanRaw().channels());
+			dataTo.laserScanRaw().channels(),
+			dataTo.laserScanInfo().maxPoints());

 	if(!guess.isNull() && !dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
 	{
@@ -164,20 +166,6 @@ Transform RegistrationIcp::computeTransformationImpl(
 							_maxCorrespondenceDistance,
 							variance,
 							correspondences);
-					/*
-					UWARN("icpT=%s", icpT.prettyPrint().c_str());
-					pcl::io::savePCDFile("fromCloud.pcd", *fromCloudNormals);
-					pcl::io::savePCDFile("toCloud.pcd", *toCloudNormals);
-					UWARN("saved fromCloud.pcd and toCloud.pcd");
-					if(!icpT.isNull())
-					{
-						pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudTmp = util3d::transformPointCloud(fromCloudNormals, icpT);
-						pcl::io::savePCDFile("fromCloudFinal.pcd", *fromCloudTmp);
-						pcl::io::savePCDFile("fromCloudFinal2.pcd", *fromCloudNormalsRegistered);
-						UWARN("saved fromCloudFinal.pcd");
-					}
-					*/
-
 				}
 			}
 			else
@@ -188,7 +176,6 @@ Transform RegistrationIcp::computeTransformationImpl(

 				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
 				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
-				bool filtered = false;
 				if(_voxelSize > 0.0f)
 				{
 					int pointsBeforeFiltering = fromCloudFiltered->size();
@@ -199,14 +186,13 @@ Transform RegistrationIcp::computeTransformationImpl(
 					toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize);
 					maxLaserScansTo = maxLaserScansTo * toCloudFiltered->size() / pointsBeforeFiltering;

-					filtered = true;
-					UDEBUG("Voxel filtering time (voxel=%f m) = %f s", _voxelSize, timer.ticks());
-
-					//Adjust maxLaserScans
-
+					UDEBUG("Voxel filtering time (voxel=%f m, ratioFrom=%f ratioTo=%f) = %f s",
+							_voxelSize,
+							float(fromCloudFiltered->size()) / float(pointsBeforeFiltering),
+							float(toCloudFiltered->size()) / float(pointsBeforeFiltering),
+							timer.ticks());
 				}

-				bool correspondencesComputed = false;
 				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
 				if(_pointToPlane) // ICP Point To Plane, only in 3D
 				{
@@ -242,9 +228,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 							   *fromCloudNormalsRegistered,
 							   _epsilon,
 							   this->force3DoF());
-						if(!filtered &&
-							!icpT.isNull() &&
-							hasConverged)
+						if(!icpT.isNull() && hasConverged)
 						{
 							util3d::computeVarianceAndCorrespondences(
 									fromCloudNormalsRegistered,
@@ -252,7 +236,6 @@ Transform RegistrationIcp::computeTransformationImpl(
 									_maxCorrespondenceDistance,
 									variance,
 									correspondences);
-							correspondencesComputed = true;
 						}
 					}
 				}
@@ -274,43 +257,21 @@ Transform RegistrationIcp::computeTransformationImpl(
 						   *fromCloudRegistered,
 						   _epsilon,
 						   this->force3DoF()); // icp2D
-				}

-				/*pcl::io::savePCDFile("fromCloud.pcd", *fromCloud);
-				pcl::io::savePCDFile("toCloud.pcd", *toCloud);
-				UWARN("saved fromCloud.pcd and toCloud.pcd");
-				if(!icpT.isNull())
-				{
-					pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudTmp = util3d::transformPointCloud(fromCloud, icpT);
-					pcl::io::savePCDFile("fromCloudFinal.pcd", *fromCloudTmp);
-					UWARN("saved fromCloudFinal.pcd");
-				}*/
-
-				if(!icpT.isNull() &&
-					hasConverged &&
-					!correspondencesComputed)
-				{
-					if(filtered)
+					if(!icpT.isNull() && hasConverged)
 					{
-						fromCloud = util3d::transformPointCloud(fromCloud, icpT);
+						util3d::computeVarianceAndCorrespondences(
+								fromCloudRegistered,
+								toCloudFiltered,
+								_maxCorrespondenceDistance,
+								variance,
+								correspondences);
 					}
-					else
-					{
-						fromCloud = fromCloudRegistered;
-					}
-
-					util3d::computeVarianceAndCorrespondences(
-							fromCloud,
-							toCloud,
-							_maxCorrespondenceDistance,
-							variance,
-							correspondences);
 				}
 			}
 			UDEBUG("ICP (iterations=%d) time = %f s", _maxIterations, timer.ticks());

-			if(!icpT.isNull() &&
-				hasConverged)
+			if(!icpT.isNull() && hasConverged)
 			{
 				float ix,iy,iz, iroll,ipitch,iyaw;
 				Transform icpInTargetReferential = guess.inverse() * icpT.inverse() * guess; // actual local ICP refinement
@@ -332,8 +293,9 @@ Transform RegistrationIcp::computeTransformationImpl(
 				}
 				else
 				{
-					// verify if there are enough correspondences
+					// verify if there are enough correspondences (using "To" by default if set, in case if "From" is merged from multiple scans)
 					int maxLaserScans = maxLaserScansTo?maxLaserScansTo:maxLaserScansFrom;
+					UDEBUG("Max scans=%d (from=%d, to=%d)", maxLaserScans, maxLaserScansFrom, maxLaserScansTo);
 					if(maxLaserScans)
 					{
 						correspondencesRatio = float(correspondences)/float(maxLaserScans);
@@ -1247,7 +1247,7 @@ bool Rtabmap::process(
 					Transform guess;
 					if(_optimizedPoses.find(*iter) != _optimizedPoses.end())
 					{
-						guess = newPose.inverse() * _optimizedPoses.at(*iter);
+						guess = _optimizedPoses.at(*iter).inverse() * newPose;
 					}

 					// For proximity by time, correspondences should be already enough precise, so don't recompute them
@@ -95,7 +95,7 @@
                <enum>QFrame::Raised</enum>
               </property>
               <property name="currentIndex">
-                <number>34</number>
+                <number>20</number>
               </property>
               <widget class="QWidget" name="page_22">
                <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -12281,7 +12281,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                   <property name="title">
                    <string>Visual Registration</string>
                   </property>
-                   <layout class="QVBoxLayout" name="verticalLayout_41" stretch="0,0,0,1">
+                   <layout class="QVBoxLayout" name="verticalLayout_41" stretch="0,0,0,1,0">
                    <item>
                     <layout class="QGridLayout" name="gridLayout_23" columnstretch="0,1">
                      <item row="4" column="0">
@@ -13115,7 +13115,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                      <property name="checked">
                       <bool>false</bool>
                      </property>
-                      <layout class="QVBoxLayout" name="verticalLayout_46" stretch="0,0,1,0">
+                      <layout class="QVBoxLayout" name="verticalLayout_46" stretch="0,0,1">
                       <item>
                        <widget class="QLabel" name="label_182">
                         <property name="text">
@@ -13401,25 +13401,38 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                            </item>
                           </layout>
                          </item>
+                          <item>
+                           <spacer name="verticalSpacer_37">
+                            <property name="orientation">
+                             <enum>Qt::Vertical</enum>
+                            </property>
+                            <property name="sizeHint" stdset="0">
+                             <size>
+                              <width>20</width>
+                              <height>40</height>
+                             </size>
+                            </property>
+                           </spacer>
+                          </item>
                         </layout>
                        </widget>
                       </item>
-                       <item>
-                        <spacer name="verticalSpacer_37">
-                         <property name="orientation">
-                          <enum>Qt::Vertical</enum>
-                         </property>
-                         <property name="sizeHint" stdset="0">
-                          <size>
-                           <width>20</width>
-                           <height>40</height>
-                          </size>
-                         </property>
-                        </spacer>
-                       </item>
                      </layout>
                     </widget>
                    </item>
+                    <item>
+                     <spacer name="verticalSpacer_66">
+                      <property name="orientation">
+                       <enum>Qt::Vertical</enum>
+                      </property>
+                      <property name="sizeHint" stdset="0">
+                       <size>
+                        <width>20</width>
+                        <height>40</height>
+                       </size>
+                      </property>
+                     </spacer>
+                    </item>
                   </layout>
                  </widget>
                 </item>