diff --git a/Algo/tracker.cpp b/Algo/tracker.cpp index 036eeee..33b5df8 100644 --- a/Algo/tracker.cpp +++ b/Algo/tracker.cpp @@ -8,10 +8,10 @@ Tracker::Tracker() { x_ = Eigen::VectorXd(5); // initial state vector P_ = Eigen::MatrixXd(5, 5); // initial covariance matrix - std_a_ = 2.0; // Process noise standard deviation longitudinal acceleration in m/s^2 + 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_ = 0.15; // Laser measurement noise standard deviation position1 in m - std_laspy_ = 0.15; // Laser measurement noise standard deviation position2 in m + 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 diff --git a/Scene/scene.cpp b/Scene/scene.cpp index 82786da..2dda393 100644 --- a/Scene/scene.cpp +++ b/Scene/scene.cpp @@ -87,8 +87,8 @@ Scene::Scene(pcl::visualization::PCLVisualizer::Ptr& viewer) { } Control Scene::randomControl(std::mt19937& gen, Car& car) { - std::uniform_real_distribution<> disTime(1, 9); - std::uniform_real_distribution<> disAcceleration(-2, 1); + std::uniform_real_distribution<> disTime(1, 8); + std::uniform_real_distribution<> disAcceleration(-5, 5); std::uniform_real_distribution<> disSteering(-0.15, 0.15); Vect3 currentPosition = car.getPosition(); @@ -118,16 +118,14 @@ Control Scene::randomControl(std::mt19937& gen, Car& car) { } } while (!withinBounds); - return Control(time, acceleration, steering); + return Control(time, acceleration * 0.25, steering); } std::vector Scene::randomControlInstructions(std::mt19937& gen, Car& car, int numInstructions) { std::vector instructions; - for(int i=0; i parkedSpots; bool visualize_lidar = true; bool visualize_radar = true; - bool visualize_pcd = false; - double projectedTime = 0; - int projectedSteps = 0; + 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); diff --git a/Toolkit/tools.cpp b/Toolkit/tools.cpp index d12b555..0fbffa6 100644 --- a/Toolkit/tools.cpp +++ b/Toolkit/tools.cpp @@ -81,9 +81,9 @@ void Tools::trackerResults(Car car, pcl::visualization::PCLVisualizer::Ptr &view 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_[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), + tracker.x_[1] + tracker.x_[2] * sin(tracker.x_[3]), 3.5), 0, 1, 0, false, car.getName()+"_tracker_vel"); if (time > 0) {