39 lines
1.0 KiB
C++
39 lines
1.0 KiB
C++
#pragma once
|
|
|
|
#include <vector>
|
|
#include <Eigen/Dense>
|
|
#include "car_nav_lite/types.hpp"
|
|
#include "car_nav_lite/fence_model.hpp"
|
|
|
|
namespace car_nav_lite {
|
|
|
|
class ConeUpdater {
|
|
public:
|
|
ConeUpdater();
|
|
|
|
// Process one scan frame: extract cone candidates, track across frames.
|
|
// confirmed_cones is the current known cone list (for dedup).
|
|
void update(const LaserScan& scan, const Pose2D& pose,
|
|
const FenceModel& fence,
|
|
const std::vector<Eigen::Vector2d>& known_cones);
|
|
|
|
// Returns newly confirmed cones and clears them from pending.
|
|
std::vector<Eigen::Vector2d> popConfirmed();
|
|
|
|
private:
|
|
struct Candidate {
|
|
Eigen::Vector2d center;
|
|
int hits = 0;
|
|
int age = 0;
|
|
};
|
|
std::vector<Candidate> candidates_;
|
|
|
|
static constexpr int CONFIRM_HITS = 3;
|
|
static constexpr double CLUSTER_DIST = 0.10;
|
|
static constexpr double MERGE_DIST = 0.25;
|
|
static constexpr double WALL_DIST = 0.15;
|
|
static constexpr int MIN_POINTS = 4;
|
|
};
|
|
|
|
} // namespace car_nav_lite
|