fix(crane_world_model_publisher): estimateInitialVelocityが減速度引数を無視する不具合を修正

crane_world_model_publisher/include/crane_world_model_publisher/calibration/initial_velocity_fit.hpp

@@ -0,0 +1,65 @@
+// Copyright (c) 2025 ibis-ssl
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file or at
+// https://opensource.org/licenses/MIT.
+
+#ifndef CRANE_WORLD_MODEL_PUBLISHER__CALIBRATION__INITIAL_VELOCITY_FIT_HPP_
+#define CRANE_WORLD_MODEL_PUBLISHER__CALIBRATION__INITIAL_VELOCITY_FIT_HPP_
+
+#include <cmath>
+#include <cstddef>
+#include <vector>
+
+namespace crane
+{
+struct InitialVelocityFit
+{
+  double initial_velocity = 0.0;
+  // 固定傾きモデルの R²。傾きを固定しているので負にもなる。速度が全点同じ（ss_tot=0）なら 0
+  double r_squared = 0.0;
+  double residual_rms = 0.0;
+};
+
+/**
+ * @brief 減速度 deceleration を固定したモデル v(t) = v0 - deceleration * t で v0 を推定する
+ * @param time_points 軌道の最初の ROLLING 点を 0 とした時刻 [s]。キック時刻ではないので、
+ *        v0 は最初の ROLLING 点での速度になる
+ * @param velocities 各時刻の速度 [m/s]（位置 [m] の差分）
+ * @param deceleration 減速度 [m/s²]（正の値で減速）
+ * @return initial_velocity は残差平方和を最小にする mean(v + deceleration * t)。点が無ければ全て 0
+ */
+inline InitialVelocityFit fitInitialVelocityWithFixedDeceleration(
+  const std::vector<double> & time_points, const std::vector<double> & velocities,
+  double deceleration)
+{
+  InitialVelocityFit fit;
+  const size_t n = time_points.size();
+  if (n == 0) {
+    return fit;
+  }
+
+  double v0_sum = 0.0;
+  double velocity_sum = 0.0;
+  for (size_t i = 0; i < n; ++i) {
+    v0_sum += velocities[i] + deceleration * time_points[i];
+    velocity_sum += velocities[i];
+  }
+  fit.initial_velocity = v0_sum / n;
+  const double mean_velocity = velocity_sum / n;
+
+  double ss_res = 0.0;
+  double ss_tot = 0.0;
+  for (size_t i = 0; i < n; ++i) {
+    const double residual = velocities[i] + deceleration * time_points[i] - fit.initial_velocity;
+    ss_res += residual * residual;
+    const double deviation = velocities[i] - mean_velocity;
+    ss_tot += deviation * deviation;
+  }
+  fit.r_squared = (ss_tot > 0.0) ? (1.0 - ss_res / ss_tot) : 0.0;
+  fit.residual_rms = std::sqrt(ss_res / n);
+  return fit;
+}
+}  // namespace crane
+
+#endif  // CRANE_WORLD_MODEL_PUBLISHER__CALIBRATION__INITIAL_VELOCITY_FIT_HPP_

crane_world_model_publisher/include/crane_world_model_publisher/calibration/simple_ball_physics_optimizer.hpp

@@ -32,7 +32,7 @@ class SimpleBallPhysicsOptimizer
     bool is_chip_kick;                              // チップキックフラグ
     double fitting_r_squared;                       // フィッティングR²値
     double trajectory_duration;                     // 軌道継続時間 [s]
-    std::pair<double, double> confidence_interval;  // 信頼区間
+    std::pair<double, double> confidence_interval;  // v0 ± 1.96×残差RMS（v0 の信頼区間ではない）
   };
 
   /**
@@ -131,7 +131,7 @@ class SimpleBallPhysicsOptimizer
   auto optimizeGlobalDeceleration(const std::vector<TrajectoryData> & all_trajectories) -> double;
 
   /**
-   * @brief 個別軌道の初速度推定（線形回帰）
+   * @brief 個別軌道の初速度推定（減速度を固定した当てはめ）
    */
   auto estimateInitialVelocity(const TrajectoryData & trajectory, double deceleration)
     -> KickPowerVelocityPair;

