Measure/Modules/MeasureNodeAlgTest/match3d/WheelFactor.cpp

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

#include "WheelFactor.h"
#include <pcl/io/pcd_io.h>

WheelsPredictor::WheelsPredictor() {
    valid_ = false;
    length_ = 0;
    wheelBase_ = 0;
    width_ = 0;
    diff_ = 0;
}

WheelsPredictor::~WheelsPredictor() = default;

void WheelsPredictor::reset() {
    valid_ = false;
}

bool WheelsPredictor::update(pcl::PointCloud<pcl::PointXYZ>::Ptr src, double cx, double cy, double theta,
                             double width, double wheelbase) {
    if (src->empty()) {
        LOG(WARNING) << "cloud is empty,cloud size:" + std::to_string(src->size());
        return false;
    }

    LOG(INFO) << "input params -----> cx: " << cx << ", cy: " << cy << ", theta: "
              << theta << ", width: " << width << ", wheelbase: " << wheelbase;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud(*src, *cloud);
    //绕原点摆正
    float minx = 1e8, maxx = -1e8, miny = 1e8, maxy = -1e8;
    float t_minx = 1e8, t_maxx = -1e8;
    for (auto &point: cloud->points) {
        pcl::PointXYZ pt(point);
        t_minx = MIN(t_minx, point.x);
        t_maxx = MAX(t_maxx, point.x);
        point.x = pt.x * cos(-theta) - pt.y * sin(-theta);
        point.y = pt.x * sin(-theta) + pt.y * cos(-theta);
        minx = MIN(minx, point.x);
        maxx = MAX(maxx, point.x);
        miny = MIN(miny, point.y);
        maxy = MAX(maxy, point.y);
    }
    float car_y = (miny + maxy) / 2.f;
    LOG(INFO) << "befor trans -----> t_minx: " << t_minx << ", t_maxx: " << t_maxx;
    LOG(INFO) << "minx: " << minx << ", maxx: " << maxx << ", miny: " << miny << ", maxy: " << maxy;

    minx = 1e8, maxx = -1e8;
    for (auto &point:cloud->points) {
        if (point.y < car_y - 0.5 || point.y > car_y + 0.5) {
            continue;
        }
        minx = MIN(minx, point.x);
        maxx = MAX(maxx, point.x);
    }
    float car_x = (minx + maxx) / 2.;
    LOG(INFO) << "after only use body-----> minx: " << minx << ", maxx: " << maxx;

    length_ = maxy - miny;
    float x = cx * cos(-theta) - cy * sin(-theta);
    float y = cx * sin(-theta) + cy * cos(-theta);
    LOG(INFO) << "carx - wheel x = (" << car_x << " - " << x << ") = " << car_x - x;
    if (fabs(car_x - x) < 0.03) {
        diff_ = y - car_y;
        width_ = width;
        wheelBase_ = wheelbase;
        LOG(INFO) << "update diff: " << diff_ << ", width: " << width_ << ", wheelbase: " << wheelBase_;
        valid_ = true;
    } else {
        valid_ = false;
    }
    return valid_;
}

bool WheelsPredictor::predict(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double theta, PreResult &res) {
    if (!valid_) {
        return false;
    }
    float minx = 1e8, maxx = -1e8, miny = 1e8, maxy = -1e8;
    for (int i = 0; i < cloud->size(); ++i) {
        pcl::PointXYZ pt = cloud->points[i];
        float x = pt.x * cos(-theta) - pt.y * sin(-theta);
        float y = pt.x * sin(-theta) + pt.y * cos(-theta);
        cloud->points[i].x = x;
        cloud->points[i].y = y;
        if (x < minx) minx = x;
        if (x > maxx) maxx = x;
        if (y < miny) miny = y;
        if (y > maxy) maxy = y;
    }
    float car_x = (minx + maxx) / 2.;
    float car_y = (miny + maxy) / 2.;

    minx = 1e8, maxx = -1e8;
    for (int i = 0; i < cloud->size(); ++i) {
        pcl::PointXYZ pt = cloud->points[i];
        if (pt.y < car_y + 0.5 && pt.y > car_y - 0.5) {
            if (pt.x < minx) minx = pt.x;
            if (pt.x > maxx) maxx = pt.x;
        }
    }
    car_x = (minx + maxx) / 2.;

    float length = fabs(maxy - miny);
    if (fabs(length_ - length) < 0.1) {
        float x = car_x;
        float y = car_y + diff_;

        res.x = x * cos(theta) - y * sin(theta);
        res.y = x * sin(theta) + y * cos(theta);
        res.theta = theta;
        res.w = width_;
        res.wheelBase = wheelBase_;
        res.frontTheta = 0;
        return true;
    }
    return false;

}