simplified parameters

corelib/test/test_imumotionpredictor.cpp

@@ -352,3 +352,18 @@ TEST(ImuMotionPredictor, holds_the_acceleration_at_the_pose_until_a_newer_sample
 	}
 	EXPECT_NEAR(reference.predict(0.3).x(), incremental.predict(0.3).x(), 1e-9);
 }
+
+TEST(ImuMotionPredictor, ignores_the_acceleration_without_a_velocity_window)
+{
+	// From rest with a constant 2 m/s^2: with a window of 0, the velocity is the one of the
+	// last interval and is kept constant, as without IMU.
+	const double a = 2.0;
+	ImuMotionPredictor predictor(1.0, 0.0);
+	addSamples(predictor, -0.05, 0.3,
+			[](double) { return Eigen::Quaterniond::Identity(); },
+			[&](double t) { return Eigen::Vector3d(t < 0.0 ? 0.0 : a, 0, 0); });
+	predictor.addPose(0.0, pose(Eigen::Vector3d::Zero(), 0));
+	predictor.addPose(0.1, pose(Eigen::Vector3d(0.5*a*0.01, 0, 0), 0));
+	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);
+}

corelib/test/test_util3d.cpp

@@ -2114,3 +2114,41 @@ TEST(Util3dTest, DeskewValidScan) {
     // empty scan
     EXPECT_TRUE(util3d::deskew(LaserScan(), inputStamp, velocity).empty());
 }
+
+TEST(Util3dTest, DeskewWithMotion) {
+    // Three points measured at y=0, 1 s before, at and 1 s after the scan stamp, while the
+    // base moves along y as (t-stamp)^2: a motion no constant velocity describes.
+    cv::Mat data = cv::Mat::zeros(1, 3, CV_32FC(5));
+    float * dataPtr = (float*)data.data;
+    dataPtr[0] = 1;  dataPtr[4] = -1;
+    dataPtr[5] = 1;  dataPtr[9] = 0;
+    dataPtr[10] = 1; dataPtr[14] = 1;
+    LaserScan scan(data, 3, 10.0f, LaserScan::kXYZIT);
+    const double inputStamp = 1000.0;
+
+    std::vector<double> stamps;
+    auto motion = [&](double stamp) {
+        stamps.push_back(stamp);
+        const double dt = stamp - inputStamp;
+        return Transform(0.0, dt*dt, 0.0, 0.0, 0.0, 0.0);
+    };
+
+    // Asked for every point time, each point is moved by its own pose
+    LaserScan result = util3d::deskew(scan, inputStamp, motion);
+    ASSERT_EQ(result.size(), 3);
+    EXPECT_EQ(stamps.size(), 3u);
+    EXPECT_FLOAT_EQ(result.field(0, 1), 1.0f);
+    EXPECT_FLOAT_EQ(result.field(1, 1), 0.0f);
+    EXPECT_FLOAT_EQ(result.field(2, 1), 1.0f);
+
+    // With slerp, only the ends are asked for, the middle point is interpolated between them
+    stamps.clear();
+    result = util3d::deskew(scan, inputStamp, motion, true);
+    ASSERT_EQ(result.size(), 3);
+    EXPECT_EQ(stamps.size(), 2u);
+    EXPECT_FLOAT_EQ(result.field(1, 1), 1.0f);
+
+    // A failing motion, or none
+    EXPECT_TRUE(util3d::deskew(scan, inputStamp, [](double) { return Transform(); }).empty());
+    EXPECT_TRUE(util3d::deskew(scan, inputStamp, std::function<Transform(double)>()).empty());
+}