crane_world_model_publisher/src/calibration/simple_ball_physics_optimizer.cpp

@@ -14,6 +14,8 @@
 #include <rclcpp/rclcpp.hpp>
 #include <regex>
 
+#include "crane_world_model_publisher/calibration/initial_velocity_fit.hpp"
+
 namespace crane
 {
 
@@ -392,7 +394,7 @@ auto SimpleBallPhysicsOptimizer::optimizeGlobalDeceleration(
 }
 
 auto SimpleBallPhysicsOptimizer::estimateInitialVelocity(
-  const TrajectoryData & trajectory, double /* deceleration */) -> KickPowerVelocityPair
+  const TrajectoryData & trajectory, double deceleration) -> KickPowerVelocityPair
 {
   KickPowerVelocityPair result;
   result.event_id = trajectory.event_id;
@@ -407,25 +409,15 @@ auto SimpleBallPhysicsOptimizer::estimateInitialVelocity(
     return result;
   }
 
-  // 速度vs時間の線形回帰: v(t) = v0 - deceleration * t
-  // ここで固定された減速度を使用して初速度を推定
-  auto [slope, intercept, r_squared] =
-    performLinearRegression(trajectory.time_points, trajectory.velocities);
-
-  result.estimated_initial_velocity = intercept;  // 切片 = 初速度
-  result.fitting_r_squared = r_squared;
-
-  // 信頼区間の計算
-  double velocity_std = 0.0;
-  for (size_t i = 0; i < trajectory.time_points.size(); ++i) {
-    double predicted_velocity = intercept + slope * trajectory.time_points[i];
-    double error = trajectory.velocities[i] - predicted_velocity;
-    velocity_std += error * error;
-  }
-  velocity_std = std::sqrt(velocity_std / trajectory.time_points.size());
+  // 減速度を固定して v0 だけを推定する（t の原点は最初の ROLLING 点）
+  const auto fit = fitInitialVelocityWithFixedDeceleration(
+    trajectory.time_points, trajectory.velocities, deceleration);
+  result.estimated_initial_velocity = fit.initial_velocity;
+  result.fitting_r_squared = fit.r_squared;
 
-  double confidence_margin = 1.96 * velocity_std;  // 95%信頼区間
-  result.confidence_interval = {intercept - confidence_margin, intercept + confidence_margin};
+  // 残差 RMS の ±1.96 倍。点数を反映しないので v0 の 95% 信頼区間ではなく、個々の点のばらつきの幅
+  const double margin = 1.96 * fit.residual_rms;
+  result.confidence_interval = {fit.initial_velocity - margin, fit.initial_velocity + margin};
 
   return result;
 }

crane_world_model_publisher/test/test_initial_velocity_fit.cpp

