feat(multi_object_tracker): add velocity gate

[purewater0901]

Signed-off-by: yutaka [email protected]

Description

Add velocity gate to the multi object tracker, so that the tracker does not use measurement velocity to update the velocity. This is useful when we use Radar to estimate the velocity of the obstacle.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

[yukkysaito]

I preffer the variable name(has_large_velocity_deviation) is enable_velocity_measurement.
How about this?

Suggested change

	// pos x, pos y, yaw, vx depending on Pose output
	bool has_large_velocity_deviation = false;
	if (object.kinematics.has_twist) {
	Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state
	ekf_.getX(X_t);
	const double current_vx = X_t(IDX::VX);
	const double observed_vx = object.kinematics.twist_with_covariance.twist.linear.x;
	if (std::fabs(current_vx - observed_vx) > velocity_deviation_threshold_) {
	has_large_velocity_deviation = true;
	}
	}
	const int dim_y = has_large_velocity_deviation ? 3 : 4;
	// Decide dimension of measuement vector
	bool enable_velocity_measurement = false;
	if (object.kinematics.has_twist) {
	Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state
	ekf_.getX(X_t);
	const double current_vx = X_t(IDX::VX);
	const double observed_vx = object.kinematics.twist_with_covariance.twist.linear.x;
	if (std::fabs(current_vx - observed_vx) > velocity_deviation_threshold_) { // It has large velocity deviation
	enable_velocity_measurement = true;
	}
	}
	const int dim_y = enable_velocity_measurement ? 3 : 4; // pos x, pos y, yaw, vx depending on Pose output

[purewater0901]

done

[codecov]

Codecov Report

Merging #1146 (b6eb981) into main (1110bc4) will decrease coverage by 0.01%.
The diff coverage is 0.00%.

@@           Coverage Diff            @@
##            main   #1146      +/-   ##
========================================
- Coverage   9.06%   9.04%   -0.02%     
========================================
  Files       1034    1034              
  Lines      70256   70401     +145     
  Branches   14657   14650       -7     
========================================
  Hits        6370    6370              
- Misses     58422   58837     +415     
+ Partials    5464    5194     -270     

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	9.05% <0.00%> (+<0.01%)	⬆️	Carriedforward from 2c1987a

*This pull request uses carry forward flags. Click here to find out more.

Impacted Files	Coverage Δ
...analysis/src/control_performance_analysis_core.cpp	0.00% <0.00%> (ø)
...analysis/src/control_performance_analysis_node.cpp	0.00% <ø> (ø)
...ject_tracker/tracker/model/big_vehicle_tracker.hpp	0.00% <ø> (ø)
...t_tracker/tracker/model/normal_vehicle_tracker.hpp	0.00% <ø> (ø)
..._tracker/src/tracker/model/big_vehicle_tracker.cpp	0.00% <0.00%> (ø)
...acker/src/tracker/model/normal_vehicle_tracker.cpp	0.00% <0.00%> (ø)
map/lanelet2_extension/lib/query.cpp	9.52% <0.00%> (ø)
map/lanelet2_extension/lib/utilities.cpp	8.89% <0.00%> (ø)
common/osqp_interface/src/osqp_interface.cpp	34.35% <0.00%> (ø)
map/lanelet2_extension/lib/visualization.cpp	0.00% <0.00%> (ø)
... and 84 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update a075e60...b6eb981. Read the comment docs.

[yukkysaito]

LGTM