Measure/Modules/MeasureNode/velodyne_lidar/match3d/WheelFactor.h

//
// Created by zx on 23-9-20.
//

#pragma once

#include <sys/param.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include "defines.hpp"

class WheelsPredictor {
public:
    struct PreResult{
        double x;
        double y;
        double theta;
        double w;
        double wheelBase;
        double frontTheta;

        std::string log() {
            std::string log = ("x: " + std::to_string(x) +
                               ", y: " + std::to_string(y) +
                               ", theta: " + std::to_string(theta) +
                               ", w: " + std::to_string(w) +
                               ", wheelBase: " + std::to_string(wheelBase) +
                               ", frontTheta: " + std::to_string(frontTheta));
            return log;
        }
    };
public:
    WheelsPredictor();
    ~WheelsPredictor();

    void reset();

    bool update(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double cx, double cy, double theta,
                double width, double wheelbase);

    bool predict(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double theta, PreResult &res);

protected:
    bool valid_;
    double length_;
    double wheelBase_;
    double width_;
    double diff_;

};