Make everything const that isn't at 3 on the tree

source/armarx/navigation/human/test/human_tracker_test.cpp

@@ -60,58 +60,58 @@ namespace armarx::navigation::human
 
         // PARAMETERS
 
-        Eigen::Vector3f initialPosition(0, 0, 2);
-        float orientation = M_PI_2;
-        Eigen::Quaternionf orientationQuat = quatFromEuler(0, 0, orientation);
+        const Eigen::Vector3f initialPosition(0, 0, 2);
+        const float orientation = M_PI_2;
+        const Eigen::Quaternionf orientationQuat = quatFromEuler(0, 0, orientation);
 
-        std::int64_t timestepMs = 100;
-        Eigen::Vector3f movementSpeedMetPerSec(1, 0, 0);
-        int cameraSteps = 10;
+        const std::int64_t timestepMs = 100;
+        const Eigen::Vector3f movementSpeedMetPerSec(1, 0, 0);
+        const int cameraSteps = 10;
 
 
         // CAMERA MEASUREMENTS
 
-        Eigen::Vector3f posDelta =
+        const Eigen::Vector3f posDelta =
             movementSpeedMetPerSec * (static_cast<float>(timestepMs) / 1000.0);
 
-        std::vector<CamMm> camMeasurements = std::vector<CamMm>();
+        const std::vector<CamMm> camMeasurements = std::vector<CamMm>();
         for (int i = 0; i < cameraSteps; i++)
         {
-            DateTime t = DateTime(Duration::MilliSeconds(i * timestepMs));
-
-            Eigen::Vector3f newPos = initialPosition + i * posDelta;
-            FramedPosition headPosition = FramedPosition(newPos, "", "");
-            FramedOrientation headOrientation = FramedOrientation(orientationQuat, "", "");
-            PoseKeypoint head = {.label = "HEAD",
-                                 .confidence = 0.95,
-                                 .positionGlobal = headPosition,
-                                 .orientationGlobal = headOrientation};
-            HumanPose pose = {"posemodelid", {{"HEAD", head}}};
-            std::vector<armem::human::HumanPose> humanPoses = {pose};
-            CamMm camMm = {t, humanPoses};
+            const DateTime t = DateTime(Duration::MilliSeconds(i * timestepMs));
+
+            const Eigen::Vector3f newPos = initialPosition + i * posDelta;
+            const FramedPosition headPosition = FramedPosition(newPos, "", "");
+            const FramedOrientation headOrientation = FramedOrientation(orientationQuat, "", "");
+            const PoseKeypoint head = {.label = "HEAD",
+                                       .confidence = 0.95,
+                                       .positionGlobal = headPosition,
+                                       .orientationGlobal = headOrientation};
+            const HumanPose pose = {"posemodelid", {{"HEAD", head}}};
+            const std::vector<armem::human::HumanPose> humanPoses = {pose};
+            const CamMm camMm = {t, humanPoses};
 
             tracker.update(camMm);
         }
 
         // LASER MEASUREMENT
 
-        DateTime tLaser = DateTime(Duration::MilliSeconds(cameraSteps * timestepMs));
+        const DateTime tLaser = DateTime(Duration::MilliSeconds(cameraSteps * timestepMs));
 
         using Point = Eigen::Vector2f;
 
-        std::vector<Point> relOffsets = {Point(0.01, 0.003),
-                                         Point(-0.02, 0.007),
-                                         Point(-0.01, -0.01),
-                                         Point(0.015, 0.009),
-                                         Point(0.002, 0.001)};
+        const std::vector<Point> relOffsets = {Point(0.01, 0.003),
+                                               Point(-0.02, 0.007),
+                                               Point(-0.01, -0.01),
+                                               Point(0.015, 0.009),
+                                               Point(0.002, 0.001)};
 
-        Eigen::Vector3f laserPos = initialPosition + cameraSteps * posDelta;
-        Eigen::Vector3f leftFootPos = laserPos + Point(0, 0.1);
-        Eigen::Vector3f rightFootPos = laserPos + Point(0, -0.1);
+        const Eigen::Vector3f laserPos = initialPosition + cameraSteps * posDelta;
+        const Eigen::Vector3f leftFootPos = laserPos + Point(0, 0.1);
+        const Eigen::Vector3f rightFootPos = laserPos + Point(0, -0.1);
 
         std::vector<Point> leftFootPoints = std::vector<Point>();
         std::vector<Point> rightFootPoints = std::vector<Point>();
-        for (Point& offset : relOffsets)
+        for (const Point& offset : relOffsets)
         {
             leftFootPoints.emplace_back(leftFootPos + offset);
             rightFootPoints.emplace_back(rightFootPos + offset);
@@ -126,20 +126,21 @@ namespace armarx::navigation::human
         rightFootPose.translation() = rightFootPos;
         rightFootPose.linear() = Eigen::Rotation2Df(orientation).toRotationMatrix();
 
-        Ellipsoid leftFootEllipse = {.pose = leftFootPose, .radii = Point(0.05, 0.04)};
-        Ellipsoid rightFootEllipse = {.pose = rightFootPose, .radii = Point(0.05, 0.04)};
+        const Ellipsoid leftFootEllipse = {.pose = leftFootPose, .radii = Point(0.05, 0.04)};
+        const Ellipsoid rightFootEllipse = {.pose = rightFootPose, .radii = Point(0.05, 0.04)};
 
-        Cluster leftFootCluster = {.ellipsoid = leftFootEllipse, .points = leftFootPoints};
-        Cluster rightFootCluster = {.ellipsoid = rightFootEllipse, .points = rightFootPoints};
+        const Cluster leftFootCluster = {.ellipsoid = leftFootEllipse, .points = leftFootPoints};
+        const Cluster rightFootCluster = {.ellipsoid = rightFootEllipse, .points = rightFootPoints};
 
-        std::vector<Cluster> clusters = std::vector<Cluster>{leftFootCluster, rightFootCluster};
+        const std::vector<Cluster> clusters =
+            std::vector<Cluster>{leftFootCluster, rightFootCluster};
 
-        LaserMm laserMm = {.detectionTime = tLaser, .clusters = clusters};
+        const LaserMm laserMm = {.detectionTime = tLaser, .clusters = clusters};
         tracker.update(laserMm);
 
-        std::vector<Human> humans = tracker.getTrackedHumans();
+        const std::vector<Human> humans = tracker.getTrackedHumans();
         BOOST_CHECK_EQUAL(humans.size(), 1); // should have detected the one human through laser
-        Human h = humans.at(0);
+        const Human h = humans.at(0);
         BOOST_CHECK_CLOSE(h.pose.translation().x(), 1.0, 0.05);
         BOOST_CHECK(h.pose.translation().y() < 0.05);
         BOOST_CHECK_CLOSE(h.linearVelocity.x(), 1.0, 0.2);