From 5fc0e0b53584357bc37e3e6f628c45cb4a2db060 Mon Sep 17 00:00:00 2001 From: AndreiMoraru123 <81184255+AndreiMoraru123@users.noreply.github.com> Date: Sun, 20 Aug 2023 19:42:26 +0300 Subject: [PATCH] formatting --- Algo/tracker.cpp | 182 ++++++++++++++++++++++++---------------- Algo/tracker.hpp | 77 +++++++++++------ CMakeLists.txt | 2 +- Control/controller.cpp | 31 ++++--- Control/controller.hpp | 9 +- Objects/car.cpp | 144 ++++++++++++++++--------------- Objects/car.hpp | 48 +++++------ Objects/obstacle.cpp | 18 ++-- Objects/obstacle.hpp | 10 ++- Objects/parkingspot.cpp | 32 ++++--- Objects/parkingspot.hpp | 10 ++- Objects/road.cpp | 81 ++++++++++-------- Objects/road.hpp | 16 ++-- Scene/scene.cpp | 157 +++++++++++++++++----------------- Scene/scene.hpp | 20 +++-- Sensors/lidar.cpp | 53 +++++++----- Sensors/lidar.hpp | 10 ++- Sensors/sensors.hpp | 5 +- Toolkit/tools.cpp | 120 ++++++++++++++------------ Toolkit/tools.hpp | 27 ++++-- View/camera.cpp | 19 +++-- View/camera.hpp | 5 +- main.cpp | 34 ++++---- 23 files changed, 612 insertions(+), 498 deletions(-) diff --git a/Algo/tracker.cpp b/Algo/tracker.cpp index 33b5df8..3edba7a 100644 --- a/Algo/tracker.cpp +++ b/Algo/tracker.cpp @@ -6,15 +6,18 @@ #include Tracker::Tracker() { - x_ = Eigen::VectorXd(5); // initial state vector - P_ = Eigen::MatrixXd(5, 5); // initial covariance matrix - std_a_ = 10.0; // Process noise standard deviation longitudinal acceleration in m/s^2 - std_yawdd_ = 1.0; // Process noise standard deviation yaw acceleration in rad/s^2 - std_laspx_ = 5.0; // Laser measurement noise standard deviation position1 in m - std_laspy_ = 5.0; // Laser measurement noise standard deviation position2 in m + x_ = Eigen::VectorXd(5); // initial state vector + P_ = Eigen::MatrixXd(5, 5); // initial covariance matrix + std_a_ = 10.0; // Process noise standard deviation longitudinal acceleration + // in m/s^2 + std_yawdd_ = + 1.0; // Process noise standard deviation yaw acceleration in rad/s^2 + std_laspx_ = 5.0; // Laser measurement noise standard deviation position1 in m + std_laspy_ = 5.0; // Laser measurement noise standard deviation position2 in m std_radr_ = 0.3; // Radar measurement noise standard deviation radius in m - std_radphi_ = 0.03; // Radar measurement noise standard deviation angle in rad - std_radrd_ = 0.3; // Radar measurement noise standard deviation radius change in m/s + std_radphi_ = 0.03; // Radar measurement noise standard deviation angle in rad + std_radrd_ = + 0.3; // Radar measurement noise standard deviation radius change in m/s is_initialized_ = false; n_x_ = 5; n_aug_ = 7; @@ -37,20 +40,25 @@ void Tracker::ProcessMeasurement(MeasurementPackage meas_package) { if (!is_initialized_) { // Initializing differs depending on the first sensor that hits the cars if (meas_package.sensorType == MeasurementPackage::SensorType::LIDAR) { - x_ << meas_package.rawMeasurements[0], meas_package.rawMeasurements[1], 0, 0, 0; + x_ << meas_package.rawMeasurements[0], meas_package.rawMeasurements[1], 0, + 0, 0; P_ = Eigen::MatrixXd::Identity(5, 5); - P_(0,0) = std_laspx_ * std_laspx_; - P_(1,1) = std_laspy_ * std_laspy_; - } else if (meas_package.sensorType == MeasurementPackage::SensorType::RADAR) { - const double x = meas_package.rawMeasurements(0) * cos(meas_package.rawMeasurements(1)); - const double y = meas_package.rawMeasurements(0) * sin(meas_package.rawMeasurements(1)); + P_(0, 0) = std_laspx_ * std_laspx_; + P_(1, 1) = std_laspy_ * std_laspy_; + } else if (meas_package.sensorType == + MeasurementPackage::SensorType::RADAR) { + const double x = meas_package.rawMeasurements(0) * + cos(meas_package.rawMeasurements(1)); + const double y = meas_package.rawMeasurements(0) * + sin(meas_package.rawMeasurements(1)); const double v = meas_package.rawMeasurements(2); - x_ << x, y, v, meas_package.rawMeasurements(0), meas_package.rawMeasurements(2); + x_ << x, y, v, meas_package.rawMeasurements(0), + meas_package.rawMeasurements(2); P_ = Eigen::MatrixXd::Identity(5, 5); - P_ = P_.array() * (std_radr_* std_radr_); - P_(2,2) = std_radrd_ * std_radrd_; - P_(3,3) = std_radphi_ * std_radphi_; - P_(4,4) = std_radphi_ * std_radphi_; + P_ = P_.array() * (std_radr_ * std_radr_); + P_(2, 2) = std_radrd_ * std_radrd_; + P_(3, 3) = std_radphi_ * std_radphi_; + P_(4, 4) = std_radphi_ * std_radphi_; } time_us_ = meas_package.timeStamp; is_initialized_ = true; @@ -97,19 +105,25 @@ Eigen::MatrixXd Tracker::createAugmentedCovarianceMatrix() const { Eigen::MatrixXd P_aug = Eigen::MatrixXd(n_aug_, n_aug_); P_aug.setZero(n_aug_, n_aug_); P_aug.topLeftCorner(5, 5) = P_; - P_aug.bottomRightCorner(2, 2) << std_a_ * std_a_, 0, 0, std_yawdd_ * std_yawdd_; + P_aug.bottomRightCorner(2, 2) << std_a_ * std_a_, 0, 0, + std_yawdd_ * std_yawdd_; return P_aug; } -Eigen::MatrixXd Tracker::createAugmentedSigmaPoints(const Eigen::MatrixXd& A, const Eigen::VectorXd& x_aug) const { +Eigen::MatrixXd +Tracker::createAugmentedSigmaPoints(const Eigen::MatrixXd &A, + const Eigen::VectorXd &x_aug) const { Eigen::MatrixXd Xsig_aug = Eigen::MatrixXd(n_aug_, 2 * n_aug_ + 1); Xsig_aug.col(0) = x_aug; - Xsig_aug.middleCols(1, n_aug_) = (std::sqrt(lambda_ + n_aug_) * A.array()).colwise() + x_aug.array(); - Xsig_aug.middleCols(n_aug_ + 1, n_aug_) = (-std::sqrt(lambda_ + n_aug_) * A.array()).colwise() + x_aug.array(); + Xsig_aug.middleCols(1, n_aug_) = + (std::sqrt(lambda_ + n_aug_) * A.array()).colwise() + x_aug.array(); + Xsig_aug.middleCols(n_aug_ + 1, n_aug_) = + (-std::sqrt(lambda_ + n_aug_) * A.array()).colwise() + x_aug.array(); return Xsig_aug; } -Eigen::MatrixXd Tracker::predictSigmaPoints(const Eigen::MatrixXd& Xsig_aug, double delta_t) const { +Eigen::MatrixXd Tracker::predictSigmaPoints(const Eigen::MatrixXd &Xsig_aug, + double delta_t) const { Eigen::MatrixXd Xsig_pred = Eigen::MatrixXd(n_x_, 2 * n_aug_ + 1); @@ -125,34 +139,36 @@ Eigen::MatrixXd Tracker::predictSigmaPoints(const Eigen::MatrixXd& Xsig_aug, dou double px_p, py_p; if (fabs(yaw_rate) > 0.001) { - px_p = px + (v/yaw_rate) * (sin(yaw + yaw_rate*delta_t) - sin(yaw)); - py_p = py + (v/yaw_rate) * (cos(yaw) - cos(yaw + yaw_rate*delta_t)); + px_p = px + (v / yaw_rate) * (sin(yaw + yaw_rate * delta_t) - sin(yaw)); + py_p = py + (v / yaw_rate) * (cos(yaw) - cos(yaw + yaw_rate * delta_t)); } else { - px_p = px + v*delta_t*cos(yaw); - py_p = py + v*delta_t*sin(yaw); + px_p = px + v * delta_t * cos(yaw); + py_p = py + v * delta_t * sin(yaw); } double v_p = v; - double yaw_p = yaw + yaw_rate*delta_t; + double yaw_p = yaw + yaw_rate * delta_t; double yawd_p = yaw_rate; // add noise - px_p = px_p + 0.5*nu_a*delta_t*delta_t*cos(yaw); - py_p = py_p + 0.5*nu_a*delta_t*delta_t*sin(yaw); - v_p = v_p + nu_a*delta_t; - yaw_p = yaw_p + 0.5*nu_yawdd*delta_t*delta_t; - yawd_p = yawd_p + nu_yawdd*delta_t; - - Xsig_pred(0,i) = px_p; - Xsig_pred(1,i) = py_p; - Xsig_pred(2,i) = v_p; - Xsig_pred(3,i) = yaw_p; - Xsig_pred(4,i) = yawd_p; + px_p = px_p + 0.5 * nu_a * delta_t * delta_t * cos(yaw); + py_p = py_p + 0.5 * nu_a * delta_t * delta_t * sin(yaw); + v_p = v_p + nu_a * delta_t; + yaw_p = yaw_p + 0.5 * nu_yawdd * delta_t * delta_t; + yawd_p = yawd_p + nu_yawdd * delta_t; + + Xsig_pred(0, i) = px_p; + Xsig_pred(1, i) = py_p; + Xsig_pred(2, i) = v_p; + Xsig_pred(3, i) = yaw_p; + Xsig_pred(4, i) = yawd_p; } return Xsig_pred; } -Eigen::MatrixXd Tracker::calculatePredictedCovariance(const Eigen::MatrixXd& Xsig_pred, const Eigen::VectorXd& x) { +Eigen::MatrixXd +Tracker::calculatePredictedCovariance(const Eigen::MatrixXd &Xsig_pred, + const Eigen::VectorXd &x) { // Initialize predicted covariance matrix Eigen::MatrixXd P_pred = Eigen::MatrixXd::Zero(n_x_, n_x_); // Calculate predicted covariance matrix @@ -165,8 +181,8 @@ Eigen::MatrixXd Tracker::calculatePredictedCovariance(const Eigen::MatrixXd& Xsi return P_pred; } -void Tracker::UpdateLidar(const MeasurementPackage& meas_package) { - int n_z = 2; // Measurement dimension +void Tracker::UpdateLidar(const MeasurementPackage &meas_package) { + int n_z = 2; // Measurement dimension Eigen::MatrixXd Zsig = createSigmaPointsLidar(n_z); Eigen::VectorXd z_pred = predictMeasurementMeanLidar(Zsig, n_z); Eigen::MatrixXd S = predictMeasurementCovarianceLidar(Zsig, z_pred, n_z); @@ -176,7 +192,9 @@ void Tracker::UpdateLidar(const MeasurementPackage& meas_package) { } Eigen::MatrixXd Tracker::createSigmaPointsLidar(int n_z) { - Eigen::MatrixXd Zsig = Eigen::MatrixXd(n_z, 2 * n_aug_ + 1); // create matrix for sigma points in measurement space + Eigen::MatrixXd Zsig = Eigen::MatrixXd( + n_z, + 2 * n_aug_ + 1); // create matrix for sigma points in measurement space // transform sigma points into measurement space for (int i = 0; i < 2 * n_aug_ + 1; i++) { double p_x = Xsig_pred_(0, i); @@ -187,8 +205,9 @@ Eigen::MatrixXd Tracker::createSigmaPointsLidar(int n_z) { return Zsig; } -Eigen::VectorXd Tracker::predictMeasurementMeanLidar(const Eigen::MatrixXd& Zsig, int n_z) { - Eigen::VectorXd z_pred = Eigen::VectorXd(n_z); // mean predicted measurement +Eigen::VectorXd +Tracker::predictMeasurementMeanLidar(const Eigen::MatrixXd &Zsig, int n_z) { + Eigen::VectorXd z_pred = Eigen::VectorXd(n_z); // mean predicted measurement // calculate mean predicted measurement z_pred.fill(0.0); for (int i = 0; i < 2 * n_aug_ + 1; i++) { @@ -197,40 +216,45 @@ Eigen::VectorXd Tracker::predictMeasurementMeanLidar(const Eigen::MatrixXd& Zsig return z_pred; } -Eigen::MatrixXd Tracker::predictMeasurementCovarianceLidar(const Eigen::MatrixXd& Zsig, const Eigen::VectorXd& z_pred, int n_z) { - Eigen::MatrixXd S = Eigen::MatrixXd(n_z, n_z); // measurement covariance matrix +Eigen::MatrixXd Tracker::predictMeasurementCovarianceLidar( + const Eigen::MatrixXd &Zsig, const Eigen::VectorXd &z_pred, int n_z) { + Eigen::MatrixXd S = + Eigen::MatrixXd(n_z, n_z); // measurement covariance matrix // calculate measurement covariance matrix S S.fill(0.0); for (int i = 0; i < 2 * n_aug_ + 1; i++) { - Eigen::VectorXd z_diff = Zsig.col(i) - z_pred; // residual + Eigen::VectorXd z_diff = Zsig.col(i) - z_pred; // residual S = S + weights_(i) * z_diff * z_diff.transpose(); } - Eigen::MatrixXd R = Eigen::MatrixXd(n_z, n_z); // measurement noise covariance matrix - R << std_laspx_ * std_laspx_, 0, - 0, std_laspy_ * std_laspy_; + Eigen::MatrixXd R = + Eigen::MatrixXd(n_z, n_z); // measurement noise covariance matrix + R << std_laspx_ * std_laspx_, 0, 0, std_laspy_ * std_laspy_; S = S + R; return S; } -Eigen::MatrixXd Tracker::calculateCrossCorrelationLidar(Eigen::MatrixXd Zsig, const Eigen::VectorXd& z_pred, int n_z) { +Eigen::MatrixXd Tracker::calculateCrossCorrelationLidar( + Eigen::MatrixXd Zsig, const Eigen::VectorXd &z_pred, int n_z) { Eigen::MatrixXd Tc = Eigen::MatrixXd(n_x_, n_z); Tc.fill(0.0); for (int i = 0; i < 2 * n_aug_ + 1; i++) { - Eigen::VectorXd z_diff = Zsig.col(i) - z_pred; // residual - Eigen::VectorXd x_diff = Xsig_pred_.col(i) - x_; // state difference + Eigen::VectorXd z_diff = Zsig.col(i) - z_pred; // residual + Eigen::VectorXd x_diff = Xsig_pred_.col(i) - x_; // state difference Tc = Tc + weights_(i) * x_diff * z_diff.transpose(); } return Tc; } -void Tracker::updateStateMeanAndCovarianceLidar(Eigen::MatrixXd K, const Eigen::VectorXd& z_pred, const MeasurementPackage& meas_package, const Eigen::MatrixXd& S) { +void Tracker::updateStateMeanAndCovarianceLidar( + Eigen::MatrixXd K, const Eigen::VectorXd &z_pred, + const MeasurementPackage &meas_package, const Eigen::MatrixXd &S) { Eigen::VectorXd z_diff = meas_package.rawMeasurements - z_pred; x_ = x_ + K * z_diff; P_ = P_ - K * S * K.transpose(); } -void Tracker::UpdateRadar(const MeasurementPackage& meas_package) { - int n_z = 3; // Measurement dimension +void Tracker::UpdateRadar(const MeasurementPackage &meas_package) { + int n_z = 3; // Measurement dimension Eigen::MatrixXd Zsig = createSigmaPointsInMeasurementSpace(n_z); Eigen::VectorXd z_pred = predictMeasurementMean(Zsig, n_z); Eigen::MatrixXd S = predictMeasurementCovariance(Zsig, z_pred, n_z); @@ -264,47 +288,59 @@ Eigen::VectorXd Tracker::predictMeasurementMean(Eigen::MatrixXd Zsig, int n_z) { return z_pred; } -Eigen::MatrixXd Tracker::predictMeasurementCovariance(Eigen::MatrixXd Zsig, const Eigen::VectorXd& z_pred, int n_z) { +Eigen::MatrixXd +Tracker::predictMeasurementCovariance(Eigen::MatrixXd Zsig, + const Eigen::VectorXd &z_pred, int n_z) { Eigen::MatrixXd S = Eigen::MatrixXd(n_z, n_z); S.fill(0.0); for (int i = 0; i < 2 * n_aug_ + 1; i++) { Eigen::VectorXd z_diff = Zsig.col(i) - z_pred; - while (z_diff(1) > M_PI) z_diff(1) -= 2. * M_PI; - while (z_diff(1) < -M_PI) z_diff(1) += 2. * M_PI; + while (z_diff(1) > M_PI) + z_diff(1) -= 2. * M_PI; + while (z_diff(1) < -M_PI) + z_diff(1) += 2. * M_PI; S = S + weights_(i) * z_diff * z_diff.transpose(); } Eigen::MatrixXd R = Eigen::MatrixXd(n_z, n_z); - R << std_radr_ * std_radr_, 0, 0, - 0, std_radphi_ * std_radphi_, 0, - 0, 0, std_radrd_ * std_radrd_; - S = S + R; // Add measurement noise covariance matrix R to the measurement prediction covariance S + R << std_radr_ * std_radr_, 0, 0, 0, std_radphi_ * std_radphi_, 0, 0, 0, + std_radrd_ * std_radrd_; + S = S + R; // Add measurement noise covariance matrix R to the measurement + // prediction covariance S return S; } -Eigen::MatrixXd Tracker::calculateCrossCorrelation(Eigen::MatrixXd Zsig, const Eigen::VectorXd& z_pred, int n_z) { +Eigen::MatrixXd +Tracker::calculateCrossCorrelation(Eigen::MatrixXd Zsig, + const Eigen::VectorXd &z_pred, int n_z) { Eigen::MatrixXd Tc = Eigen::MatrixXd(n_x_, n_z); Tc.fill(0.0); for (int i = 0; i < 2 * n_aug_ + 1; i++) { Eigen::VectorXd x_diff = Xsig_pred_.col(i) - x_; - x_diff(3) -= 2.0 * M_PI * std::floor((x_diff(3) + M_PI) * (1.0 / (2.0 * M_PI))); + x_diff(3) -= + 2.0 * M_PI * std::floor((x_diff(3) + M_PI) * (1.0 / (2.0 * M_PI))); Eigen::VectorXd z_diff = Zsig.col(i) - z_pred; - z_diff(1) -= 2.0 * M_PI * std::floor((z_diff(1) + M_PI) * (1.0 / (2.0 * M_PI))); + z_diff(1) -= + 2.0 * M_PI * std::floor((z_diff(1) + M_PI) * (1.0 / (2.0 * M_PI))); Tc = Tc + weights_(i) * x_diff * z_diff.transpose(); } return Tc; } - -Eigen::MatrixXd Tracker::calculateKalmanGain(const Eigen::MatrixXd& Tc, const Eigen::MatrixXd& S) { +Eigen::MatrixXd Tracker::calculateKalmanGain(const Eigen::MatrixXd &Tc, + const Eigen::MatrixXd &S) { Eigen::MatrixXd K = Tc * S.inverse(); return K; } -void Tracker::updateStateMeanAndCovariance(Eigen::MatrixXd K, const Eigen::VectorXd& z_pred, const MeasurementPackage& meas_package, Eigen::MatrixXd& S) { +void Tracker::updateStateMeanAndCovariance( + Eigen::MatrixXd K, const Eigen::VectorXd &z_pred, + const MeasurementPackage &meas_package, Eigen::MatrixXd &S) { Eigen::VectorXd z = meas_package.rawMeasurements; Eigen::VectorXd z_diff = z - z_pred; - while (z_diff(1) > M_PI) z_diff(1) -= 2. * M_PI; - while (z_diff(1) < -M_PI) z_diff(1) += 2. * M_PI; + while (z_diff(1) > M_PI) + z_diff(1) -= 2. * M_PI; + while (z_diff(1) < -M_PI) + z_diff(1) += 2. * M_PI; x_ = x_ + K * z_diff; P_ = P_ - K * S * K.transpose(); } diff --git a/Algo/tracker.hpp b/Algo/tracker.hpp index 47e48f8..391fafe 100644 --- a/Algo/tracker.hpp +++ b/Algo/tracker.hpp @@ -5,9 +5,9 @@ #ifndef NFS_TRACKER_HPP #define NFS_TRACKER_HPP -#include -#include #include "../Sensors/sensors.hpp" +#include +#include class Tracker { public: @@ -15,46 +15,71 @@ class Tracker { virtual ~Tracker(); void ProcessMeasurement(MeasurementPackage meas_package); void Prediction(double delta_t); - void UpdateLidar(const MeasurementPackage& meas_package); - void UpdateRadar(const MeasurementPackage& meas_package); + void UpdateLidar(const MeasurementPackage &meas_package); + void UpdateRadar(const MeasurementPackage &meas_package); - bool is_initialized_; // Initially set to false, set to true in first call of ProcessMeasurement - Eigen::VectorXd x_; // state vector - Eigen::MatrixXd P_; // state covariance matrix + bool is_initialized_; // Initially set to false, set to true in first call of + // ProcessMeasurement + Eigen::VectorXd x_; // state vector + Eigen::MatrixXd P_; // state covariance matrix Eigen::MatrixXd Xsig_pred_; // predicted sigma points matrix - double std_a_; // Process noise standard deviation longitudinal acceleration in m/s^2 + double std_a_; // Process noise standard deviation longitudinal acceleration + // in m/s^2 long long time_us_; // time when the state is true, in us - double std_yawdd_; // Process noise standard deviation yaw acceleration in rad/s^2 - double std_laspx_; // Laser measurement noise standard deviation position1 in m - double std_laspy_; // Laser measurement noise standard deviation position2 in m + double std_yawdd_; // Process noise standard deviation yaw acceleration in + // rad/s^2 + double + std_laspx_; // Laser measurement noise standard deviation position1 in m + double + std_laspy_; // Laser measurement noise standard deviation position2 in m double std_radr_; // Process noise standard deviation radius in m double std_radphi_; // Process noise standard deviation yaw angle in rad - double std_radrd_; // Process noise standard deviation radius change in m/s + double std_radrd_; // Process noise standard deviation radius change in m/s Eigen::VectorXd weights_; // Weights of sigma points - double lambda_; // spreading parameter - int n_x_; // state dimension - int n_aug_; // augmented state dimension + double lambda_; // spreading parameter + int n_x_; // state dimension + int n_aug_; // augmented state dimension // Helper functions for Prediction - Eigen::MatrixXd calculatePredictedCovariance(const Eigen::MatrixXd &Xsig_pred, const Eigen::VectorXd &x); - [[nodiscard]] Eigen::MatrixXd predictSigmaPoints(const Eigen::MatrixXd &Xsig_aug, double delta_t) const; - [[nodiscard]] Eigen::MatrixXd createAugmentedSigmaPoints(const Eigen::MatrixXd &A, const Eigen::VectorXd &x_aug) const; + Eigen::MatrixXd calculatePredictedCovariance(const Eigen::MatrixXd &Xsig_pred, + const Eigen::VectorXd &x); + [[nodiscard]] Eigen::MatrixXd + predictSigmaPoints(const Eigen::MatrixXd &Xsig_aug, double delta_t) const; + [[nodiscard]] Eigen::MatrixXd + createAugmentedSigmaPoints(const Eigen::MatrixXd &A, + const Eigen::VectorXd &x_aug) const; [[nodiscard]] Eigen::MatrixXd createAugmentedCovarianceMatrix() const; [[nodiscard]] Eigen::VectorXd createAugmentedMeanVector() const; // Helper functions for Radar Update - static Eigen::MatrixXd calculateKalmanGain(const Eigen::MatrixXd& Tc, const Eigen::MatrixXd& S); - Eigen::MatrixXd calculateCrossCorrelation(Eigen::MatrixXd Zsig, const Eigen::VectorXd& z_pred, int n_z); - Eigen::MatrixXd predictMeasurementCovariance(Eigen::MatrixXd Zsig, const Eigen::VectorXd& z_pred, int n_z); + static Eigen::MatrixXd calculateKalmanGain(const Eigen::MatrixXd &Tc, + const Eigen::MatrixXd &S); + Eigen::MatrixXd calculateCrossCorrelation(Eigen::MatrixXd Zsig, + const Eigen::VectorXd &z_pred, + int n_z); + Eigen::MatrixXd predictMeasurementCovariance(Eigen::MatrixXd Zsig, + const Eigen::VectorXd &z_pred, + int n_z); Eigen::VectorXd predictMeasurementMean(Eigen::MatrixXd Zsig, int n_z); Eigen::MatrixXd createSigmaPointsInMeasurementSpace(int n_z); - void updateStateMeanAndCovariance(Eigen::MatrixXd K, const Eigen::VectorXd& z_pred, const MeasurementPackage& meas_package, Eigen::MatrixXd &S); + void updateStateMeanAndCovariance(Eigen::MatrixXd K, + const Eigen::VectorXd &z_pred, + const MeasurementPackage &meas_package, + Eigen::MatrixXd &S); // Helper functions for Lidar Update - void updateStateMeanAndCovarianceLidar(Eigen::MatrixXd K, const Eigen::VectorXd& z_pred, const MeasurementPackage &meas_package, const Eigen::MatrixXd& S); - Eigen::MatrixXd calculateCrossCorrelationLidar(Eigen::MatrixXd Zsig, const Eigen::VectorXd& z_pred, int n_z); - Eigen::MatrixXd predictMeasurementCovarianceLidar(const Eigen::MatrixXd &Zsig, const Eigen::VectorXd &z_pred, int n_z); - Eigen::VectorXd predictMeasurementMeanLidar(const Eigen::MatrixXd &Zsig, int n_z); + void updateStateMeanAndCovarianceLidar(Eigen::MatrixXd K, + const Eigen::VectorXd &z_pred, + const MeasurementPackage &meas_package, + const Eigen::MatrixXd &S); + Eigen::MatrixXd calculateCrossCorrelationLidar(Eigen::MatrixXd Zsig, + const Eigen::VectorXd &z_pred, + int n_z); + Eigen::MatrixXd + predictMeasurementCovarianceLidar(const Eigen::MatrixXd &Zsig, + const Eigen::VectorXd &z_pred, int n_z); + Eigen::VectorXd predictMeasurementMeanLidar(const Eigen::MatrixXd &Zsig, + int n_z); Eigen::MatrixXd createSigmaPointsLidar(int z); }; diff --git a/CMakeLists.txt b/CMakeLists.txt index 4e5db8f..0f964a5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,4 +19,4 @@ add_executable(play main.cpp Toolkit/tools.cpp Objects/car.cpp Objects/road.cpp Objects/parkingspot.cpp) -target_link_libraries (play ${PCL_LIBRARIES}) \ No newline at end of file +target_link_libraries(play ${PCL_LIBRARIES}) \ No newline at end of file diff --git a/Control/controller.cpp b/Control/controller.cpp index 7dbeb13..acabcd6 100644 --- a/Control/controller.cpp +++ b/Control/controller.cpp @@ -4,27 +4,24 @@ #include "controller.hpp" - -EgoCarController::EgoCarController(pcl::visualization::PCLVisualizer::Ptr& viewer, Car& car) +EgoCarController::EgoCarController( + pcl::visualization::PCLVisualizer::Ptr &viewer, Car &car) : viewer_(viewer), car_(car), currentAngle_(FPS), timeUs_(0) { angles_ = {XY, TopDown, Side, FPS}; - keyStates_ = { - {"Left", false}, - {"Right", false}, - {"Up", false}, - {"Down", false}, - {"x", false}, - {"space", false} - }; + keyStates_ = {{"Left", false}, {"Right", false}, {"Up", false}, + {"Down", false}, {"x", false}, {"space", false}}; } void EgoCarController::registerKeyboardCallbacks() { - viewer_->registerKeyboardCallback([&](const pcl::visualization::KeyboardEvent& event) { - if (keyStates_.find(event.getKeySym()) != keyStates_.end()) { - keyStates_[event.getKeySym()] = event.keyDown(); - } - if (event.isShiftPressed()) {car_.accelerate(10.0, 1);} - }); + viewer_->registerKeyboardCallback( + [&](const pcl::visualization::KeyboardEvent &event) { + if (keyStates_.find(event.getKeySym()) != keyStates_.end()) { + keyStates_[event.getKeySym()] = event.keyDown(); + } + if (event.isShiftPressed()) { + car_.accelerate(10.0, 1); + } + }); } void EgoCarController::handleKeyboardInput() { @@ -53,7 +50,7 @@ void EgoCarController::handleKeyboardInput() { } } -void EgoCarController::update(float dt, Scene& scene) { +void EgoCarController::update(float dt, Scene &scene) { scene.stepScene(car_, dt, 10, viewer_); timeUs_ += intervalUs_; } diff --git a/Control/controller.hpp b/Control/controller.hpp index a0732ff..28d6b21 100644 --- a/Control/controller.hpp +++ b/Control/controller.hpp @@ -6,21 +6,22 @@ #define NFS_CONTROLLER_HPP #include "../Objects/car.hpp" -#include "../View/camera.hpp" #include "../Scene/scene.hpp" +#include "../View/camera.hpp" #include #include class EgoCarController { public: - EgoCarController(pcl::visualization::PCLVisualizer::Ptr& viewer, Car& car); + EgoCarController(pcl::visualization::PCLVisualizer::Ptr &viewer, Car &car); void registerKeyboardCallbacks(); void handleKeyboardInput(); - void update(float dt, Scene& scene); + void update(float dt, Scene &scene); void stop(); + private: pcl::visualization::PCLVisualizer::Ptr viewer_; - Car& car_; + Car &car_; std::unordered_map keyStates_; CameraAngle currentAngle_; std::vector angles_; diff --git a/Objects/car.cpp b/Objects/car.cpp index 813d058..05ca038 100644 --- a/Objects/car.cpp +++ b/Objects/car.cpp @@ -4,32 +4,21 @@ #include "car.hpp" Car::Car() - : position(Vect3(0, 0, 0)), - dimensions(Vect3(0, 0, 0)), - orientation(Eigen::Quaternionf(0, 0, 0, 0)), - name("car"), - color(Color(0.0f, 0.0f, 0.0f)), - velocity(0), - angle(0), - acceleration(0), - steering(0), rollingInstance(0), frontCenterDistance(0), - control_index(0), - sinNegAngle(0), - cosNegAngle(0), - air_resistance(0.0), - tire_friction(0.0) {} - -Car::Car( - Vect3 setPosition, Vect3 setDimensions, Color setColor, - float setAngle, float setFrontCenterDistance, std::string setName - ): position(setPosition), - dimensions(setDimensions), - color(setColor), + : position(Vect3(0, 0, 0)), dimensions(Vect3(0, 0, 0)), + orientation(Eigen::Quaternionf(0, 0, 0, 0)), name("car"), + color(Color(0.0f, 0.0f, 0.0f)), velocity(0), angle(0), acceleration(0), + steering(0), rollingInstance(0), frontCenterDistance(0), control_index(0), + sinNegAngle(0), cosNegAngle(0), air_resistance(0.0), tire_friction(0.0) {} + +Car::Car(Vect3 setPosition, Vect3 setDimensions, Color setColor, float setAngle, + float setFrontCenterDistance, std::string setName) + : position(setPosition), dimensions(setDimensions), color(setColor), angle(setAngle), frontCenterDistance(setFrontCenterDistance), name(std::move(setName)) { - orientation = Eigen::Quaternionf(Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ())); - sinNegAngle = sin(- angle); - cosNegAngle = cos(- angle); + orientation = + Eigen::Quaternionf(Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ())); + sinNegAngle = sin(-angle); + cosNegAngle = cos(-angle); acceleration = 0; steering = 0; velocity = 0; @@ -39,7 +28,7 @@ Car::Car( tire_friction = 0.0; } -void Car::renderBottom(pcl::visualization::PCLVisualizer::Ptr& viewer) const { +void Car::renderBottom(pcl::visualization::PCLVisualizer::Ptr &viewer) const { viewer->addCube(Eigen::Vector3f(position.x, position.y, dimensions.z * 1 / 3), orientation, dimensions.x, dimensions.y, dimensions.z * 2 / 3, name); @@ -50,7 +39,7 @@ void Car::renderBottom(pcl::visualization::PCLVisualizer::Ptr& viewer) const { pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, name); } -void Car::renderTop(pcl::visualization::PCLVisualizer::Ptr& viewer) const { +void Car::renderTop(pcl::visualization::PCLVisualizer::Ptr &viewer) const { viewer->addCube(Eigen::Vector3f(position.x, position.y, dimensions.z * 5 / 6), orientation, dimensions.x / 2, dimensions.y, dimensions.z * 1 / 3, name + "Top"); @@ -74,38 +63,44 @@ pcl::ModelCoefficients Car::generateWheelCoefficients(int i, int j) const { pcl::ModelCoefficients coeffs; coeffs.values.push_back(position.x + i * dimensions.x / 5); // x position coeffs.values.push_back(position.y + j * dimensions.y / 2); // y position - coeffs.values.push_back(position.z + dimensions.z / 7); // z position - coeffs.values.push_back(1); // Direction x - coeffs.values.push_back(0); // Direction y - coeffs.values.push_back(0); // Direction z - coeffs.values.push_back(dimensions.z * 1 / 5); // Radius + coeffs.values.push_back(position.z + dimensions.z / 7); // z position + coeffs.values.push_back(1); // Direction x + coeffs.values.push_back(0); // Direction y + coeffs.values.push_back(0); // Direction z + coeffs.values.push_back(dimensions.z * 1 / 5); // Radius return coeffs; } void Car::renderWheels(pcl::visualization::PCLVisualizer::Ptr &viewer) const { - for(int i=-1; i<=1; i+=2) { - for(int j=-1; j<=1; j+=2) { + for (int i = -1; i <= 1; i += 2) { + for (int j = -1; j <= 1; j += 2) { pcl::ModelCoefficients coeffs = generateWheelCoefficients(i, j); - viewer->addCylinder(coeffs, name + "Wheel" + std::to_string(i) + std::to_string(j)); - viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, - 0, 0, 0, name + "Wheel" + std::to_string(i) + std::to_string(j)); + viewer->addCylinder(coeffs, name + "Wheel" + std::to_string(i) + + std::to_string(j)); + viewer->setShapeRenderingProperties( + pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 0, + name + "Wheel" + std::to_string(i) + std::to_string(j)); } } } -void Car::renderWindshields(pcl::visualization::PCLVisualizer::Ptr &viewer) const { - for(int i=-1; i<=1; i+=2) { +void Car::renderWindshields( + pcl::visualization::PCLVisualizer::Ptr &viewer) const { + for (int i = -1; i <= 1; i += 2) { Eigen::Vector3f globalPosition = generateWindshieldPosition(i); - viewer->addCube(globalPosition, orientation, dimensions.x / 4, dimensions.y, dimensions.z / 6, name + "Windshield" + std::to_string(i)); - viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, - 0.3, 0.7, 1.0, name + "Windshield" + std::to_string(i)); - viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, - pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, - name + "Windshield" + std::to_string(i)); + viewer->addCube(globalPosition, orientation, dimensions.x / 4, dimensions.y, + dimensions.z / 6, name + "Windshield" + std::to_string(i)); + viewer->setShapeRenderingProperties( + pcl::visualization::PCL_VISUALIZER_COLOR, 0.3, 0.7, 1.0, + name + "Windshield" + std::to_string(i)); + viewer->setShapeRenderingProperties( + pcl::visualization::PCL_VISUALIZER_REPRESENTATION, + pcl::visualization::PCL_VISUALIZER_REPRESENTATION_SURFACE, + name + "Windshield" + std::to_string(i)); } } -void Car::render(pcl::visualization::PCLVisualizer::Ptr& viewer) const { +void Car::render(pcl::visualization::PCLVisualizer::Ptr &viewer) const { renderBottom(viewer); renderTop(viewer); renderWheels(viewer); @@ -113,21 +108,20 @@ void Car::render(pcl::visualization::PCLVisualizer::Ptr& viewer) const { } void Car::accelerate(float acc, int dir) { - acceleration = acc * dir; + acceleration = dir * acc - air_resistance * velocity * abs(velocity) - + tire_friction * sign(velocity); } -void Car::steer(float s) { - steering = s; -} +void Car::steer(float s) { steering = s; } -void Car::control(const std::vector& c) { - for (auto& control : c) { +void Car::control(const std::vector &c) { + for (auto &control : c) { controls.push_back(control); } } void Car::move(float dt, int time_us) { - if (!controls.empty() && control_index < (int) controls.size() - 1) { + if (!controls.empty() && control_index < (int)controls.size() - 1) { if (time_us >= controls[control_index + 1].timeUs) { accelerate(controls[control_index + 1].acceleration); steer(controls[control_index + 1].steering); @@ -138,7 +132,8 @@ void Car::move(float dt, int time_us) { position.x += velocity * cos(angle) * dt; position.y += velocity * sin(angle) * dt; angle += velocity * steering * dt / frontCenterDistance; - orientation = Eigen::Quaternionf(Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ())); + orientation = + Eigen::Quaternionf(Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ())); velocity += acceleration * dt; if (acceleration == 0) { @@ -156,34 +151,37 @@ void Car::move(float dt, int time_us) { } } -[[nodiscard]] Vect3& Car::getPosition() {return position;} -[[nodiscard]] std::string& Car::getName() {return name;} -[[nodiscard]] float& Car::getVelocity() {return velocity;} -[[nodiscard]] float& Car::getAngle() {return angle;} -[[nodiscard]] Tracker& Car::getTracker() {return tracker;} -[[nodiscard]] Vect3 Car:: getDirection() { +[[nodiscard]] Vect3 &Car::getPosition() { return position; } +[[nodiscard]] std::string &Car::getName() { return name; } +[[nodiscard]] float &Car::getVelocity() { return velocity; } +[[nodiscard]] float &Car::getAngle() { return angle; } +[[nodiscard]] Tracker &Car::getTracker() { return tracker; } +[[nodiscard]] Vect3 Car::getDirection() { float angle = getAngle(); return {cos(angle), sin(angle), 0.0f}; } -void Car::setTracker(const Tracker& t) {tracker = t;} -void Car::setPosition(const Vect3 &p) {position = p;} -void Car::setVelocity(float v) {velocity = v;} +void Car::setTracker(const Tracker &t) { tracker = t; } +void Car::setPosition(const Vect3 &p) { position = p; } +void Car::setVelocity(float v) { velocity = v; } static bool inbetween(double point, double center, double range) { return point >= center - range && point <= center + range; } [[nodiscard]] bool Car::checkCollision(Vect3 point) const { - double xPrime = ((point.x-position.x) * cosNegAngle - (point.y-position.y) * sinNegAngle)+position.x; - double yPrime = ((point.y-position.y) * cosNegAngle + (point.x-position.x) * sinNegAngle)+position.y; - return ( - inbetween(xPrime, position.x, dimensions.x / 2) && - inbetween(yPrime, position.y, dimensions.y / 2) && - inbetween(point.z, position.z + dimensions.z / 3, dimensions.z / 3)) || - ( - inbetween(xPrime, position.x, dimensions.x / 4) && - inbetween(yPrime, position.y, dimensions.y / 2) && - inbetween(point.z, position.z + dimensions.z * 5 / 6, dimensions.z / 6) - ); + double xPrime = ((point.x - position.x) * cosNegAngle - + (point.y - position.y) * sinNegAngle) + + position.x; + double yPrime = ((point.y - position.y) * cosNegAngle + + (point.x - position.x) * sinNegAngle) + + position.y; + return (inbetween(xPrime, position.x, dimensions.x / 2) && + inbetween(yPrime, position.y, dimensions.y / 2) && + inbetween(point.z, position.z + dimensions.z / 3, + dimensions.z / 3)) || + (inbetween(xPrime, position.x, dimensions.x / 4) && + inbetween(yPrime, position.y, dimensions.y / 2) && + inbetween(point.z, position.z + dimensions.z * 5 / 6, + dimensions.z / 6)); } \ No newline at end of file diff --git a/Objects/car.hpp b/Objects/car.hpp index b993772..cbe8d72 100644 --- a/Objects/car.hpp +++ b/Objects/car.hpp @@ -4,19 +4,17 @@ #ifndef NFS_CAR_HPP #define NFS_CAR_HPP +#include "../Algo/tracker.hpp" #include #include #include #include #include -#include "../Algo/tracker.hpp" class Color { public: float r, g, b; - Color(float setR, float setG, float setB) - : r(setR), g(setG), b(setB) - {} + Color(float setR, float setG, float setB) : r(setR), g(setG), b(setB) {} void changeColor(float setR, float setG, float setB) { r = setR; g = setG; @@ -27,14 +25,12 @@ class Color { class Vect3 { public: float x, y, z; - Vect3(float setX, float setY, float setZ) - : x(setX), y(setY), z(setZ) - {} - Vect3 operator + (const Vect3& vec) const { + Vect3(float setX, float setY, float setZ) : x(setX), y(setY), z(setZ) {} + Vect3 operator+(const Vect3 &vec) const { Vect3 result(x + vec.x, y + vec.y, z + vec.z); return result; } - Vect3 operator * (const float& scalar) const { + Vect3 operator*(const float &scalar) const { Vect3 result(x * scalar, y * scalar, z * scalar); return result; } @@ -46,8 +42,7 @@ class Control { float acceleration; float steering; Control(long long t, float acc, float s) - : timeUs(t), acceleration(acc), steering(s) - {} + : timeUs(t), acceleration(acc), steering(s) {} }; class Car { @@ -69,35 +64,34 @@ class Car { double cosNegAngle; float air_resistance; float tire_friction; + public: Car(); Car(Vect3 setPosition, Vect3 setDimensions, Color setColor, float setAngle, float setFrontCenterDistance, std::string setName); - void renderBottom(pcl::visualization::PCLVisualizer::Ptr& viewer) const; - void renderTop(pcl::visualization::PCLVisualizer::Ptr& viewer) const; - void renderWheels(pcl::visualization::PCLVisualizer::Ptr& viewer) const; - void renderWindshields(pcl::visualization::PCLVisualizer::Ptr& viewer) const; - void render(pcl::visualization::PCLVisualizer::Ptr& viewer) const; + void renderBottom(pcl::visualization::PCLVisualizer::Ptr &viewer) const; + void renderTop(pcl::visualization::PCLVisualizer::Ptr &viewer) const; + void renderWheels(pcl::visualization::PCLVisualizer::Ptr &viewer) const; + void renderWindshields(pcl::visualization::PCLVisualizer::Ptr &viewer) const; + void render(pcl::visualization::PCLVisualizer::Ptr &viewer) const; void accelerate(float acc, int dir = 1); void steer(float s); - void control(const std::vector& c); + void control(const std::vector &c); void move(float dt, int time_us); - void setTracker(const Tracker& t); - void setPosition(const Vect3& p); + void setTracker(const Tracker &t); + void setPosition(const Vect3 &p); void setVelocity(float v); - [[nodiscard]] Vect3& getPosition(); - [[nodiscard]] float& getVelocity(); - [[nodiscard]] float& getAngle(); - [[nodiscard]] std::string& getName(); - [[nodiscard]] Tracker& getTracker(); + [[nodiscard]] Vect3 &getPosition(); + [[nodiscard]] float &getVelocity(); + [[nodiscard]] float &getAngle(); + [[nodiscard]] std::string &getName(); + [[nodiscard]] Tracker &getTracker(); [[nodiscard]] Vect3 getDirection(); [[nodiscard]] bool checkCollision(Vect3 point) const; Eigen::Vector3f generateWindshieldPosition(int i) const; pcl::ModelCoefficients generateWheelCoefficients(int i, int j) const; }; -inline int sign(float x) { - return (x > 0) - (x < 0); -} +inline int sign(float x) { return (x > 0) - (x < 0); } #endif // NFS_CAR_HPP diff --git a/Objects/obstacle.cpp b/Objects/obstacle.cpp index 2a5a4b6..4e2fc3d 100644 --- a/Objects/obstacle.cpp +++ b/Objects/obstacle.cpp @@ -4,19 +4,19 @@ #include "obstacle.hpp" -Obstacle::Obstacle(const Vect3& position, const Vect3& dimensions) +Obstacle::Obstacle(const Vect3 &position, const Vect3 &dimensions) : position_(position), dimensions_(dimensions) {} [[maybe_unused]] Vect3 Obstacle::getPosition() const { return position_; } [[maybe_unused]] Vect3 Obstacle::getDimensions() const { return dimensions_; } [[maybe_unused]] std::string Obstacle::getName() const { return name_; } -void Obstacle::render(pcl::visualization::PCLVisualizer::Ptr& viewer) const { - viewer->addCube(position_.x, position_.x + dimensions_.x, - position_.y, position_.y + dimensions_.y, - position_.z, position_.z + dimensions_.z, - 1, 1, 0, name_); - viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 0, name_); +void Obstacle::render(pcl::visualization::PCLVisualizer::Ptr &viewer) const { + viewer->addCube(position_.x, position_.x + dimensions_.x, position_.y, + position_.y + dimensions_.y, position_.z, + position_.z + dimensions_.z, 1, 1, 0, name_); + viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, + 1, 1, 0, name_); } -bool Obstacle::checkCollision(Car& car) const { +bool Obstacle::checkCollision(Car &car) const { return car.checkCollision(position_); } -void Obstacle::setName(const std::string& name) { name_ = name; } +void Obstacle::setName(const std::string &name) { name_ = name; } diff --git a/Objects/obstacle.hpp b/Objects/obstacle.hpp index e58220c..a803eac 100644 --- a/Objects/obstacle.hpp +++ b/Objects/obstacle.hpp @@ -9,13 +9,15 @@ class Obstacle { public: - Obstacle(const Vect3& position, const Vect3& dimensions); + Obstacle(const Vect3 &position, const Vect3 &dimensions); [[maybe_unused]] [[nodiscard]] Vect3 getPosition() const; [[maybe_unused]] [[nodiscard]] Vect3 getDimensions() const; [[maybe_unused]] [[nodiscard]] std::string getName() const; - [[maybe_unused]] void render(pcl::visualization::PCLVisualizer::Ptr& viewer) const; - bool checkCollision(Car& car) const; - void setName(const std::string& name); + [[maybe_unused]] void + render(pcl::visualization::PCLVisualizer::Ptr &viewer) const; + bool checkCollision(Car &car) const; + void setName(const std::string &name); + private: Vect3 position_; Vect3 dimensions_; diff --git a/Objects/parkingspot.cpp b/Objects/parkingspot.cpp index 008e262..ce659e3 100644 --- a/Objects/parkingspot.cpp +++ b/Objects/parkingspot.cpp @@ -4,27 +4,33 @@ #include "parkingspot.hpp" -ParkingSpot::ParkingSpot(const pcl::PointXYZ& position, double length, double width, double height, int id) - : position_(position), length_(length), width_(width), height_(height), id_(id) {} +ParkingSpot::ParkingSpot(const pcl::PointXYZ &position, double length, + double width, double height, int id) + : position_(position), length_(length), width_(width), height_(height), + id_(id) {} -[[maybe_unused]] pcl::PointXYZ ParkingSpot::getPosition() const { return position_; } +[[maybe_unused]] pcl::PointXYZ ParkingSpot::getPosition() const { + return position_; +} [[maybe_unused]] double ParkingSpot::getLength() const { return length_; } [[maybe_unused]] double ParkingSpot::getWidth() const { return width_; } [[maybe_unused]] double ParkingSpot::getHeight() const { return height_; } [[maybe_unused]] std::string ParkingSpot::getName() const { return name_; } -void ParkingSpot::render(pcl::visualization::PCLVisualizer::Ptr& viewer) const { +void ParkingSpot::render(pcl::visualization::PCLVisualizer::Ptr &viewer) const { viewer->addCube(position_.x - length_ / 2, position_.x + length_ / 2, position_.y - width_ / 2, position_.y + width_ / 2, - position_.z, position_.z + height_, - 1.0, 1.0, 0.0, name_); - viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.5, 0.5, 0.5, name_); + position_.z, position_.z + height_, 1.0, 1.0, 0.0, name_); + viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, + 0.5, 0.5, 0.5, name_); } -void ParkingSpot::setName(const std::string& name) { name_ = name; } -bool ParkingSpot::isCarParked(Car& car) const { +void ParkingSpot::setName(const std::string &name) { name_ = name; } +bool ParkingSpot::isCarParked(Car &car) const { Vect3 carPosition = car.getPosition(); - bool withinX = (carPosition.x >= (position_.x - length_ / 2)) && (carPosition.x <= (position_.x + length_ / 2)); - bool withinY = (carPosition.y >= (position_.y - width_ / 2)) && (carPosition.y <= (position_.y + width_ / 2)); - bool withinZ = (carPosition.z >= position_.z) && (carPosition.z <= (position_.z + height_)); + bool withinX = (carPosition.x >= (position_.x - length_ / 2)) && + (carPosition.x <= (position_.x + length_ / 2)); + bool withinY = (carPosition.y >= (position_.y - width_ / 2)) && + (carPosition.y <= (position_.y + width_ / 2)); + bool withinZ = (carPosition.z >= position_.z) && + (carPosition.z <= (position_.z + height_)); return withinX && withinY && withinZ; } - diff --git a/Objects/parkingspot.hpp b/Objects/parkingspot.hpp index fd1d1d0..e5526bc 100644 --- a/Objects/parkingspot.hpp +++ b/Objects/parkingspot.hpp @@ -9,15 +9,17 @@ class ParkingSpot { public: - ParkingSpot(const pcl::PointXYZ& position, double length, double width, double height, int id); + ParkingSpot(const pcl::PointXYZ &position, double length, double width, + double height, int id); [[maybe_unused]] [[nodiscard]] pcl::PointXYZ getPosition() const; [[maybe_unused]] [[nodiscard]] double getLength() const; [[maybe_unused]] [[nodiscard]] double getWidth() const; [[maybe_unused]] [[nodiscard]] double getHeight() const; [[maybe_unused]] [[nodiscard]] std::string getName() const; - void render(pcl::visualization::PCLVisualizer::Ptr& viewer) const; - void setName(const std::string& name); - bool isCarParked(Car& car) const; + void render(pcl::visualization::PCLVisualizer::Ptr &viewer) const; + void setName(const std::string &name); + bool isCarParked(Car &car) const; + private: pcl::PointXYZ position_; double length_; diff --git a/Objects/road.cpp b/Objects/road.cpp index 9f08589..9a3a251 100644 --- a/Objects/road.cpp +++ b/Objects/road.cpp @@ -4,20 +4,20 @@ #include "road.hpp" - -Road::Road() : arenaRadius(50.0), laneWidth(5.0), laneCount(2), poleRadius(0.5), poleHeight(3.0), poleSpacing(10.0) { +Road::Road() + : arenaRadius(50.0), laneWidth(5.0), laneCount(2), poleRadius(0.5), + poleHeight(3.0), poleSpacing(10.0) { computeCoefficients(); computeDummies(); } - void Road::computeCoefficients() { for (int i = 0; i < laneCount; ++i) { pcl::ModelCoefficients circleCoeffs; circleCoeffs.values.resize(3); - circleCoeffs.values[0] = 0; // center x - circleCoeffs.values[1] = 0; // center y + circleCoeffs.values[0] = 0; // center x + circleCoeffs.values[1] = 0; // center y circleCoeffs.values[2] = arenaRadius - i * laneWidth; // radius laneCoefficients.push_back(circleCoeffs); } @@ -27,37 +27,38 @@ void Road::computeCoefficients() { poleCoeffs.values.resize(7); poleCoeffs.values[0] = arenaRadius * std::cos(angle); // x position poleCoeffs.values[1] = arenaRadius * std::sin(angle); // y position - poleCoeffs.values[2] = 0; // z position - poleCoeffs.values[3] = 0; // direction x - poleCoeffs.values[4] = 0; // direction y - poleCoeffs.values[5] = poleHeight; // direction z - poleCoeffs.values[6] = poleRadius; // radius + poleCoeffs.values[2] = 0; // z position + poleCoeffs.values[3] = 0; // direction x + poleCoeffs.values[4] = 0; // direction y + poleCoeffs.values[5] = poleHeight; // direction z + poleCoeffs.values[6] = poleRadius; // radius poleCoefficients.push_back(poleCoeffs); } } -void Road::renderLanes(pcl::visualization::PCLVisualizer::Ptr& viewer) const { +void Road::renderLanes(pcl::visualization::PCLVisualizer::Ptr &viewer) const { for (int i = 0; i < laneCount; ++i) { viewer->addCircle(laneCoefficients[i], "lane" + std::to_string(i), 0); - viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 0, "lane" + std::to_string(i)); // Yellow lanes + viewer->setShapeRenderingProperties( + pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 0, + "lane" + std::to_string(i)); // Yellow lanes } } -void Road::renderPoles(pcl::visualization::PCLVisualizer::Ptr& viewer) const { +void Road::renderPoles(pcl::visualization::PCLVisualizer::Ptr &viewer) const { for (size_t i = 0; i < poleCoefficients.size(); ++i) { viewer->addCylinder(poleCoefficients[i], "pole" + std::to_string(i)); - viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0.5, 0, "pole" + std::to_string(i)); // Orange poles + viewer->setShapeRenderingProperties( + pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0.5, 0, + "pole" + std::to_string(i)); // Orange poles } } void Road::computeDummies() { std::vector dummyObstacles = { - pcl::PointXYZ(-15, -15, 0), - pcl::PointXYZ(-15, 15, 0), - pcl::PointXYZ(15, -15, 0), - pcl::PointXYZ(15, 15, 0) - }; + pcl::PointXYZ(-15, -15, 0), pcl::PointXYZ(-15, 15, 0), + pcl::PointXYZ(15, -15, 0), pcl::PointXYZ(15, 15, 0)}; std::random_device rd; std::mt19937 gen(rd()); @@ -68,55 +69,61 @@ void Road::computeDummies() { pcl::PointXYZ(disLoc(gen), -disLoc(gen), 0), pcl::PointXYZ(-disLoc(gen), disLoc(gen), 0), pcl::PointXYZ(disLoc(gen), -disLoc(gen), 0), - pcl::PointXYZ(disLoc(gen), disLoc(gen), 0) - }; + pcl::PointXYZ(disLoc(gen), disLoc(gen), 0)}; - for (const auto& location : dummyObstacles) { + for (const auto &location : dummyObstacles) { double dimension = disDim(gen); obstacles.emplace_back(dimension, location); } - for (auto& location : dummyParkingSpots) { + for (auto &location : dummyParkingSpots) { parkingSpots.push_back(location); } } -void Road::renderDummies(pcl::visualization::PCLVisualizer::Ptr& viewer) const { +void Road::renderDummies(pcl::visualization::PCLVisualizer::Ptr &viewer) const { double parkingSpotWidth = 2.0; double parkingSpotLength = 4.0; double parkingSpotHeight = 0.1; for (size_t i = 0; i < parkingSpots.size(); ++i) { pcl::PointXYZ location = parkingSpots[i]; - viewer->addCube(location.x - parkingSpotLength / 2, location.x + parkingSpotLength / 2, - location.y - parkingSpotWidth / 2, location.y + parkingSpotWidth / 2, - location.z, location.z + parkingSpotHeight, - 0.5, 0.5, 0.5, "dummyParkingSpot" + std::to_string(i)); + viewer->addCube( + location.x - parkingSpotLength / 2, location.x + parkingSpotLength / 2, + location.y - parkingSpotWidth / 2, location.y + parkingSpotWidth / 2, + location.z, location.z + parkingSpotHeight, 0.5, 0.5, 0.5, + "dummyParkingSpot" + std::to_string(i)); } for (size_t i = 0; i < obstacles.size(); ++i) { double dimension = obstacles[i].first; pcl::PointXYZ location = obstacles[i].second; - viewer->addCube(location.x, location.x + dimension, - location.y, location.y + dimension, - location.z, location.z + dimension, + viewer->addCube(location.x, location.x + dimension, location.y, + location.y + dimension, location.z, location.z + dimension, 1, 1, 0, "dummyObstacle" + std::to_string(i)); - viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 0, "dummyObstacle" + std::to_string(i)); + viewer->setShapeRenderingProperties( + pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 0, + "dummyObstacle" + std::to_string(i)); } } -void Road::render(pcl::visualization::PCLVisualizer::Ptr& viewer) const { +void Road::render(pcl::visualization::PCLVisualizer::Ptr &viewer) const { renderLanes(viewer); renderPoles(viewer); renderDummies(viewer); } -std::vectorRoad::getLaneCoefficients() const { +std::vector Road::getLaneCoefficients() const { return laneCoefficients; } -void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud::Ptr& cloud, const std::string& name, Color color) { +void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr &viewer, + const pcl::PointCloud::Ptr &cloud, + const std::string &name, Color color) { viewer->addPointCloud(cloud, name); - viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name); - viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name); + viewer->setPointCloudRenderingProperties( + pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, + name); } \ No newline at end of file diff --git a/Objects/road.hpp b/Objects/road.hpp index ddb1021..1cf750b 100644 --- a/Objects/road.hpp +++ b/Objects/road.hpp @@ -1,8 +1,8 @@ // // Created by Andrei on 23-Apr-23. // -#include #include "car.hpp" +#include #include #ifndef NFS_ROAD_HPP @@ -13,11 +13,12 @@ class Road { Road(); void computeCoefficients(); void computeDummies(); - void renderPoles(pcl::visualization::PCLVisualizer::Ptr& viewer) const; - void renderLanes(pcl::visualization::PCLVisualizer::Ptr& viewer) const; - void renderDummies(pcl::visualization::PCLVisualizer::Ptr& viewer) const; - void render(pcl::visualization::PCLVisualizer::Ptr& viewer) const; + void renderPoles(pcl::visualization::PCLVisualizer::Ptr &viewer) const; + void renderLanes(pcl::visualization::PCLVisualizer::Ptr &viewer) const; + void renderDummies(pcl::visualization::PCLVisualizer::Ptr &viewer) const; + void render(pcl::visualization::PCLVisualizer::Ptr &viewer) const; std::vector getLaneCoefficients() const; + private: float arenaRadius; float laneWidth; @@ -31,7 +32,8 @@ class Road { std::vector> obstacles; }; -void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud::Ptr& cloud, const std::string& name, Color color = Color(1, 1, 1)); - +void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr &viewer, + const pcl::PointCloud::Ptr &cloud, + const std::string &name, Color color = Color(1, 1, 1)); #endif // NFS_ROAD_HPP diff --git a/Scene/scene.cpp b/Scene/scene.cpp index 2dda393..e57a9e6 100644 --- a/Scene/scene.cpp +++ b/Scene/scene.cpp @@ -4,7 +4,7 @@ #include "scene.hpp" -Scene::Scene(pcl::visualization::PCLVisualizer::Ptr& viewer) { +Scene::Scene(pcl::visualization::PCLVisualizer::Ptr &viewer) { tools = Tools(); std::mt19937 gen(std::chrono::system_clock::now().time_since_epoch().count()); @@ -25,39 +25,36 @@ Scene::Scene(pcl::visualization::PCLVisualizer::Ptr& viewer) { for (int i = 0; i <= 3; ++i) { pcl::PointXYZ position(disPos(gen), disPos(gen), 0.0); - ParkingSpot parkingSpot(position, parkingSpotLength, parkingSpotWidth, parkingSpotHeight, i); + ParkingSpot parkingSpot(position, parkingSpotLength, parkingSpotWidth, + parkingSpotHeight, i); parkingSpot.setName("parkingSpot" + std::to_string(i)); parkingSpots.push_back(parkingSpot); } - Car car1 = Car( - Vect3(-10, 4, 0), // Position - Vect3(4, 2, 2), // Dimensions - Color(1, 0, 0), // Color - 10, // Angle - 2, // Front Center Distance - "car1" // Name + Car car1 = Car(Vect3(-10, 4, 0), // Position + Vect3(4, 2, 2), // Dimensions + Color(1, 0, 0), // Color + 10, // Angle + 2, // Front Center Distance + "car1" // Name ); - Car car2 = Car( - Vect3(25, -40, 0), // Position - Vect3(4, 2, 2), // Dimensions - Color(0, 0, 1), // Color - -6, // Angle - 2, // Front Center Distance - "car2" // Name + Car car2 = Car(Vect3(25, -40, 0), // Position + Vect3(4, 2, 2), // Dimensions + Color(0, 0, 1), // Color + -6, // Angle + 2, // Front Center Distance + "car2" // Name ); - Car car3 = Car( - Vect3(-12, 30, 0), // Position - Vect3(4, 2, 2), // Dimensions - Color(1, 1, 1), // Color - 2, // Angle - 2, // Front Center Distance - "car3" // Name + Car car3 = Car(Vect3(-12, 30, 0), // Position + Vect3(4, 2, 2), // Dimensions + Color(1, 1, 1), // Color + 2, // Angle + 2, // Front Center Distance + "car3" // Name ); - if (trackCars[0]) { Tracker tracker1; car1.setTracker(tracker1); @@ -86,7 +83,7 @@ Scene::Scene(pcl::visualization::PCLVisualizer::Ptr& viewer) { car3.render(viewer); } -Control Scene::randomControl(std::mt19937& gen, Car& car) { +Control Scene::randomControl(std::mt19937 &gen, Car &car) { std::uniform_real_distribution<> disTime(1, 8); std::uniform_real_distribution<> disAcceleration(-5, 5); std::uniform_real_distribution<> disSteering(-0.15, 0.15); @@ -103,14 +100,13 @@ Control Scene::randomControl(std::mt19937& gen, Car& car) { acceleration = disAcceleration(gen); steering = disSteering(gen); - Vect3 newPosition = currentPosition + Vect3(time * acceleration * cos(steering), - time * acceleration * sin(steering), - 0); + Vect3 newPosition = + currentPosition + Vect3(time * acceleration * cos(steering), + time * acceleration * sin(steering), 0); withinBounds = true; - for (const auto& coeff : road.getLaneCoefficients()) { - double distance = sqrt( - pow(newPosition.x - coeff.values[0], 2) + - pow(newPosition.y - coeff.values[1], 2)); + for (const auto &coeff : road.getLaneCoefficients()) { + double distance = sqrt(pow(newPosition.x - coeff.values[0], 2) + + pow(newPosition.y - coeff.values[1], 2)); if (distance < coeff.values[2] / 2) { withinBounds = false; break; @@ -121,20 +117,23 @@ Control Scene::randomControl(std::mt19937& gen, Car& car) { return Control(time, acceleration * 0.25, steering); } -std::vector Scene::randomControlInstructions(std::mt19937& gen, Car& car, int numInstructions) { +std::vector Scene::randomControlInstructions(std::mt19937 &gen, + Car &car, + int numInstructions) { std::vector instructions; - for(int i=0; iaddText("Parked!", 200, 200, 20, 1, 1, 1, "parkingText"); } @@ -142,7 +141,8 @@ void Scene::stepScene(Car& egoCar, double dt, long long timestamp, pcl::visualiz if (parkedSpots.size() == parkingSpots.size()) { win = true; - viewer->addText("Parked on all spots!", 200, 200, 20, 1, 1, 1, "completeParkingText"); + viewer->addText("Parked on all spots!", 200, 200, 20, 1, 1, 1, + "completeParkingText"); } egoCar.move(dt, timestamp); @@ -151,7 +151,7 @@ void Scene::stepScene(Car& egoCar, double dt, long long timestamp, pcl::visualiz Car predictedEgoCar = egoCar; predictedEgoCar.move(dt, timestamp); - for (const Obstacle& obstacle: obstacles) { + for (const Obstacle &obstacle : obstacles) { obstacle.render(viewer); if (obstacle.checkCollision(predictedEgoCar)) { viewer->addText("Crashed!", 200, 200, 20, 1, 1, 1, "collisionText"); @@ -163,7 +163,7 @@ void Scene::stepScene(Car& egoCar, double dt, long long timestamp, pcl::visualiz } for (size_t i = 0; i < traffic.size(); ++i) { - Car& car = traffic[i]; + Car &car = traffic[i]; viewer->removeShape(car.getName()); viewer->removeShape(car.getName() + "front"); car.move(dt, timestamp); @@ -172,8 +172,8 @@ void Scene::stepScene(Car& egoCar, double dt, long long timestamp, pcl::visualiz if (trackCars[i]) { Eigen::VectorXd gt(4); gt << car.getPosition().x, car.getPosition().y, - car.getVelocity() * cos(car.getAngle()), - car.getVelocity() * sin(car.getAngle()); + car.getVelocity() * cos(car.getAngle()), + car.getVelocity() * sin(car.getAngle()); tools.groundTruth.push_back(gt); tools.lidarSense(car, viewer, timestamp, visualize_lidar); @@ -184,14 +184,14 @@ void Scene::stepScene(Car& egoCar, double dt, long long timestamp, pcl::visualiz Eigen::VectorXd estimate(4); double v = traffic[i].getTracker().x_(2); double yaw = traffic[i].getTracker().x_(3); - double vx = cos(yaw)*v; - double vy = sin(yaw)*v; - estimate << traffic[i].getTracker().x_[0], traffic[i].getTracker().x_[1], vx, vy; + double vx = cos(yaw) * v; + double vy = sin(yaw) * v; + estimate << traffic[i].getTracker().x_[0], traffic[i].getTracker().x_[1], + vx, vy; tools.estimations.push_back(estimate); - Car estimatedCar = car; - estimatedCar.setPosition(Vect3(estimate[0], estimate[1], car.getPosition().z)); - if (predictedEgoCar.checkCollision(estimatedCar.getPosition())) { + if (predictedEgoCar.checkCollision( + Vect3(estimate[0], estimate[1], car.getPosition().z))) { viewer->addText("Crashed!", 200, 200, 20, 1, 1, 1, "collisionText"); egoCar.setVelocity(0.0); Vect3 currentPos = egoCar.getPosition(); @@ -202,52 +202,55 @@ void Scene::stepScene(Car& egoCar, double dt, long long timestamp, pcl::visualiz } viewer->addText("Accuracy - RMSE:", 30, 300, 20, 1, 1, 1, "rmse"); - Eigen::VectorXd rmse = Tools::calculateRMSE(tools.estimations, tools.groundTruth); - viewer->addText(" X: " + std::to_string(rmse[0]), 30, 275, 20, 1, 1, 1, "rmse_x"); - viewer->addText(" Y: " + std::to_string(rmse[1]), 30, 250, 20, 1, 1, 1, "rmse_y"); - viewer->addText("Vx: " + std::to_string(rmse[2]), 30, 225, 20, 1, 1, 1, "rmse_vx"); - viewer->addText("Vy: " + std::to_string(rmse[3]), 30, 200, 20, 1, 1, 1, "rmse_vy"); - - if(timestamp > 1.0e6) - { - - if(rmse[0] > rmseThreshold[0]) - { + Eigen::VectorXd rmse = + Tools::calculateRMSE(tools.estimations, tools.groundTruth); + viewer->addText(" X: " + std::to_string(rmse[0]), 30, 275, 20, 1, 1, 1, + "rmse_x"); + viewer->addText(" Y: " + std::to_string(rmse[1]), 30, 250, 20, 1, 1, 1, + "rmse_y"); + viewer->addText("Vx: " + std::to_string(rmse[2]), 30, 225, 20, 1, 1, 1, + "rmse_vx"); + viewer->addText("Vy: " + std::to_string(rmse[3]), 30, 200, 20, 1, 1, 1, + "rmse_vy"); + + if (timestamp > 1.0e6) { + + if (rmse[0] > rmseThreshold[0]) { rmseFailLog[0] = rmse[0]; pass = false; } - if(rmse[1] > rmseThreshold[1]) - { + if (rmse[1] > rmseThreshold[1]) { rmseFailLog[1] = rmse[1]; pass = false; } - if(rmse[2] > rmseThreshold[2]) - { + if (rmse[2] > rmseThreshold[2]) { rmseFailLog[2] = rmse[2]; pass = false; } - if(rmse[3] > rmseThreshold[3]) - { + if (rmse[3] > rmseThreshold[3]) { rmseFailLog[3] = rmse[3]; pass = false; } } - if(!pass) - { + if (!pass) { viewer->addText("RMSE Failed Threshold", 30, 150, 20, 1, 0, 0, "rmse_fail"); - if(rmseFailLog[0] > 0) - viewer->addText(" X: "+std::to_string(rmseFailLog[0]), 30, 125, 20, 1, 0, 0, "rmse_fail_x"); - if(rmseFailLog[1] > 0) - viewer->addText(" Y: "+std::to_string(rmseFailLog[1]), 30, 100, 20, 1, 0, 0, "rmse_fail_y"); - if(rmseFailLog[2] > 0) - viewer->addText("Vx: "+std::to_string(rmseFailLog[2]), 30, 75, 20, 1, 0, 0, "rmse_fail_vx"); - if(rmseFailLog[3] > 0) - viewer->addText("Vy: "+std::to_string(rmseFailLog[3]), 30, 50, 20, 1, 0, 0, "rmse_fail_vy"); + if (rmseFailLog[0] > 0) + viewer->addText(" X: " + std::to_string(rmseFailLog[0]), 30, 125, 20, 1, + 0, 0, "rmse_fail_x"); + if (rmseFailLog[1] > 0) + viewer->addText(" Y: " + std::to_string(rmseFailLog[1]), 30, 100, 20, 1, + 0, 0, "rmse_fail_y"); + if (rmseFailLog[2] > 0) + viewer->addText("Vx: " + std::to_string(rmseFailLog[2]), 30, 75, 20, 1, 0, + 0, "rmse_fail_vx"); + if (rmseFailLog[3] > 0) + viewer->addText("Vy: " + std::to_string(rmseFailLog[3]), 30, 50, 20, 1, 0, + 0, "rmse_fail_vy"); } } -bool Scene::checkTrafficCollision(Car& egoCar) { - for (Car& car : traffic) { +bool Scene::checkTrafficCollision(Car &egoCar) { + for (Car &car : traffic) { if (egoCar.checkCollision(car.getPosition())) { return true; } diff --git a/Scene/scene.hpp b/Scene/scene.hpp index 4af0d21..139b0ef 100644 --- a/Scene/scene.hpp +++ b/Scene/scene.hpp @@ -6,12 +6,12 @@ #define NFS_SCENE_HPP #include "../Objects/car.hpp" -#include "../Objects/road.hpp" #include "../Objects/obstacle.hpp" #include "../Objects/parkingspot.hpp" +#include "../Objects/road.hpp" #include "../Toolkit/tools.hpp" -#include #include +#include class Scene { public: @@ -21,8 +21,8 @@ class Scene { bool pass = true; bool win = false; std::vector trackCars = {true, true, true}; - std::vector rmseThreshold = {1.20,1.50,2.50,0.9}; - std::vector rmseFailLog = {0.0,0.0,0.0,0.0}; + std::vector rmseThreshold = {1.20, 1.50, 2.50, 0.9}; + std::vector rmseFailLog = {0.0, 0.0, 0.0, 0.0}; std::vector obstacles; std::vector parkingSpots; std::set parkedSpots; @@ -31,11 +31,13 @@ class Scene { bool visualize_track = false; double projectedTime = 2; int projectedSteps = 6; - explicit Scene(pcl::visualization::PCLVisualizer::Ptr& viewer); - void stepScene(Car& egoCar, double dt, long long timestamp, pcl::visualization::PCLVisualizer::Ptr& viewer); - bool checkTrafficCollision(Car& egoCar); - std::vector randomControlInstructions(std::mt19937& gen, Car& car, int numInstructions); - Control randomControl(std::mt19937& gen, Car& car); + explicit Scene(pcl::visualization::PCLVisualizer::Ptr &viewer); + void stepScene(Car &egoCar, double dt, long long timestamp, + pcl::visualization::PCLVisualizer::Ptr &viewer); + bool checkTrafficCollision(Car &egoCar); + std::vector randomControlInstructions(std::mt19937 &gen, Car &car, + int numInstructions); + Control randomControl(std::mt19937 &gen, Car &car); }; #endif // NFS_SCENE_HPP diff --git a/Sensors/lidar.cpp b/Sensors/lidar.cpp index 8419fd2..a0d8081 100644 --- a/Sensors/lidar.cpp +++ b/Sensors/lidar.cpp @@ -3,42 +3,52 @@ // #include "lidar.hpp" -Ray::Ray(Vect3 setOrigin, double horizontalAngle, double verticalAngle, double setResolution) +Ray::Ray(Vect3 setOrigin, double horizontalAngle, double verticalAngle, + double setResolution) : origin(setOrigin), resolution(setResolution), direction(resolution * cos(verticalAngle) * cos(horizontalAngle), resolution * cos(verticalAngle) * sin(horizontalAngle), resolution * sin(verticalAngle)), castPosition(origin), castDistance(0) {} -void Ray::rayCast(const std::vector& cars, double minDistance, double maxDistance, pcl::PointCloud::Ptr& cloud, double slopeAngle, double stdErr) { +void Ray::rayCast(const std::vector &cars, double minDistance, + double maxDistance, + pcl::PointCloud::Ptr &cloud, double slopeAngle, + double stdErr) { castPosition = origin; castDistance = 0; bool collision = false; - while(!collision && castDistance < maxDistance && (castPosition.y <= 6 && castPosition.y >= -6 && castPosition.x <= 50 && castPosition.x >= -15)) { + while (!collision && castDistance < maxDistance && + (castPosition.y <= 6 && castPosition.y >= -6 && castPosition.x <= 50 && + castPosition.x >= -15)) { castPosition = castPosition + direction; castDistance += resolution; collision = (castPosition.z <= castPosition.x * tan(slopeAngle)); - if(!collision && castDistance < maxDistance) { - for(Car car : cars) { + if (!collision && castDistance < maxDistance) { + for (Car car : cars) { collision |= car.checkCollision(castPosition); - if(collision) { + if (collision) { break; } } } } - if((castDistance >= minDistance)&&(castDistance<=maxDistance)&& (castPosition.y <= 6 && castPosition.y >= -6 && castPosition.x <= 50 && castPosition.x >= -15)) { - double rx = ((double) rand() / (RAND_MAX)); - double ry = ((double) rand() / (RAND_MAX)); - double rz = ((double) rand() / (RAND_MAX)); - cloud->points.emplace_back(castPosition.x+rx*stdErr, castPosition.y+ry*stdErr, castPosition.z+rz*stdErr); + if ((castDistance >= minDistance) && (castDistance <= maxDistance) && + (castPosition.y <= 6 && castPosition.y >= -6 && castPosition.x <= 50 && + castPosition.x >= -15)) { + double rx = ((double)rand() / (RAND_MAX)); + double ry = ((double)rand() / (RAND_MAX)); + double rz = ((double)rand() / (RAND_MAX)); + cloud->points.emplace_back(castPosition.x + rx * stdErr, + castPosition.y + ry * stdErr, + castPosition.z + rz * stdErr); } } Lidar::Lidar(std::vector setCars, double setGroundSlope) - : cloud (new pcl::PointCloud), position(0, 0, 3.0) { + : cloud(new pcl::PointCloud), position(0, 0, 3.0) { minDistance = 0; maxDistance = 120; @@ -46,15 +56,15 @@ Lidar::Lidar(std::vector setCars, double setGroundSlope) stdErr = 0.02; cars = setCars; groundSlope = setGroundSlope; - //TODO increase the number of layers to 8 to get a higher resolution pcd int numLayers = 64; - double steepestAngle = - 24.8 * M_PI / 180; + double steepestAngle = -24.8 * M_PI / 180; double angleRange = 26.8 * M_PI / 180; - //TODO set to pi/64 to get a higher resolution pcd double horizontalAngleInc = M_PI / 180; - double angleIncrement = angleRange/ numLayers; + double angleIncrement = angleRange / numLayers; - for (double angleVerical = steepestAngle; angleVerical < steepestAngle + angleRange; angleVerical += angleIncrement) { + for (double angleVerical = steepestAngle; + angleVerical < steepestAngle + angleRange; + angleVerical += angleIncrement) { for (double angle = 0; angle <= 2 * M_PI; angle += horizontalAngleInc) { Ray ray(position, angle, angleVerical, resolution); rays.push_back(ray); @@ -62,15 +72,14 @@ Lidar::Lidar(std::vector setCars, double setGroundSlope) } } -Lidar::~Lidar() {/**pcl deals with the memory via smart pointers */} - -void Lidar::updateCars(std::vector setCars) { - cars = setCars; +Lidar::~Lidar() { /**pcl deals with the memory via smart pointers */ } +void Lidar::updateCars(std::vector setCars) { cars = setCars; } + pcl::PointCloud::Ptr Lidar::scan() { cloud->points.clear(); - for (Ray ray: rays) { + for (Ray ray : rays) { ray.rayCast(cars, minDistance, maxDistance, cloud, groundSlope, stdErr); } cloud->width = cloud->points.size(); diff --git a/Sensors/lidar.hpp b/Sensors/lidar.hpp index 13fb59d..eeed8a3 100644 --- a/Sensors/lidar.hpp +++ b/Sensors/lidar.hpp @@ -7,7 +7,6 @@ #include "../Objects/car.hpp" - class Ray { public: Vect3 origin; @@ -16,8 +15,11 @@ class Ray { Vect3 castPosition; double castDistance; - Ray(Vect3 setOrigin, double horizontalAngle, double verticalAngle, double setResolution); - void rayCast(const std::vector& cars, double minDistance, double maxDistance, pcl::PointCloud::Ptr& cloud, double slopeAngle, double stdErr); + Ray(Vect3 setOrigin, double horizontalAngle, double verticalAngle, + double setResolution); + void rayCast(const std::vector &cars, double minDistance, + double maxDistance, pcl::PointCloud::Ptr &cloud, + double slopeAngle, double stdErr); }; class Lidar { @@ -35,6 +37,6 @@ class Lidar { ~Lidar(); void updateCars(std::vector setCars); pcl::PointCloud::Ptr scan(); - }; +}; #endif // NFS_LIDAR_HPP diff --git a/Sensors/sensors.hpp b/Sensors/sensors.hpp index c645a06..ca87097 100644 --- a/Sensors/sensors.hpp +++ b/Sensors/sensors.hpp @@ -10,10 +10,7 @@ class MeasurementPackage { public: long long timeStamp; - enum SensorType{ - LIDAR, - RADAR - } sensorType; + enum SensorType { LIDAR, RADAR } sensorType; Eigen::VectorXd rawMeasurements; }; diff --git a/Toolkit/tools.cpp b/Toolkit/tools.cpp index 0fbffa6..46daa65 100644 --- a/Toolkit/tools.cpp +++ b/Toolkit/tools.cpp @@ -1,26 +1,32 @@ // // Created by Andrei on 30-Apr-23. // +#include "tools.hpp" #include #include -#include "tools.hpp" Tools::Tools() = default; Tools::~Tools() = default; double Tools::noise(double stdDev, long long seedNum) { std::mt19937::result_type seed = seedNum; - auto dist = std::bind(std::normal_distribution{0, stdDev}, std::mt19937(seed)); + auto dist = std::bind(std::normal_distribution{0, stdDev}, + std::mt19937(seed)); return dist(); } -lidarMarker Tools::lidarSense(Car& car, pcl::visualization::PCLVisualizer::Ptr& viewer, long long timestamp, bool visualize) { +lidarMarker Tools::lidarSense(Car &car, + pcl::visualization::PCLVisualizer::Ptr &viewer, + long long timestamp, bool visualize) { MeasurementPackage measPackage; measPackage.sensorType = MeasurementPackage::LIDAR; measPackage.rawMeasurements = Eigen::VectorXd(2); - lidarMarker marker = lidarMarker(car.getPosition().x + noise(0.15, timestamp), car.getPosition().y + noise(0.15, timestamp + 1)); + lidarMarker marker = + lidarMarker(car.getPosition().x + noise(0.15, timestamp), + car.getPosition().y + noise(0.15, timestamp + 1)); if (visualize) { - viewer->addSphere(pcl::PointXYZ(marker.x, marker.y, 3.0), 0.5, 1, 0, 0, car.getName() + "_lmarker"); + viewer->addSphere(pcl::PointXYZ(marker.x, marker.y, 3.0), 0.5, 1, 0, 0, + car.getName() + "_lmarker"); } measPackage.rawMeasurements << marker.x, marker.y; measPackage.timeStamp = timestamp; @@ -28,39 +34,42 @@ lidarMarker Tools::lidarSense(Car& car, pcl::visualization::PCLVisualizer::Ptr& return marker; } -radarMarker Tools::radarSense(Car& car, Car ego, pcl::visualization::PCLVisualizer::Ptr& viewer, long long timestamp, bool visualize) { +radarMarker Tools::radarSense(Car &car, Car ego, + pcl::visualization::PCLVisualizer::Ptr &viewer, + long long timestamp, bool visualize) { viewer->removeShape(car.getName() + "_rho"); viewer->removeShape(car.getName() + "_rhoDot"); - double rho = sqrt((car.getPosition().x - ego.getPosition().x) * (car.getPosition().x - ego.getPosition().x) + - (car.getPosition().y - ego.getPosition().y) * (car.getPosition().y - ego.getPosition().y)); - double phi = atan2(car.getPosition().y - ego.getPosition().y, car.getPosition().x - ego.getPosition().x); + double rho = sqrt((car.getPosition().x - ego.getPosition().x) * + (car.getPosition().x - ego.getPosition().x) + + (car.getPosition().y - ego.getPosition().y) * + (car.getPosition().y - ego.getPosition().y)); + double phi = atan2(car.getPosition().y - ego.getPosition().y, + car.getPosition().x - ego.getPosition().x); double rhoDot = (car.getVelocity() * cos(car.getAngle()) * rho * cos(phi) + - car.getVelocity() * sin(car.getAngle()) * rho * sin(phi)) / rho; + car.getVelocity() * sin(car.getAngle()) * rho * sin(phi)) / + rho; - radarMarker marker = radarMarker( - rho + noise(0.3, timestamp + 2), - phi + noise(0.03, timestamp + 3), - rhoDot + noise(0.3, timestamp + 4)); + radarMarker marker = radarMarker(rho + noise(0.3, timestamp + 2), + phi + noise(0.03, timestamp + 3), + rhoDot + noise(0.3, timestamp + 4)); if (visualize) { - viewer->addLine(pcl::PointXYZ(ego.getPosition().x, ego.getPosition().y, 3.0), - pcl::PointXYZ(ego.getPosition().x + marker.rho * cos(marker.phi), - ego.getPosition().y + marker.rho *sin(marker.phi), - 3.0), - 1, 0, 1, car.getName() + "_rho"); - viewer->addArrow( - pcl::PointXYZ( - ego.getPosition().x + marker.rho * cos (marker.phi), - ego.getPosition().y + marker.rho * sin(marker.phi), - 3.0 - ), - pcl::PointXYZ( - ego.getPosition().x + marker.rho * cos(marker.phi) + marker.rhoDot * cos(marker.phi), - ego.getPosition().y + marker.rho * sin(marker.phi) + marker.rhoDot * sin(marker.phi), - 3.0 - ), 1, 0, 1, false, car.getName() + "_rhoDot"); + viewer->addLine( + pcl::PointXYZ(ego.getPosition().x, ego.getPosition().y, 3.0), + pcl::PointXYZ(ego.getPosition().x + marker.rho * cos(marker.phi), + ego.getPosition().y + marker.rho * sin(marker.phi), 3.0), + 1, 0, 1, car.getName() + "_rho"); + viewer->addArrow( + pcl::PointXYZ(ego.getPosition().x + marker.rho * cos(marker.phi), + ego.getPosition().y + marker.rho * sin(marker.phi), 3.0), + pcl::PointXYZ(ego.getPosition().x + marker.rho * cos(marker.phi) + + marker.rhoDot * cos(marker.phi), + ego.getPosition().y + marker.rho * sin(marker.phi) + + marker.rhoDot * sin(marker.phi), + 3.0), + 1, 0, 1, false, car.getName() + "_rhoDot"); } MeasurementPackage measPackage; @@ -72,46 +81,48 @@ radarMarker Tools::radarSense(Car& car, Car ego, pcl::visualization::PCLVisualiz return marker; } -void Tools::trackerResults(Car car, pcl::visualization::PCLVisualizer::Ptr &viewer, double time, int steps) { +void Tools::trackerResults(Car car, + pcl::visualization::PCLVisualizer::Ptr &viewer, + double time, int steps) { Tracker tracker = car.getTracker(); - viewer->addSphere( - pcl::PointXYZ(tracker.x_[0],tracker.x_[1],3.5), - 0.5, 0, 1, 0,car.getName()+"_tracker"); + viewer->addSphere(pcl::PointXYZ(tracker.x_[0], tracker.x_[1], 3.5), 0.5, 0, 1, + 0, car.getName() + "_tracker"); viewer->addArrow( pcl::PointXYZ(tracker.x_[0], tracker.x_[1], 3.5), pcl::PointXYZ(tracker.x_[0] + tracker.x_[2] * cos(tracker.x_[3]), tracker.x_[1] + tracker.x_[2] * sin(tracker.x_[3]), 3.5), - 0, 1, 0, false, car.getName()+"_tracker_vel"); + 0, 1, 0, false, car.getName() + "_tracker_vel"); if (time > 0) { double dt = time / steps; double ct = dt; while (ct <= time) { tracker.Prediction(dt); - viewer->addSphere(pcl::PointXYZ(tracker.x_[0], tracker.x_[1], 3.5), - 0.5, 0, 1, 0, car.getName() + "_ukf" + std::to_string(ct)); - viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, - 1.0 - 0.8 * (ct / time), - car.getName() + "_ukf" + std::to_string(ct)); + viewer->addSphere(pcl::PointXYZ(tracker.x_[0], tracker.x_[1], 3.5), 0.5, + 0, 1, 0, car.getName() + "_ukf" + std::to_string(ct)); + viewer->setShapeRenderingProperties( + pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0 - 0.8 * (ct / time), + car.getName() + "_ukf" + std::to_string(ct)); viewer->addArrow( - pcl::PointXYZ(tracker.x_[0], tracker.x_[1],3.5), + pcl::PointXYZ(tracker.x_[0], tracker.x_[1], 3.5), pcl::PointXYZ(tracker.x_[0] + tracker.x_[2] * cos(tracker.x_[3]), - tracker.x_[1] + tracker.x_[2] * sin(tracker.x_[3]), 3.5), - 0, 1, 0, false, car.getName() + "_ukf_vel"+std::to_string(ct)); - viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, - 1.0-0.8*(ct/time), - car.getName() + "_ukf_vel"+std::to_string(ct)); + tracker.x_[1] + tracker.x_[2] * sin(tracker.x_[3]), + 3.5), + 0, 1, 0, false, car.getName() + "_ukf_vel" + std::to_string(ct)); + viewer->setShapeRenderingProperties( + pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0 - 0.8 * (ct / time), + car.getName() + "_ukf_vel" + std::to_string(ct)); ct += dt; } } } -Eigen::VectorXd Tools::calculateRMSE( - const std::vector &estimations, - const std::vector &groundTruth) { +Eigen::VectorXd +Tools::calculateRMSE(const std::vector &estimations, + const std::vector &groundTruth) { Eigen::VectorXd rmse(4); rmse << 0, 0, 0, 0; if (estimations.size() != groundTruth.size() || estimations.size() == 0) { @@ -128,16 +139,19 @@ Eigen::VectorXd Tools::calculateRMSE( return rmse; } -void Tools::savePcd(const typename pcl::PointCloud::Ptr& cloud, std::string filename) { +void Tools::savePcd(const typename pcl::PointCloud::Ptr &cloud, + std::string filename) { pcl::io::savePCDFileASCII(filename, *cloud); - std::cerr << "Saved " << cloud->points.size() << " data points to " << filename << std::endl; + std::cerr << "Saved " << cloud->points.size() << " data points to " + << filename << std::endl; } -pcl::PointCloud::Ptr Tools::loadPcd(const std::string& file) { +pcl::PointCloud::Ptr Tools::loadPcd(const std::string &file) { pcl::PointCloud::Ptr cloud(new pcl::PointCloud); if (pcl::io::loadPCDFile(file, *cloud) == -1) { PCL_ERROR("Couldn't read file \n"); } - std::cerr << "Loaded " << cloud->points.size() << " data points from " << file << std::endl; + std::cerr << "Loaded " << cloud->points.size() << " data points from " << file + << std::endl; return cloud; } \ No newline at end of file diff --git a/Toolkit/tools.hpp b/Toolkit/tools.hpp index 4b780ae..630cafb 100644 --- a/Toolkit/tools.hpp +++ b/Toolkit/tools.hpp @@ -5,11 +5,11 @@ #ifndef NFS_TOOLS_HPP #define NFS_TOOLS_HPP -#include -#include #include "../Objects/car.hpp" #include "../Sensors/sensors.hpp" +#include #include +#include struct lidarMarker { double x, y; @@ -18,7 +18,8 @@ struct lidarMarker { struct radarMarker { double rho, phi, rhoDot; - radarMarker(double rho, double phi, double rhoDot) : rho(rho), phi(phi), rhoDot(rhoDot) {} + radarMarker(double rho, double phi, double rhoDot) + : rho(rho), phi(phi), rhoDot(rhoDot) {} }; class Tools { @@ -28,11 +29,19 @@ class Tools { double noise(double stdDev, long long seedNum); std::vector estimations; std::vector groundTruth; - lidarMarker lidarSense(Car& car, pcl::visualization::PCLVisualizer::Ptr& viewer, long long timestamp, bool visualize); - radarMarker radarSense(Car& car, Car ego, pcl::visualization::PCLVisualizer::Ptr& viewer, long long timestamp, bool visualize); - void trackerResults(Car car, pcl::visualization::PCLVisualizer::Ptr& viewer, double time, int steps); - static Eigen::VectorXd calculateRMSE(const std::vector& estimations, const std::vector& groundTruth); - void savePcd(const typename pcl::PointCloud::Ptr& cloud, std::string file); - pcl::PointCloud::Ptr loadPcd(const std::string& file); + lidarMarker lidarSense(Car &car, + pcl::visualization::PCLVisualizer::Ptr &viewer, + long long timestamp, bool visualize); + radarMarker radarSense(Car &car, Car ego, + pcl::visualization::PCLVisualizer::Ptr &viewer, + long long timestamp, bool visualize); + void trackerResults(Car car, pcl::visualization::PCLVisualizer::Ptr &viewer, + double time, int steps); + static Eigen::VectorXd + calculateRMSE(const std::vector &estimations, + const std::vector &groundTruth); + void savePcd(const typename pcl::PointCloud::Ptr &cloud, + std::string file); + pcl::PointCloud::Ptr loadPcd(const std::string &file); }; #endif // NFS_TOOLS_HPP diff --git a/View/camera.cpp b/View/camera.cpp index 92de562..8a20559 100644 --- a/View/camera.cpp +++ b/View/camera.cpp @@ -3,12 +3,21 @@ // #include "camera.hpp" -void changeCameraView(CameraAngle setAngle, pcl::visualization::PCLVisualizer::Ptr& viewer) { +void changeCameraView(CameraAngle setAngle, + pcl::visualization::PCLVisualizer::Ptr &viewer) { int distance = 50; switch (setAngle) { - case XY : viewer->setCameraPosition(-distance, -distance, distance, 1, 1, 0); break; - case TopDown : viewer->setCameraPosition(0, 0, distance, 1, 0, 1); break; - case Side : viewer->setCameraPosition(0, -distance, 0, 0, 0, 1); break; - case FPS : viewer->setCameraPosition(-10, 0, 0, 0, 0, 1); break; + case XY: + viewer->setCameraPosition(-distance, -distance, distance, 1, 1, 0); + break; + case TopDown: + viewer->setCameraPosition(0, 0, distance, 1, 0, 1); + break; + case Side: + viewer->setCameraPosition(0, -distance, 0, 0, 0, 1); + break; + case FPS: + viewer->setCameraPosition(-10, 0, 0, 0, 0, 1); + break; } } \ No newline at end of file diff --git a/View/camera.hpp b/View/camera.hpp index 947fd5f..b2b404e 100644 --- a/View/camera.hpp +++ b/View/camera.hpp @@ -7,7 +7,8 @@ #include -enum CameraAngle {XY, TopDown, Side, FPS}; -void changeCameraView(CameraAngle setAngle, pcl::visualization::PCLVisualizer::Ptr& viewer); +enum CameraAngle { XY, TopDown, Side, FPS }; +void changeCameraView(CameraAngle setAngle, + pcl::visualization::PCLVisualizer::Ptr &viewer); #endif // NFS_CAMERA_HPP diff --git a/main.cpp b/main.cpp index 4be9bc5..fe01deb 100644 --- a/main.cpp +++ b/main.cpp @@ -1,23 +1,24 @@ +#include "Control/controller.hpp" #include "Objects/car.hpp" -#include "View/camera.hpp" #include "Scene/scene.hpp" -#include "Control/controller.hpp" +#include "View/camera.hpp" #include #include int main() { - pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer")); + pcl::visualization::PCLVisualizer::Ptr viewer( + new pcl::visualization::PCLVisualizer("3D Viewer")); viewer->getRenderWindow()->GlobalWarningDisplayOff(); viewer->setBackgroundColor(0, 0, 0); viewer->addCoordinateSystem(1.0); viewer->initCameraParameters(); - int* screenSize = viewer->getRenderWindow()->GetScreenSize(); + int *screenSize = viewer->getRenderWindow()->GetScreenSize(); viewer->setSize(1280, 780); viewer->setCameraPosition(50, 50, 50, // Camera position - 0, 0, 0, // Focal point (origin) - 0, 0, 1, // View up direction - 0); // Set camera viewpoint to (0, 0, 0) + 0, 0, 0, // Focal point (origin) + 0, 0, 1, // View up direction + 0); // Set camera viewpoint to (0, 0, 0) viewer->setCameraFieldOfView(0.8); viewer->setCameraClipDistances(0.01, 500); @@ -27,11 +28,8 @@ int main() { float angle = -180.0; float front_center_distance = 1.0; std::string car_name = "egoCar"; - Car car = Car( - position, dimensions, color, - angle, front_center_distance, - car_name - ); + Car car = + Car(position, dimensions, color, angle, front_center_distance, car_name); EgoCarController carController(viewer, car); carController.registerKeyboardCallbacks(); @@ -41,22 +39,21 @@ int main() { const int interval_us = 100000; float dt = interval_us * 1e-6; Scene scene(viewer); - + while (!viewer->wasStopped()) { - + viewer->removeAllPointClouds(); viewer->removeAllShapes(); carController.handleKeyboardInput(); carController.update(dt, scene); viewer->spinOnce(1); Vect3 car_position = car.getPosition(); - + if (car_position.x < x_min || car_position.x > x_max || car_position.y < y_min || car_position.y > y_max) { viewer->setCameraPosition(car_position.x - 10, car_position.y - 10, car_position.z + 10, car_position.x, - car_position.y, car_position.z, - 0, 0, 1); + car_position.y, car_position.z, 0, 0, 1); viewer->setCameraClipDistances(0.01, 500); x_min = car_position.x - boundary_margin; x_max = car_position.x + boundary_margin; @@ -75,7 +72,8 @@ int main() { int ypos = (screenSize[1] - textHeight); int fontSize = 40; double r = 1.0, g = 1.0, b = 1.0; - viewer->addText("You've won the game!", xpos, ypos, fontSize, r, g, b, "winning text"); + viewer->addText("You've won the game!", xpos, ypos, fontSize, r, g, b, + "winning text"); viewer->spinOnce(3000); break; }