@@ -0,0 +1,103 @@
+// Copyright (c) 2025 ibis-ssl
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file or at
+// https://opensource.org/licenses/MIT.
+
+#include <gtest/gtest.h>
+
+#include <vector>
+
+#include "crane_world_model_publisher/calibration/initial_velocity_fit.hpp"
+
+namespace
+{
+// t = t_offset + 0, 0.1, ..., 0.9 [s] に、最初の点で v0 から decel で減速する速度列を作る
+struct Samples
+{
+  std::vector<double> t;
+  std::vector<double> v;
+};
+
+Samples makeLinear(double v0, double decel, double t_offset = 0.0)
+{
+  Samples s;
+  for (int i = 0; i < 10; ++i) {
+    const double t = t_offset + 0.1 * i;
+    s.t.push_back(t);
+    s.v.push_back(v0 - decel * (t - t_offset));
+  }
+  return s;
+}
+}  // namespace
+
+TEST(InitialVelocityFit, KnownDecelerationRecoversInitialVelocity)
+{
+  const auto s = makeLinear(3.0, 0.5);
+  const auto fit = crane::fitInitialVelocityWithFixedDeceleration(s.t, s.v, 0.5);
+  EXPECT_NEAR(fit.initial_velocity, 3.0, 1e-9);
+  EXPECT_NEAR(fit.r_squared, 1.0, 1e-9);
+  EXPECT_NEAR(fit.residual_rms, 0.0, 1e-9);
+}
+
+TEST(InitialVelocityFit, UsesGivenDecelerationInsteadOfFreeSlope)
+{
+  // 実際の傾きは 0.5 だが 0.3 で固定すると v0 = 3.0 - 0.2 * mean(t)（mean(t) = 0.45）
+  const auto s = makeLinear(3.0, 0.5);
+  const auto fit = crane::fitInitialVelocityWithFixedDeceleration(s.t, s.v, 0.3);
+  EXPECT_NEAR(fit.initial_velocity, 3.0 - 0.2 * 0.45, 1e-9);
+  EXPECT_LT(fit.r_squared, 1.0);
+  EXPECT_GT(fit.residual_rms, 0.0);
+}
+
+TEST(InitialVelocityFit, InitialVelocityIsSpeedAtTimeZero)
+{
+  // 同じ速度列でも t が 1.0 秒ずれていれば、v0 は t=0 まで外挿した decel * 1.0 だけ大きい
+  const auto s = makeLinear(3.0, 0.5, 1.0);
+  const auto fit = crane::fitInitialVelocityWithFixedDeceleration(s.t, s.v, 0.5);
+  EXPECT_NEAR(fit.initial_velocity, 3.5, 1e-9);
+}
+
+TEST(InitialVelocityFit, FewPoints)
+{
+  // 2 点: v0 = mean(2.0 + 0, 1.8 + 0.05) = 1.925、残差 ±0.075、ss_tot = 0.02
+  const auto two = crane::fitInitialVelocityWithFixedDeceleration({0.0, 0.1}, {2.0, 1.8}, 0.5);
+  EXPECT_NEAR(two.initial_velocity, 1.925, 1e-9);
+  EXPECT_NEAR(two.r_squared, 1.0 - 0.01125 / 0.02, 1e-9);
+  EXPECT_NEAR(two.residual_rms, 0.075, 1e-9);
+
+  // 1 点: ss_tot = 0 なので R² は 0
+  const auto one = crane::fitInitialVelocityWithFixedDeceleration({0.2}, {1.0}, 0.5);
+  EXPECT_NEAR(one.initial_velocity, 1.1, 1e-9);
+  EXPECT_DOUBLE_EQ(one.r_squared, 0.0);
+  EXPECT_NEAR(one.residual_rms, 0.0, 1e-9);
+
+  const auto none = crane::fitInitialVelocityWithFixedDeceleration({}, {}, 0.5);
+  EXPECT_DOUBLE_EQ(none.initial_velocity, 0.0);
+  EXPECT_DOUBLE_EQ(none.r_squared, 0.0);
+  EXPECT_DOUBLE_EQ(none.residual_rms, 0.0);
+}
+
+TEST(InitialVelocityFit, RSquaredBecomesNegativeWhenSlopeContradictsDeceleration)
+{
+  // 加速している軌道に正の減速度を当てはめると、平均値より悪いので R² < 0
+  const auto s = makeLinear(1.0, -0.5);
+  const auto fit = crane::fitInitialVelocityWithFixedDeceleration(s.t, s.v, 0.5);
+  EXPECT_LT(fit.r_squared, 0.0);
+}
+
+TEST(InitialVelocityFit, ConstantVelocityGivesZeroRSquared)
+{
+  const auto s = makeLinear(2.0, 0.0);
+
+  // 全点同じ速度だと ss_tot = 0 なので、当てはまりが完全でも R² は 0
+  const auto exact = crane::fitInitialVelocityWithFixedDeceleration(s.t, s.v, 0.0);
+  EXPECT_NEAR(exact.initial_velocity, 2.0, 1e-9);
+  EXPECT_DOUBLE_EQ(exact.r_squared, 0.0);
+  EXPECT_NEAR(exact.residual_rms, 0.0, 1e-9);
+
+  const auto decel = crane::fitInitialVelocityWithFixedDeceleration(s.t, s.v, 0.5);
+  EXPECT_NEAR(decel.initial_velocity, 2.0 + 0.5 * 0.45, 1e-9);
+  EXPECT_DOUBLE_EQ(decel.r_squared, 0.0);
+  EXPECT_GT(decel.residual_rms, 0.0);
+}