OdomF2M: Scan gravity optimization

corelib/include/rtabmap/core/ImuMotionPredictor.h

@@ -35,7 +35,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <Eigen/Geometry>
 
 #include <map>
-#include <vector>
 
 namespace rtabmap {
 
@@ -61,17 +60,6 @@ namespace rtabmap {
  * ignored: its centripetal and tangential accelerations are small over the fraction of a
  * second this predicts.
  *
- * The gravity removed can be estimated from the IMU itself (see the constructor): an
- * accelerometer can read a few percent off at rest (scale or bias), and that error,
- * integrated as an acceleration, would bias the predicted velocity along gravity. The
- * accelerometer is averaged over the windows where it is quiet: with a gyro and an
- * accelerometer that barely change, the IMU is not accelerating (at rest or at constant
- * velocity), so it only measures gravity. The IMU must then be held still (or at constant
- * velocity) for a moment, e.g., before moving: until then, standard gravity is used, and
- * a steady acceleration without rotation would be taken for gravity. Only the error along
- * gravity is corrected, which is all of it for an IMU that stays about level; one changing
- * attitude would need its axes calibrated.
- *
  * Not thread-safe: a caller sharing it between threads must lock around every call.
  */
 class RTABMAP_CORE_EXPORT ImuMotionPredictor
@@ -93,9 +81,7 @@ public:
 	 *                        velocity itself. 0: the displacement since the previous pose,
 	 *                        without acceleration.
 	 * @param gravity         magnitude (m/s^2) of the gravity removed from the specific force
-	 *                        given to addImu(), standard gravity by default. <= 0: estimated
+	 *                        given to addImu(), standard gravity by default
-	 *                        from the IMU when it is still (see the class description), with
-	 *                        standard gravity until then.
 	 *
 	 * These are fixed for the life of the predictor (there is no setter): changing them
 	 * while it estimates would mix poses and samples taken under different settings.
@@ -107,12 +93,7 @@ public:
 
 	double maxPoseInterval() const {return maxPoseInterval_;}
 	double velocityWindow() const {return velocityWindow_;}
-	/// Gravity (m/s^2) removed from the specific force: the one given, or the estimate
 	double gravity() const {return gravity_;}
-	/// Whether the gravity is estimated from the IMU (see the constructor)
-	bool isGravityEstimated() const {return gravityEstimated_;}
-	/// Number of quiet IMU windows the gravity estimate is from (0: not estimated yet)
-	size_t gravityWindows() const {return gravityWindowsCount_;}
 
 	/**
 	 * @brief Adds an IMU measurement.
@@ -134,8 +115,7 @@ public:
 	 */
 	void addPose(double stamp, const rtabmap::Transform & pose);
 
-	/// Forgets the poses and the IMU samples. The gravity estimate is kept: it is the
+	/// Forgets the poses and the IMU samples.
-	/// sensor's.
 	void reset();
 
 	/**
@@ -160,22 +140,13 @@ public:
 private:
 	// orientation: of the base frame in the IMU's world frame; acceleration: of the base in
 	// that frame, gravity removed
-	// specificForce: in that frame too, including the reaction to gravity (see
+	void addSample(double stamp, const Eigen::Quaterniond & orientation, const Eigen::Vector3d & acceleration);
-	// hasAcceleration)
-	void addSample(double stamp, const Eigen::Quaterniond & orientation,
-			const Eigen::Vector3d & specificForce, bool hasAcceleration);
-	// Gravity estimation, from the IMU's own specific force and angular velocity norm
-	void estimateGravity(double stamp, const Eigen::Vector3d & specificForce, double angularVelocity);
 
 	struct Sample
 	{
 		Eigen::Quaterniond orientation;
-		// Kept with gravity, which is removed when used: the estimate can change
+		Eigen::Vector3d acceleration;
-		Eigen::Vector3d specificForce;
-		bool hasAcceleration;
 	};
-	// Acceleration of a sample, gravity removed (null without acceleration)
-	Eigen::Vector3d accelerationOf(const Sample & sample) const;
 	Eigen::Quaterniond orientationAt(double stamp) const;
 	Eigen::Vector3d accelerationAt(double stamp) const;
 	void integrate(double from, double to, const Eigen::Quaterniond & rotation,
@@ -195,20 +166,7 @@ private:
 	double maxPoseInterval_;
 	double velocityWindow_;
 	double gravity_;
-	bool gravityEstimated_;
 	std::map<double, Sample> samples_;
-
-	// Gravity estimation: the samples of the current window, and the gravity measured over
-	// the last quiet windows
-	struct StillSample
-	{
-		double stamp;
-		Eigen::Vector3d specificForce; // in the IMU frame
-		double angularVelocity;        // norm
-	};
-	std::vector<StillSample> stillWindow_;
-	std::vector<double> gravityWindows_;
-	size_t gravityWindowsCount_;
 	// Recent odometry poses, to estimate the velocity from (see velocityWindow)
 	std::map<double, rtabmap::Transform> poses_;
 

corelib/include/rtabmap/core/Parameters.h

@@ -489,7 +489,7 @@ class RTABMAP_CORE_EXPORT Parameters
 
     RTABMAP_PARAM(Optimizer, Baseline,           double, 0.075,   "When doing bundle adjustment with RGB-D data (mono camera + depth), set a fake baseline (m) so the BA backend treats depth as stereo disparity. Applies to all BA-capable backends (g2o, GTSAM, Ceres). Set to 0 to keep the problem mono (depth observations are ignored). For real stereo data the baseline in the calibration (Tx) is used directly.");
     RTABMAP_PARAM(Optimizer, PixelVariance,     double, 1.0,     "Pixel variance used on the u/v axes of every bundle adjustment reprojection edge. Applies to all BA-capable backends (g2o, GTSAM, Ceres). Should approximate the squared 1-sigma keypoint localization error in pixels. Set higher (e.g. 4-9) if features are noisy (low texture, motion blur, low light, or large detector scale). Set lower (e.g. 0.01-0.1) if features are sub-pixel refined (Lucas-Kanade tracking, parabolic peak interpolation). Intuition: the lower the pixel variance, the more the optimizer trusts the keypoint positions.");
-    RTABMAP_PARAM(Optimizer, DisparityVariance, double, 1.0,     "Disparity variance used on the disparity axis (u - u_right) of stereo / RGB-D bundle adjustment edges (g2o, GTSAM, Ceres). Defaults to the same value as PixelVariance for backward compatibility. The lower it is, the more the optimizer trusts the depth. Set higher (e.g. 2-4) if the depth is noisier than the features' u/v precision (stereo block matchers / SGM at long range, narrow-baseline low-resolution stereo). Set lower (e.g. 0.01-0.1) if it is more accurate (ToF / LiDAR-fused depth, measured rather than triangulated), or around 0.1-0.5 for wide-baseline high-resolution stereo.");
+    RTABMAP_PARAM(Optimizer, DisparityVariance, double, 1.0,     "Disparity variance used on the disparity axis (u - u_right) of stereo / RGB-D bundle adjustment edges. Applies to all BA-capable backends (g2o, GTSAM, Ceres). Defaults to the same value as PixelVariance for backward compatibility. Set higher (e.g. 2-4) if your depth source is noisier than your feature detector's u/v precision (typical for stereo block matchers / SGM at long range). Set lower (e.g. 0.01-0.1) if your depth source is more accurate than the u/v detector (typical for ToF / LiDAR-fused depth where range is measured directly rather than triangulated). Intuition: the lower the disparity variance, the more the optimizer trusts the depth measurements. Geometric note: wider baseline and/or higher image resolution improve a block matcher's effective disparity precision (larger disparity magnitudes and finer sub-pixel refinement), so wide-baseline high-resolution stereo pairs can usually afford a lower disparity variance (e.g. 0.1-0.5); narrow-baseline low-resolution pairs should keep it higher (e.g. 1-4).");
     RTABMAP_PARAM(Optimizer, RobustKernelDelta, double, 8,       "Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Applies to all BA-capable backends (g2o, GTSAM, Ceres). Observations with chi2 over this threshold will be ignored in the second optimization pass.");
 
     RTABMAP_PARAM(GTSAM, Optimizer,       int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");
@@ -512,8 +512,8 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Odom, KalmanProcessNoise,     float, 0.001, "Process noise covariance value.");
     RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01,  "Process measurement covariance value.");
     RTABMAP_PARAM(Odom, GuessMotion,            bool, true,   "Guess next transformation from the last motion computed.");
-    RTABMAP_PARAM(Odom, ImuGravity,             float, 9.80665, uFormat("Gravity magnitude (m/s^2) removed from the IMU linear acceleration (used with \"%s\" > 0). Standard gravity by default. Set it to what the accelerometer reads at rest to compensate for its scale error, or for another gravity than Earth's. 0: estimated from the IMU while it is still (gyro and accelerometer quiet over 0.5 s), with standard gravity until then: the robot should then be perfectly still for a moment (e.g., at start), and a steady acceleration without rotation would be taken for gravity.", kOdomGuessSmoothingDelay().c_str()));
+    RTABMAP_PARAM(Odom, ImuGravity,             float, 9.80665, uFormat("Gravity magnitude (m/s^2) removed from the IMU linear acceleration (used with \"%s\" > 0). Standard gravity by default. Set it to what the accelerometer reads at rest to compensate for its scale error, or for another gravity than Earth's.", kOdomGuessSmoothingDelay().c_str()));
-    RTABMAP_PARAM(Odom, GuessSmoothingDelay,    float, 0,     uFormat("Guess smoothing delay (s). The velocity is averaged over the last transforms up to this delay, for a smoother velocity prediction. The last velocity is used directly if \"%s\" is set or the delay is below the odometry rate. With an IMU giving orientation and linear acceleration, a delay > 0 also enables the IMU acceleration: the velocity is the displacement over this delay corrected by the acceleration measured since, so it is not delayed, and the motion guess and lidar deskewing (\"%s\") integrate the acceleration from it (~0.5 s recommended). With 0, the IMU only gives the orientation. Without IMU, the average is delayed by half this delay: useful for platforms with inertia, high frame rates, or lidar deskewing, where the velocity noise would feed back into the next poses.", kOdomFilteringStrategy().c_str(), kOdomDeskewing().c_str()));
+    RTABMAP_PARAM(Odom, GuessSmoothingDelay,    float, 0,     uFormat("Guess smoothing delay (s). Estimated velocity is averaged based on last transforms up to this maximum delay. This can help to get smoother velocity prediction. Last velocity computed is used directly if \"%s\" is set or the delay is below the odometry rate. With an IMU giving orientation and linear acceleration, a delay > 0 also enables the IMU acceleration: the velocity is then the displacement over this delay corrected by the acceleration measured since, so that it is the velocity at the last frame rather than a delayed average, and the motion guess and lidar deskewing (\"%s\") integrate the acceleration from it. Recommended (~0.5 s) with an IMU. With 0, the IMU only gives the orientation. Without IMU, the average is delayed by half this delay: it can make sense for platforms with inertia (e.g., cars at high speed, where one bad frame would otherwise change the predicted velocity a lot), for high frame rates, or when the velocity is used for lidar deskewing, where its noise would feed back into the next poses.", kOdomFilteringStrategy().c_str(), kOdomDeskewing().c_str()));
     RTABMAP_PARAM(Odom, KeyFrameThr,            float, 0.3,   "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
     RTABMAP_PARAM(Odom, VisKeyFrameThr,         int, 150,     "[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
     RTABMAP_PARAM(Odom, ScanKeyFrameThr,        float, 0.9,   "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
@@ -529,6 +529,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(OdomF2M, ScanMaxSize,         int, 2000,    "[Geometry] Maximum local scan map size.");
     RTABMAP_PARAM(OdomF2M, ScanSubtractRadius,  float, 0.05,  "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
     RTABMAP_PARAM(OdomF2M, ScanSubtractAngle,   float, 45,    uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str());
+    RTABMAP_PARAM(OdomF2M, ScanGravity,         bool, false,  uFormat("[Geometry] With an IMU, keep the local scan map aligned with gravity: the scan keyframes of the local map are kept in a local graph, with their registration links and a gravity link from the IMU orientation at each of them (weighted by \"%s\"), optimized when a keyframe is added (the oldest one fixed). The map is then reassembled from the keyframes' clouds at their optimized poses, so roll and pitch don't drift. Requires g2o or GTSAM.", kOptimizerGravitySigma().c_str()));
     RTABMAP_PARAM(OdomF2M, ScanRange,           float, 0,     "[Geometry] Distance Range used to filter points of local map (when > 0). 0 means local map is updated using time and not range.");
     RTABMAP_PARAM(OdomF2M, ValidDepthRatio,     float, 0.75,  "If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map.");
 #if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)

corelib/include/rtabmap/core/odometry/OdometryF2M.h

@@ -82,7 +82,22 @@ private:
 	Signature * map_;
 	Signature * lastFrame_;
 	int lastFrameOldestNewId_;
-	std::vector<std::pair<pcl::PointCloud<pcl::PointXYZINormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
+	// The scan keyframes the local scan map is assembled from
+	struct ScanKeyFrame
+	{
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud; // in odometry frame, at pose
+		pcl::IndicesPtr indices; // points kept in the map (not subtracted), all if empty
+		int id = 0;
+		Transform pose;          // odometry pose of the keyframe
+		Transform gravity;       // IMU orientation at the keyframe (null if not available)
+		Link link;               // registration from the previous keyframe (not set on the first)
+	};
+	std::vector<ScanKeyFrame> scansBuffer_;
+	pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembleScanMap() const;
+	bool alignScanKeyFramesWithGravity();
+	bool scanGravity_;
+	Optimizer * scanOptimizer_;
+	int scanKeyFrameSeq_;
 
 	std::map<int, std::map<int, FeatureBA> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>
 	std::map<int, Transform> bundlePoses_;

corelib/src/ImuMotionPredictor.cpp

@@ -26,9 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
 #include <rtabmap/core/ImuMotionPredictor.h>
-#include <rtabmap/utilite/ULogger.h>
 
-#include <algorithm>
 #include <vector>
 
 namespace rtabmap {
@@ -37,29 +35,14 @@ namespace rtabmap {
 // bounded while no odometry pose comes to trim it.
 static const double kMaxBufferDuration = 10.0;
 
-// Gravity estimation (see the class description). The IMU is still over a window if its
-// angular velocity stays under kStillMaxAngularVelocity and the standard deviation of its
-// specific force, on every axis, under kStillMaxAccelerationStd. Those are above the noise
-// and vibrations of an IMU on a robot at rest (e.g., ~0.1 m/s^2 on a drone on the ground,
-// against ~1 m/s^2 in flight), and well under what any acceleration that matters would
-// cause. The gravity is the median over the last kGravityWindows quiet windows.
-static const double kStillWindow = 0.5;              // s
-static const double kStillMaxAngularVelocity = 0.05; // rad/s
-static const double kStillMaxAccelerationStd = 0.15; // m/s^2
-static const size_t kStillMinSamples = 5;
-static const size_t kGravityWindows = 100;
-static const double kStandardGravity = 9.80665;
-
 ImuMotionPredictor::ImuMotionPredictor(double maxPoseInterval, double velocityWindow, double gravity) :
 	maxPoseInterval_(maxPoseInterval),
 	velocityWindow_(velocityWindow),
-	gravity_(gravity > 0.0 ? gravity : kStandardGravity),
+	gravity_(gravity),
-	gravityEstimated_(gravity <= 0.0),
+	integratedStartIsFinal_(false),
-	gravityWindowsCount_(0),
 	poseStamp_(0.0),
 	velocity_(Eigen::Vector3d::Zero()),
-	worldToOdom_(Eigen::Quaterniond::Identity()),
+	worldToOdom_(Eigen::Quaterniond::Identity())
-	integratedStartIsFinal_(false)
 {
 }
 
@@ -80,93 +63,21 @@ void ImuMotionPredictor::addImu(double stamp, const IMU & imu)
 			imu.localTransform().getQuaterniond();
 
 	const cv::Vec3d & f = imu.linearAcceleration();
-	const bool hasAcceleration = (f[0] != 0.0 || f[1] != 0.0 || f[2] != 0.0) &&
+	Eigen::Vector3d acceleration = Eigen::Vector3d::Zero();
-			(imu.linearAccelerationCovariance().empty() || imu.linearAccelerationCovariance().at<double>(0,0) != -1.0);
+	if((f[0] != 0.0 || f[1] != 0.0 || f[2] != 0.0) &&
-	if(hasAcceleration && gravityEstimated_)
+	   (imu.linearAccelerationCovariance().empty() || imu.linearAccelerationCovariance().at<double>(0,0) != -1.0))
 	{
-		const cv::Vec3d & w = imu.angularVelocity();
+		// The specific force includes the reaction to gravity, up in the world frame
-		estimateGravity(stamp, Eigen::Vector3d(f[0], f[1], f[2]), Eigen::Vector3d(w[0], w[1], w[2]).norm());
+		acceleration = worldToImu * Eigen::Vector3d(f[0], f[1], f[2]) - Eigen::Vector3d(0, 0, gravity_);
 	}
-	// The specific force includes the reaction to gravity, up in the world frame: it is
+	addSample(stamp, worldToImu * baseToImu.inverse(), acceleration);
-	// removed when used (accelerationOf())
-	addSample(stamp, worldToImu * baseToImu.inverse(),
-			hasAcceleration ? Eigen::Vector3d(worldToImu * Eigen::Vector3d(f[0], f[1], f[2])) : Eigen::Vector3d::Zero(),
-			hasAcceleration);
 }
 
-void ImuMotionPredictor::estimateGravity(double stamp, const Eigen::Vector3d & specificForce, double angularVelocity)
+void ImuMotionPredictor::addSample(double stamp, const Eigen::Quaterniond & orientation, const Eigen::Vector3d & acceleration)
-{
-	if(!stillWindow_.empty() && stamp < stillWindow_.back().stamp)
-	{
-		// Out of order
-		stillWindow_.clear();
-	}
-	if(!stillWindow_.empty() && stamp - stillWindow_.front().stamp >= kStillWindow)
-	{
-		// The window is complete (windows don't overlap: each sample counts once)
-		if(stillWindow_.size() >= kStillMinSamples)
-		{
-			Eigen::Vector3d mean = Eigen::Vector3d::Zero();
-			double maxAngularVelocity = 0.0;
-			for(size_t i=0; i<stillWindow_.size(); ++i)
-			{
-				mean += stillWindow_[i].specificForce;
-				maxAngularVelocity = std::max(maxAngularVelocity, stillWindow_[i].angularVelocity);
-			}
-			mean /= double(stillWindow_.size());
-			Eigen::Vector3d variance = Eigen::Vector3d::Zero();
-			for(size_t i=0; i<stillWindow_.size(); ++i)
-			{
-				variance += (stillWindow_[i].specificForce - mean).cwiseAbs2();
-			}
-			variance /= double(stillWindow_.size());
-			if(maxAngularVelocity < kStillMaxAngularVelocity &&
-			   variance.maxCoeff() < kStillMaxAccelerationStd*kStillMaxAccelerationStd)
-			{
-				// Not accelerating: the mean specific force is gravity, whatever the
-				// attitude (the norm of the mean, not the mean of the norms, which the
-				// noise would bias up)
-				gravityWindows_.push_back(mean.norm());
-				if(gravityWindows_.size() > kGravityWindows)
-				{
-					gravityWindows_.erase(gravityWindows_.begin());
-				}
-				++gravityWindowsCount_;
-				std::vector<double> sorted = gravityWindows_;
-				std::nth_element(sorted.begin(), sorted.begin() + sorted.size()/2, sorted.end());
-				const double gravity = sorted[sorted.size()/2];
-				if(gravityWindowsCount_ == 1)
-				{
-					UINFO("IMU gravity estimated at %f m/s^2 (the IMU is still)", gravity);
-				}
-				else
-				{
-					UDEBUG("IMU gravity estimated at %f m/s^2 (%d still windows)", gravity, (int)gravityWindowsCount_);
-				}
-				if(gravity != gravity_)
-				{
-					gravity_ = gravity;
-					// The accelerations integrated changed
-					integrated_.clear();
-				}
-			}
-		}
-		stillWindow_.clear();
-	}
-	StillSample sample;
-	sample.stamp = stamp;
-	sample.specificForce = specificForce;
-	sample.angularVelocity = angularVelocity;
-	stillWindow_.push_back(sample);
-}
-
-void ImuMotionPredictor::addSample(double stamp, const Eigen::Quaterniond & orientation,
-		const Eigen::Vector3d & specificForce, bool hasAcceleration)
 {
 	Sample sample;
 	sample.orientation = orientation.normalized();
-	sample.specificForce = specificForce;
+	sample.acceleration = acceleration;
-	sample.hasAcceleration = hasAcceleration;
 	samples_[stamp] = sample;
 	if(!integrated_.empty() && stamp <= integrated_.rbegin()->first)
 	{
@@ -261,7 +172,6 @@ void ImuMotionPredictor::addPose(double stamp, const rtabmap::Transform & pose)
 void ImuMotionPredictor::reset()
 {
 	samples_.clear();
-	stillWindow_.clear();
 	poses_.clear();
 	integrated_.clear();
 	pose_.setNull();
@@ -377,7 +287,7 @@ void ImuMotionPredictor::updateIntegration() const
 		Integrated next;
 		// Same segment as in integrate(), from the previous sample ti to this one:
 		// D(ti+1) = D(ti) + V(ti)*dt + dt^2*(2*a(ti) + a(ti+1))/6, V(ti+1) = V(ti) + dt*(a(ti) + a(ti+1))/2
-		next.acceleration = worldToOdom_ * accelerationOf(iter->second);
+		next.acceleration = worldToOdom_ * iter->second.acceleration;
 		next.position = previous.second.position + previous.second.velocity * dt +
 				dt * dt * (2.0 * previous.second.acceleration + next.acceleration) / 6.0;
 		next.velocity = previous.second.velocity + dt * (previous.second.acceleration + next.acceleration) / 2.0;
@@ -401,30 +311,20 @@ Eigen::Quaterniond ImuMotionPredictor::orientationAt(double stamp) const
 	return before->second.orientation.slerp(ratio, after->second.orientation);
 }
 
-Eigen::Vector3d ImuMotionPredictor::accelerationOf(const Sample & sample) const
-{
-	if(!sample.hasAcceleration)
-	{
-		return Eigen::Vector3d::Zero();
-	}
-	return sample.specificForce - Eigen::Vector3d(0, 0, gravity_);
-}
-
 Eigen::Vector3d ImuMotionPredictor::accelerationAt(double stamp) const
 {
 	std::map<double, Sample>::const_iterator after = samples_.lower_bound(stamp);
 	if(after == samples_.end())
 	{
-		return accelerationOf(samples_.rbegin()->second);
+		return samples_.rbegin()->second.acceleration;
 	}
 	if(after == samples_.begin() || after->first == stamp)
 	{
-		return accelerationOf(after->second);
+		return after->second.acceleration;
 	}
 	std::map<double, Sample>::const_iterator before = std::prev(after);
 	const double ratio = (stamp - before->first) / (after->first - before->first);
-	const Eigen::Vector3d accelerationBefore = accelerationOf(before->second);
+	return before->second.acceleration + ratio * (after->second.acceleration - before->second.acceleration);
-	return accelerationBefore + ratio * (accelerationOf(after->second) - accelerationBefore);
 }
 
 void ImuMotionPredictor::integrate(double from, double to, const Eigen::Quaterniond & rotation,

corelib/src/odometry/OdometryF2M.cpp

@@ -79,6 +79,9 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	validDepthRatio_(Parameters::defaultOdomF2MValidDepthRatio()),
 	pointToPlaneK_(Parameters::defaultIcpPointToPlaneK()),
 	pointToPlaneRadius_(Parameters::defaultIcpPointToPlaneRadius()),
+	scanGravity_(Parameters::defaultOdomF2MScanGravity()),
+	scanOptimizer_(0),
+	scanKeyFrameSeq_(0),
 	map_(new Signature(-1)),
 	lastFrame_(new Signature(1)),
 	lastFrameOldestNewId_(0),
@@ -109,6 +112,31 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), pointToPlaneK_);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), pointToPlaneRadius_);
+	Parameters::parse(parameters, Parameters::kOdomF2MScanGravity(), scanGravity_);
+	if(scanGravity_)
+	{
+		const Optimizer::Type type = Optimizer::isAvailable(Optimizer::kTypeG2O)?Optimizer::kTypeG2O:
+				Optimizer::isAvailable(Optimizer::kTypeGTSAM)?Optimizer::kTypeGTSAM:Optimizer::kTypeUndef;
+		if(type == Optimizer::kTypeUndef)
+		{
+			UWARN("\"%s\" is enabled but neither g2o nor GTSAM is available, the local scan map is "
+					"not aligned with gravity.", Parameters::kOdomF2MScanGravity().c_str());
+			scanGravity_ = false;
+		}
+		else
+		{
+			// Gravity links weighted by Optimizer/GravitySigma
+			scanOptimizer_ = Optimizer::create(type, parameters);
+			if(scanOptimizer_->gravitySigma() <= 0.0f)
+			{
+				UWARN("\"%s\" is enabled but \"%s\" is 0, the local scan map is not aligned with gravity.",
+						Parameters::kOdomF2MScanGravity().c_str(), Parameters::kOptimizerGravitySigma().c_str());
+				delete scanOptimizer_;
+				scanOptimizer_ = 0;
+				scanGravity_ = false;
+			}
+		}
+	}
 
 	UASSERT(bundleMaxFrames_ >= 0);
 	ParametersMap bundleParameters = parameters;
@@ -182,6 +210,7 @@ OdometryF2M::~OdometryF2M()
 	delete map_;
 	delete lastFrame_;
 	delete sba_;
+	delete scanOptimizer_;
 	delete regPipeline_;
 	UDEBUG("");
 }
@@ -196,6 +225,7 @@ void OdometryF2M::reset(const Transform & initialPose)
 	*lastFrame_ = Signature(1);
 	*map_ = Signature(-1);
 	scansBuffer_.clear();
+	scanKeyFrameSeq_ = 0;
 	bundleWordReferences_.clear();
 	bundlePoses_.clear();
 	bundleLinks_.clear();
@@ -205,6 +235,73 @@ void OdometryF2M::reset(const Transform & initialPose)
 	lastFrameOldestNewId_ = 0;
 }
 
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr OdometryF2M::assembleScanMap() const
+{
+	// As when the map is trimmed: the oldest keyframe is added whole (what it overlapped
+	// is no longer in the map), the others only with the points they added.
+	pcl::PointCloud<pcl::PointXYZINormal>::Ptr map(new pcl::PointCloud<pcl::PointXYZINormal>);
+	for(const ScanKeyFrame & keyFrame : scansBuffer_)
+	{
+		if(&keyFrame != &scansBuffer_.front() && keyFrame.indices->size())
+		{
+			pcl::PointCloud<pcl::PointXYZINormal> tmp;
+			pcl::copyPointCloud(*keyFrame.cloud, *keyFrame.indices, tmp);
+			*map += tmp;
+		}
+		else
+		{
+			*map += *keyFrame.cloud;
+		}
+	}
+	return map;
+}
+
+bool OdometryF2M::alignScanKeyFramesWithGravity()
+{
+	// Local graph of the keyframes of the scan map: their registration links, and a
+	// gravity link at each one with an IMU orientation. The oldest one is fixed, so as the
+	// map moves on, its orientation converges to the IMU's instead of keeping the first
+	// keyframe's (and the drift since) forever.
+	if(!scanOptimizer_ || scansBuffer_.size() < 2)
+	{
+		return false;
+	}
+	std::map<int, Transform> poses;
+	std::multimap<int, Link> links;
+	int gravityLinks = 0;
+	for(const ScanKeyFrame & keyFrame : scansBuffer_)
+	{
+		poses.insert(std::make_pair(keyFrame.id, keyFrame.pose));
+		if(keyFrame.link.isValid() && poses.find(keyFrame.link.from()) != poses.end())
+		{
+			links.insert(std::make_pair(keyFrame.link.from(), keyFrame.link));
+		}
+		if(!keyFrame.gravity.isNull())
+		{
+			links.insert(std::make_pair(keyFrame.id, Link(keyFrame.id, keyFrame.id, Link::kGravity, keyFrame.gravity)));
+			++gravityLinks;
+		}
+	}
+	if(gravityLinks == 0 || links.size() < poses.size()-1+gravityLinks)
+	{
+		return false;
+	}
+	const std::map<int, Transform> optimized = scanOptimizer_->optimize(scansBuffer_.front().id, poses, links);
+	if(optimized.size() != poses.size())
+	{
+		UWARN("Optimization of the local scan map with gravity failed, it is kept as is.");
+		return false;
+	}
+	for(ScanKeyFrame & keyFrame : scansBuffer_)
+	{
+		const Transform & pose = optimized.at(keyFrame.id);
+		const Transform correction = pose * keyFrame.pose.inverse();
+		keyFrame.cloud = util3d::transformPointCloud(keyFrame.cloud, correction);
+		keyFrame.pose = pose;
+	}
+	return true;
+}
+
 // return not null transform if odometry is correctly computed
 Transform OdometryF2M::computeTransform(
 		SensorData & data,
@@ -231,6 +328,13 @@ Transform OdometryF2M::computeTransform(
 		}
 	}
 
+	// IMU orientation at the frame, for the gravity links of the local scan map
+	Transform scanImuT;
+	if(scanOptimizer_ && !imus().empty())
+	{
+		scanImuT = Transform::getTransform(imus(), data.stamp());
+	}
+
 	RegistrationInfo regInfo;
 	int nFeatures = 0;
 
@@ -1162,7 +1266,18 @@ Transform OdometryF2M::computeTransform(
 								copyPointCloud(*normals, *mapCloudNormals);
 
 							} else {
-								scansBuffer_.push_back(std::make_pair(frameCloudNormals, frameCloudNormalsIndices));
+								ScanKeyFrame keyFrame;
+								keyFrame.cloud = frameCloudNormals;
+								keyFrame.indices = frameCloudNormalsIndices;
+								keyFrame.id = ++scanKeyFrameSeq_;
+								keyFrame.pose = newFramePose;
+								keyFrame.gravity = scanImuT;
+								if(!scansBuffer_.empty())
+								{
+									keyFrame.link = Link(scansBuffer_.back().id, keyFrame.id, Link::kNeighbor,
+											scansBuffer_.back().pose.inverse()*newFramePose, regInfo.covariance.inv());
+								}
+								scansBuffer_.push_back(keyFrame);
 
 								//remove points if too big
 								UDEBUG("scansBuffer=%d, mapSize=%d newPoints=%d maxPoints=%d",
@@ -1180,31 +1295,31 @@ Transform OdometryF2M::computeTransform(
 									int i = int(scansBuffer_.size())-1;
 									for(; i>=0; --i)
 									{
-										int pointsToAdd = scansBuffer_[i].second->size()?scansBuffer_[i].second->size():scansBuffer_[i].first->size();
+										int pointsToAdd = scansBuffer_[i].indices->size()?scansBuffer_[i].indices->size():scansBuffer_[i].cloud->size();
 										if((int)mapCloudNormals->size() + pointsToAdd > scanMaximumMapSize_ ||
 										   i == 0)
 										{
-											*mapCloudNormals += *scansBuffer_[i].first;
+											*mapCloudNormals += *scansBuffer_[i].cloud;
 											break;
 										}
 										else
 										{
-											if(scansBuffer_[i].second->size())
+											if(scansBuffer_[i].indices->size())
 											{
 												pcl::PointCloud<pcl::PointXYZINormal> tmp;
-												pcl::copyPointCloud(*scansBuffer_[i].first, *scansBuffer_[i].second, tmp);
+												pcl::copyPointCloud(*scansBuffer_[i].cloud, *scansBuffer_[i].indices, tmp);
 												*mapCloudNormals += tmp;
 											}
 											else
 											{
-												*mapCloudNormals += *scansBuffer_[i].first;
+												*mapCloudNormals += *scansBuffer_[i].cloud;
 											}
 										}
 									}
 									// remove old clouds
 									if(i > 0)
 									{
-										std::vector<std::pair<pcl::PointCloud<pcl::PointXYZINormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
+										std::vector<ScanKeyFrame> scansTmp(scansBuffer_.size()-i);
 										int oi = 0;
 										for(; i<(int)scansBuffer_.size(); ++i)
 										{
@@ -1217,19 +1332,28 @@ Transform OdometryF2M::computeTransform(
 								else
 								{
 									// just append the last cloud
-									if(scansBuffer_.back().second->size())
+									if(scansBuffer_.back().indices->size())
 									{
 										pcl::PointCloud<pcl::PointXYZINormal> tmp;
-										pcl::copyPointCloud(*scansBuffer_.back().first, *scansBuffer_.back().second, tmp);
+										pcl::copyPointCloud(*scansBuffer_.back().cloud, *scansBuffer_.back().indices, tmp);
 										*mapCloudNormals += tmp;
 									}
 									else
 									{
-										*mapCloudNormals += *scansBuffer_.back().first;
+										*mapCloudNormals += *scansBuffer_.back().cloud;
 									}
 								}
 							}
 
+							if(scanMapMaxRange_ <= 0 && alignScanKeyFramesWithGravity())
+							{
+								// The keyframes moved: the map is reassembled from them, and this
+								// frame (the newest keyframe) follows its optimized pose.
+								mapCloudNormals = assembleScanMap();
+								newFramePose = scansBuffer_.back().pose;
+								output = this->getPose().inverse() * newFramePose;
+							}
+
 							if(mapScan.is2d())
 							{
 								Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
@@ -1493,7 +1617,13 @@ Transform OdometryF2M::computeTransform(
 						if (scanMapMaxRange_ > 0 ){
 							UINFO("Local map will be updated using range instead of time with range threshold set at %f", scanMapMaxRange_);
 						} else {
-							scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
+							ScanKeyFrame keyFrame;
+							keyFrame.cloud = mapCloudNormals;
+							keyFrame.indices = pcl::IndicesPtr(new std::vector<int>);
+							keyFrame.id = ++scanKeyFrameSeq_;
+							keyFrame.pose = newFramePose;
+							keyFrame.gravity = scanImuT;
+							scansBuffer_.push_back(keyFrame);
 						}
 						if(lastFrame_->sensorData().laserScanRaw().is2d())
 						{

corelib/test/CMakeLists.txt

@@ -42,7 +42,6 @@ set(corelib_test_sources
 	test_link.cpp                       #Link.h
 	test_optimizer.cpp                  #Optimizer.h
 	test_gps.cpp                        #GPS.h
-	test_parameters.cpp                 #Parameters.h
 	test_imu.cpp                        #IMU.h
 	test_imufilter.cpp                  #IMUFilter.h
 	test_imumotionpredictor.cpp         #ImuMotionPredictor.h

corelib/test/test_imumotionpredictor.cpp

@@ -1,7 +1,3 @@
-// M_PI on MSVC (must come before any header including <cmath>)
-#ifndef _USE_MATH_DEFINES
-#define _USE_MATH_DEFINES
-#endif
 #include <gtest/gtest.h>
 
 #include <cmath>
@@ -41,19 +37,15 @@ rtabmap::IMU measuredImu(const Eigen::Quaterniond & worldToImu,
 
 /// IMU measurements at 200 Hz over [from, to] of an IMU at the base origin, oriented and
 /// accelerating (in its world frame) as given. Without accelerometer, it measures no
-/// linear acceleration at all. The stamps are on the 200 Hz grid (from must be too).
+/// linear acceleration at all.
 void addSamples(ImuMotionPredictor & predictor, double from, double to,
 		const std::function<Eigen::Quaterniond(double)> & orientation,
 		const std::function<Eigen::Vector3d(double)> & acceleration,
 		bool withAccelerometer = true)
 {
-	// An integer over 200 rather than from + i*0.005: with FMA (e.g., -march=x86-64-v3),
+	for(int i=0; from + i*0.005 <= to + 1e-9; ++i)
-	// -0.05 + 10*0.005 gives -1.7e-18, not 0, putting that sample on the wrong side of
-	// a step at t=0.
-	const double first = std::round(from * 200.0);
-	for(int i=0; (first + i) / 200.0 <= to + 1e-9; ++i)
 	{
-		const double t = (first + i) / 200.0;
+		const double t = from + i*0.005;
 		rtabmap::IMU imu = measuredImu(orientation(t), acceleration(t), predictor.gravity(), rtabmap::Transform::getIdentity());
 		if(!withAccelerometer)
 		{
@@ -375,86 +367,3 @@ TEST(ImuMotionPredictor, ignores_the_acceleration_without_a_velocity_window)
 	EXPECT_NEAR(predictor.velocity().x(), 0.5*a*0.1, 1e-6);
 	EXPECT_NEAR(predictor.predict(0.2).x(), 0.5*a*0.01 + 0.5*a*0.1*0.1, 1e-6);
 }
-
-namespace {
-
-/// An IMU tilted by 0.1 rad in pitch whose accelerometer reads `measuredGravity` at rest,
-/// with a vibration of the given amplitude (m/s^2) on every axis and the given angular
-/// velocity (rad/s, about z).
-rtabmap::IMU tiltedImu(double t, double measuredGravity, double vibration = 0.05, double angularVelocity = 0.0)
-{
-	const Eigen::Quaterniond worldToImu(Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitY()));
-	const Eigen::Vector3d f = worldToImu.inverse() * Eigen::Vector3d(0, 0, measuredGravity) +
-			vibration * Eigen::Vector3d(std::sin(t*331.0), std::sin(t*457.0), std::sin(t*563.0));
-	return rtabmap::IMU(cv::Vec4d(worldToImu.x(), worldToImu.y(), worldToImu.z(), worldToImu.w()), cv::Mat::eye(3,3,CV_64FC1),
-			cv::Vec3d(0, 0, angularVelocity), cv::Mat::eye(3,3,CV_64FC1),
-			cv::Vec3d(f.x(), f.y(), f.z()), cv::Mat::eye(3,3,CV_64FC1),
-			rtabmap::Transform::getIdentity());
-}
-
-}  // namespace
-
-TEST(ImuMotionPredictor, estimates_the_gravity_while_the_imu_is_still)
-{
-	// An accelerometer reading 9.55 m/s^2 at rest, as the netherdrone's does.
-	ImuMotionPredictor predictor(1.0, 0.5, 0.0);
-	EXPECT_TRUE(predictor.isGravityEstimated());
-	EXPECT_DOUBLE_EQ(predictor.gravity(), 9.80665) << "standard gravity until estimated";
-
-	for(int i=0; i<100; ++i) // 0.5 s at 200 Hz: the first window is not complete yet
-	{
-		predictor.addImu(i*0.005, tiltedImu(i*0.005, 9.55));
-	}
-	EXPECT_EQ(predictor.gravityWindows(), 0u);
-	EXPECT_DOUBLE_EQ(predictor.gravity(), 9.80665);
-
-	for(int i=100; i<=400; ++i)
-	{
-		predictor.addImu(i*0.005, tiltedImu(i*0.005, 9.55));
-	}
-	EXPECT_GE(predictor.gravityWindows(), 3u);
-	EXPECT_NEAR(predictor.gravity(), 9.55, 0.005) << "whatever the attitude";
-
-	// At rest, it doesn't look like falling anymore.
-	predictor.addPose(1.5, rtabmap::Transform::getIdentity());
-	predictor.addPose(2.0, rtabmap::Transform::getIdentity());
-	EXPECT_NEAR(predictor.predict(2.0).z(), 0.0, 1e-3);
-	for(int i=401; i<=460; ++i)
-	{
-		predictor.addImu(i*0.005, tiltedImu(i*0.005, 9.55));
-	}
-	EXPECT_NEAR(predictor.predict(2.3).z(), 0.0, 2e-3);
-
-	// The estimate is the sensor's: a reset keeps it.
-	predictor.reset();
-	EXPECT_NEAR(predictor.gravity(), 9.55, 0.005);
-}
-
-TEST(ImuMotionPredictor, does_not_take_a_moving_imu_for_a_still_one)
-{
-	// Turning (centripetal acceleration, gravity moving between the axes) or vibrating
-	// as in flight: not still, standard gravity is kept.
-	ImuMotionPredictor turning(1.0, 0.5, 0.0);
-	ImuMotionPredictor vibrating(1.0, 0.5, 0.0);
-	for(int i=0; i<=400; ++i)
-	{
-		turning.addImu(i*0.005, tiltedImu(i*0.005, 9.55, 0.05, 0.5));
-		vibrating.addImu(i*0.005, tiltedImu(i*0.005, 9.55, 1.0));
-	}
-	EXPECT_EQ(turning.gravityWindows(), 0u);
-	EXPECT_DOUBLE_EQ(turning.gravity(), 9.80665);
-	EXPECT_EQ(vibrating.gravityWindows(), 0u);
-	EXPECT_DOUBLE_EQ(vibrating.gravity(), 9.80665);
-}
-
-TEST(ImuMotionPredictor, keeps_the_gravity_it_is_given)
-{
-	ImuMotionPredictor predictor;
-	for(int i=0; i<=400; ++i)
-	{
-		predictor.addImu(i*0.005, tiltedImu(i*0.005, 9.55));
-	}
-	EXPECT_FALSE(predictor.isGravityEstimated());
-	EXPECT_EQ(predictor.gravityWindows(), 0u);
-	EXPECT_DOUBLE_EQ(predictor.gravity(), 9.80665);
-}

corelib/test/test_odometry.cpp

@@ -12,6 +12,7 @@
 #include <opencv2/imgcodecs.hpp>
 #include <memory>
 #include <rtabmap/core/IMU.h>
+#include <rtabmap/core/Optimizer.h>
 #include <rtabmap/utilite/UStl.h>
 #include <cmath>
 #include <limits>
@@ -793,3 +794,56 @@ TEST(OdometryTest, InitialOrientationIsTheImuOrientationAtTheFirstFrame)
 	}
 	EXPECT_NEAR(given->getPose().theta(), 1.0, 1e-6);
 }
+
+TEST(OdometryTest, ScanMapRealignsWithGravity)
+{
+	// Odometry starts with a roll 5 degrees off (an initial pose given with a rotation is
+	// not overridden by the IMU), while the IMU says the sensor is level. Without gravity in
+	// the local scan map, the error stays; with it, the map's keyframes are pulled toward the
+	// IMU's gravity and the odometry's roll converges.
+	if(!Optimizer::isAvailable(Optimizer::kTypeG2O) && !Optimizer::isAvailable(Optimizer::kTypeGTSAM))
+	{
+		GTEST_SKIP() << "Requires g2o or GTSAM";
+	}
+	Trajectory moving;
+	moving.yaw = 0.3;
+	moving.acceleration = 1.0;
+	const double initialRoll = 5.0 * M_PI / 180.0;
+	auto finalRoll = [&](bool scanGravity)
+	{
+		ParametersMap parameters;
+		parameters.insert(ParametersPair(Parameters::kRegStrategy(), "1"));
+		parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0.1"));
+		parameters.insert(ParametersPair(Parameters::kIcpPointToPlane(), "true"));
+		parameters.insert(ParametersPair(Parameters::kIcpPointToPlaneK(), "10"));
+		parameters.insert(ParametersPair(Parameters::kIcpMaxTranslation(), "0"));
+		parameters.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), "0.5"));
+		parameters.insert(ParametersPair(Parameters::kOdomScanKeyFrameThr(), "0.95"));
+		parameters.insert(ParametersPair(Parameters::kOdomF2MScanGravity(), scanGravity?"true":"false"));
+		// Gravity links with Optimizer/GravitySigma's default (0.3 rad)
+		std::unique_ptr<Odometry> odometry(Odometry::create(parameters));
+		odometry->reset(Transform(0, 0, 0, float(initialRoll), 0, float(moving.yaw)));
+		double imuStamp = 0.0;
+		Transform pose;
+		for(int i=1; i<=30; ++i)
+		{
+			const double stamp = 0.1*i;
+			for(; imuStamp <= stamp + kSweep + 0.005; imuStamp += 0.005)
+			{
+				SensorData imu(makeImu(moving, imuStamp), 0, imuStamp);
+				odometry->process(imu);
+			}
+			SensorData data(makeSweep(moving, stamp), cv::Mat(), cv::Mat(), CameraModel(), i, stamp);
+			OdometryInfo info;
+			pose = odometry->process(data, &info);
+			EXPECT_FALSE(pose.isNull()) << "frame " << i << " not registered (scan gravity " << scanGravity << ")";
+		}
+		float roll, pitch, yaw;
+		pose.getEulerAngles(roll, pitch, yaw);
+		return std::fabs(roll);
+	};
+	const double withoutGravity = finalRoll(false);
+	const double withGravity = finalRoll(true);
+	EXPECT_NEAR(withoutGravity, initialRoll, 0.01) << "without gravity, the initial error should stay";
+	EXPECT_LT(withGravity, initialRoll * 0.3) << "with gravity, the roll should converge to the IMU's";
+}

corelib/test/test_parameters.cpp

@@ -1,20 +0,0 @@
-#include <gtest/gtest.h>
-#include <rtabmap/core/Parameters.h>
-
-using rtabmap::Parameters;
-using rtabmap::ParametersMap;
-
-TEST(Parameters, descriptions_are_shorter_than_1024_characters)
-{
-	// Long descriptions don't fit in the GUI's tooltips nor in a ROS parameter listing:
-	// keep them short, the details belong in the documentation. 1024 was also the size
-	// beyond which uFormat(), which formats most of them, aborted on Windows.
-	const ParametersMap & parameters = Parameters::getDefaultParameters();
-	ASSERT_FALSE(parameters.empty());
-	for(ParametersMap::const_iterator iter = parameters.begin(); iter != parameters.end(); ++iter)
-	{
-		const std::string description = Parameters::getDescription(iter->first);
-		EXPECT_LT(description.size(), 1024u) << iter->first << " has a description of "
-				<< description.size() << " characters: shorten it";
-	}
-}

guilib/src/PreferencesDialog.cpp

@@ -1517,6 +1517,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->doubleSpinBox_odom_f2m_scanRadius->setObjectName(Parameters::kOdomF2MScanSubtractRadius().c_str());
 	_ui->doubleSpinBox_odom_f2m_scanAngle->setObjectName(Parameters::kOdomF2MScanSubtractAngle().c_str());
 	_ui->doubleSpinBox_odom_f2m_scanRange->setObjectName(Parameters::kOdomF2MScanRange().c_str());
+	_ui->odom_f2m_scanGravity->setObjectName(Parameters::kOdomF2MScanGravity().c_str());
 	_ui->odom_f2m_validDepthRatio->setObjectName(Parameters::kOdomF2MValidDepthRatio().c_str());
 	_ui->odom_f2m_initDepthFactor->setObjectName(Parameters::kOdomF2MInitDepthFactor().c_str());
 	_ui->odom_f2m_bundleStrategy->setObjectName(Parameters::kOdomF2MBundleAdjustment().c_str());

guilib/src/ui/preferencesDialog.ui

@@ -16794,7 +16794,7 @@ With <0, the length is estimated once for each unique marker, then re-used fo
                       <item row="8" column="1">
                        <widget class="QLabel" name="label_520">
                         <property name="text">
-                         <string>Guess smoothing delay (s). The velocity is averaged over the last transforms up to this delay, for a smoother velocity prediction. The last velocity is used directly if a filtering strategy is set or the delay is below the odometry rate. With an IMU giving orientation and linear acceleration, a delay &gt; 0 also enables the IMU acceleration: the velocity is the displacement over this delay corrected by the acceleration measured since, so it is not delayed, and the motion guess and lidar deskewing integrate the acceleration from it (~0.5 s recommended). With 0, the IMU only gives the orientation. Without IMU, the average is delayed by half this delay: useful for platforms with inertia, high frame rates, or lidar deskewing, where the velocity noise would feed back into the next poses.</string>
+                         <string>Guess smoothing delay (s). Estimated velocity is averaged based on last transforms up to this maximum delay. This can help to get smoother velocity prediction. Last velocity computed is used directly if a filtering strategy is set or the delay is below the odometry rate. With an IMU giving orientation and linear acceleration, a delay &gt; 0 also enables the IMU acceleration: the velocity is then corrected by the acceleration measured since, so that it is the velocity at the last frame rather than a delayed average, and the motion guess and lidar deskewing integrate the acceleration from it. Recommended (~0.5 s) with an IMU; with 0, the IMU only gives the orientation. Without IMU, the average is delayed by half this delay: it can make sense for platforms with inertia (e.g., cars at high speed, where one bad frame would otherwise change the predicted velocity a lot), for high frame rates, or when the velocity is used for lidar deskewing, where its noise would feed back into the next poses.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -16866,9 +16866,6 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                       </item>
                       <item row="9" column="0">
                        <widget class="QDoubleSpinBox" name="odom_imu_gravity">
-                        <property name="specialValueText">
-                         <string>Auto</string>
-                        </property>
                         <property name="suffix">
                          <string> m/s²</string>
                         </property>
@@ -16889,7 +16886,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                       <item row="9" column="1">
                        <widget class="QLabel" name="label_odom_imu_gravity">
                         <property name="text">
-                         <string>Gravity magnitude removed from the IMU linear acceleration (used with a guess smoothing delay &gt; 0 above). Standard gravity by default. Set it to what the accelerometer reads at rest to compensate for its scale error, or for another gravity than Earth's. Auto (0): estimated from the IMU while it is still, with standard gravity until then: the robot should then be perfectly still for a moment (e.g., at start).</string>
+                         <string>Gravity magnitude removed from the IMU linear acceleration (used with a guess smoothing delay &gt; 0 above). Standard gravity by default. Set it to what the accelerometer reads at rest to compensate for its scale error, or for another gravity than Earth's.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -16955,7 +16952,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="9" column="1">
+                             <item row="10" column="1">
                               <widget class="QLabel" name="label_357">
                                <property name="text">
                                 <string>[Visual] Local bundle adjustment. See Optimizer panel. This will not work if Optical Flow correspondences strategy is selected in Visual Registration panel.</string>
@@ -16968,7 +16965,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="10" column="1">
+                             <item row="11" column="1">
                               <widget class="QLabel" name="label_358">
                                <property name="text">
                                 <string>[Visual] Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).</string>
@@ -16997,7 +16994,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="13" column="1">
+                             <item row="14" column="1">
                               <widget class="QLabel" name="label_762">
                                <property name="text">
                                 <string>[Visual] Maximum keyframes per feature for bundle adjustment. 0 means not limit.</string>
@@ -17010,14 +17007,14 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="14" column="0">
+                             <item row="15" column="0">
                               <widget class="QCheckBox" name="odom_f2m_bundleUpdateFeatureMapOnAllFrames">
                                <property name="text">
                                 <string/>
                                </property>
                               </widget>
                              </item>
-                             <item row="12" column="1">
+                             <item row="13" column="1">
                               <widget class="QLabel" name="label_761">
                                <property name="text">
                                 <string>[Visual] To create a new keyframe with bundle adjustment, a minimum motion (in pixels) can be required. The motion is computed by the average distance between inliers of the previous keyframe and new frame.</string>
@@ -17079,7 +17076,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="11" column="0">
+                             <item row="12" column="0">
                               <widget class="QDoubleSpinBox" name="odom_f2m_gravitySigma">
                                <property name="suffix">
                                 <string/>
@@ -17101,14 +17098,14 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="10" column="0">
+                             <item row="11" column="0">
                               <widget class="QSpinBox" name="odom_f2m_bundleMaxFrames">
                                <property name="maximum">
                                 <number>999999</number>
                                </property>
                               </widget>
                              </item>
-                             <item row="12" column="0">
+                             <item row="13" column="0">
                               <widget class="QDoubleSpinBox" name="odom_f2m_bundleMinMotion">
                                <property name="suffix">
                                 <string> pixels</string>
@@ -17146,7 +17143,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="11" column="1">
+                             <item row="12" column="1">
                               <widget class="QLabel" name="label_598">
                                <property name="text">
                                 <string>[Visual] Gravity sigma used for bundle adjustment (&lt;0, use same value than Optimizer/GravitySigma parameter)</string>
@@ -17159,7 +17156,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="8" column="1">
+                             <item row="9" column="1">
                               <widget class="QLabel" name="label_765">
                                <property name="text">
                                 <string>[Visual] Depth factor used to initialize depth of features without depth. Depth = Factor * fx.</string>
@@ -17172,7 +17169,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="7" column="1">
+                             <item row="8" column="1">
                               <widget class="QLabel" name="label_524">
                                <property name="text">
                                 <string>[Visual] If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map.</string>
@@ -17211,7 +17208,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="7" column="0">
+                             <item row="8" column="0">
                               <widget class="QDoubleSpinBox" name="odom_f2m_validDepthRatio">
                                <property name="suffix">
                                 <string/>
@@ -17243,7 +17240,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="13" column="0">
+                             <item row="14" column="0">
                               <widget class="QSpinBox" name="odom_f2m_bundleMaxKeyFramesPerFeature">
                                <property name="maximum">
                                 <number>999999</number>
@@ -17282,7 +17279,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="14" column="1">
+                             <item row="15" column="1">
                               <widget class="QLabel" name="label_7631">
                                <property name="text">
                                 <string>[Visual] Update 3D local feature map on every frames with bundle adjustment. Recommended if Vis/DepthAsMask=false and Mem/UseOdomFeatures=true so that features without depth are better triangulated on every frame (not only on keyframes). If disabled, the feature map is updated only when a new keyframe is added (legacy approach).</string>
@@ -17295,7 +17292,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="9" column="0">
+                             <item row="10" column="0">
                               <widget class="QComboBox" name="odom_f2m_bundleStrategy">
                                <property name="sizeAdjustPolicy">
                                 <enum>QComboBox::AdjustToContents</enum>
@@ -17335,7 +17332,7 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
-                             <item row="8" column="0">
+                             <item row="9" column="0">
                               <widget class="QDoubleSpinBox" name="odom_f2m_initDepthFactor">
                                <property name="suffix">
                                 <string/>
@@ -17357,6 +17354,26 @@ With &lt;0, the length is estimated once for each unique marker, then re-used fo
                                </property>
                               </widget>
                              </item>
+                             <item row="7" column="0">
+                              <widget class="QCheckBox" name="odom_f2m_scanGravity">
+                               <property name="text">
+                                <string/>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="7" column="1">
+                              <widget class="QLabel" name="label_odom_f2m_scanGravity">
+                               <property name="text">
+                                <string>[Geometry] With an IMU, keep the local scan map aligned with gravity: the scan keyframes of the local map are kept in a local graph, with their registration links and a gravity link from the IMU orientation at each of them (weighted by the optimizer's gravity sigma), optimized when a keyframe is added. The map is then reassembled from the keyframes at their optimized poses, so roll and pitch don't drift. Requires g2o or GTSAM.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
                             </layout>
                            </item>
                           </layout>

utilite/src/UConversion.cpp

@@ -338,8 +338,7 @@ std::string uFormatv (const char *fmt, va_list args)
 
         // Try to vsnprintf into our buffer.
 #ifdef _MSC_VER
-    	// _TRUNCATE: return -1 if it doesn't fit (count=size would call the invalid parameter handler and abort)
+    	int needed = vsnprintf_s(buf, size, size, fmt, argsTmp);
-    	int needed = vsnprintf_s(buf, size, _TRUNCATE, fmt, argsTmp);
 #else
     	int needed = vsnprintf (buf, size, fmt, argsTmp);
 #endif

utilite/test/test_uconversion.cpp

@@ -201,14 +201,3 @@ TEST(UConversionTest, UFormat)
     EXPECT_NE(result.find("42"), std::string::npos);
 }
 
-TEST(UConversionTest, UFormatLongerThanItsFirstBuffer)
-{
-    // Over the 1024 bytes uFormat first tries: formatted whole, not truncated (MSVC's
-    // checked vsnprintf_s used to abort the process instead).
-    const std::string longText(3000, 'a');
-    std::string result = uFormat("%s %d", longText.c_str(), 42);
-    EXPECT_EQ(result, longText + " 42");
-    result = uFormat("%s", std::string(1023, 'b').c_str());
-    EXPECT_EQ(result, std::string(1023, 'b'));
-}
-