visualize track

AndreiMoraru123 · Jun 20, 2023 · 01117ca · 01117ca
1 parent d864e27
commit 01117ca
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 16 deletions.
diff --git 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
@@ -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<Control> Scene::randomControlInstructions(std::mt19937& gen, Car& car, int numInstructions) {
   std::vector<Control> instructions;
-
   for(int i=0; i<numInstructions; ++i) {
     instructions.push_back(randomControl(gen, car));
   }
-
   return instructions;
 }
 
@@ -171,8 +169,6 @@ void Scene::stepScene(Car& egoCar, double dt, long long timestamp, pcl::visualiz
     car.move(dt, timestamp);
     car.render(viewer);
 
-    if (!visualize_pcd) {}
-
     if (trackCars[i]) {
       Eigen::VectorXd gt(4);
       gt << car.getPosition().x, car.getPosition().y,
@@ -182,7 +178,9 @@ void Scene::stepScene(Car& egoCar, double dt, long long timestamp, pcl::visualiz
       tools.groundTruth.push_back(gt);
       tools.lidarSense(car, viewer, timestamp, visualize_lidar);
       tools.radarSense(car, egoCar, viewer, timestamp, visualize_radar);
-      tools.trackerResults(car, viewer, projectedTime, projectedSteps);
+      if (visualize_track) {
+        tools.trackerResults(car, viewer, projectedTime, projectedSteps);
+      }
       Eigen::VectorXd estimate(4);
       double v = traffic[i].getTracker().x_(2);
       double yaw = traffic[i].getTracker().x_(3);

diff --git a/Scene/scene.hpp b/Scene/scene.hpp
@@ -28,9 +28,9 @@ class Scene {
   std::set<std::string> 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
@@ -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) {