helper_testing.cpp

Go to the documentation of this file.

// GTest headers
#include <gtest/gtest.h>
 
// Local headers
#include "alb_helper.h"
 
TEST(AlbHelper, trackedClassIds)
{
        EXPECT_EQ("UNKNOWN", Helper::get_object_class(0));
        EXPECT_EQ("PERSON", Helper::get_object_class(1));
        EXPECT_EQ("LUGGAGE", Helper::get_object_class(2));
        EXPECT_EQ("TROLLEY", Helper::get_object_class(3));
        EXPECT_EQ("TRUCK", Helper::get_object_class(4));
        EXPECT_EQ("BUS", Helper::get_object_class(5));
        EXPECT_EQ("CAR", Helper::get_object_class(6));
        EXPECT_EQ("VAN", Helper::get_object_class(7));
        EXPECT_EQ("TWO_WHEELER", Helper::get_object_class(8));
        EXPECT_EQ("MASK", Helper::get_object_class(9));
        EXPECT_EQ("NO_MASK", Helper::get_object_class(10));
}
 
TEST(AlbHelper, poseOrientation)
{
        // Identity rotation matrix.
        geometry_msgs::Pose poseI;
        Helper::define_pose(poseI, { 0.0f, 0.0f, 0.0f }, { 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f });
 
        EXPECT_EQ(poseI.orientation.x, 0);
        EXPECT_EQ(poseI.orientation.y, 0);
        EXPECT_EQ(poseI.orientation.z, 0);
        EXPECT_EQ(poseI.orientation.w, 1);
 
        // Rotation of +90degrees along X.
        geometry_msgs::Pose poseX;
        Helper::define_pose(poseX, { 0.0f, 0.0f, 0.0f }, { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, -1.0f, 0.0f });
 
        EXPECT_NEAR(poseX.orientation.x, sqrt(2) / 2, 10e-5);
        EXPECT_EQ(poseX.orientation.y, 0.0f);
        EXPECT_EQ(poseX.orientation.z, 0.0f);
        EXPECT_NEAR(poseX.orientation.w, sqrt(2) / 2, 10e-5);
 
        // Rotation of +90degrees along Y.
        geometry_msgs::Pose poseY;
        Helper::define_pose(poseY, { 0.0f, 0.0f, 0.0f }, { 0.0f, 0.0f, -1.0f, 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 0.0f });
 
        EXPECT_EQ(poseY.orientation.x, 0.0f);
        EXPECT_NEAR(poseY.orientation.y, sqrt(2) / 2, 10e-5);
        EXPECT_EQ(poseY.orientation.z, 0.0f);
        EXPECT_NEAR(poseY.orientation.w, sqrt(2) / 2, 10e-5);
 
        // Rotation of +90degrees along Z.
        geometry_msgs::Pose poseZ;
        Helper::define_pose(poseZ, { 0.0f, 0.0f, 0.0f }, { 0.0f, 1.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f });
 
        EXPECT_EQ(poseZ.orientation.x, 0.0f);
        EXPECT_EQ(poseZ.orientation.y, 0.0f);
        EXPECT_NEAR(poseZ.orientation.z, sqrt(2) / 2, 10e-5);
        EXPECT_NEAR(poseZ.orientation.w, sqrt(2) / 2, 10e-5);
}
 
TEST(AlbHelper, unorganizedPointCloudMessage)
{
        float points[6] = { 0.0, 1.0, 2.0, 3.0, 4.0, 5.0 };
 
        // Test with an unorganized point cloud.
        sensor_msgs::PointCloud2 pointCloudUnorganized;
        Helper::define_points_data(pointCloudUnorganized, 0, 2, points, true);
 
        EXPECT_EQ(pointCloudUnorganized.width, 2);
        EXPECT_EQ(pointCloudUnorganized.height, 1);
        EXPECT_EQ(pointCloudUnorganized.data.size(), 2 * 3 * sizeof(float));
 
        float *recast_points = reinterpret_cast<float *>(pointCloudUnorganized.data.data());
        EXPECT_EQ(recast_points[0], 0.0);
        EXPECT_EQ(recast_points[1], 1.0);
        EXPECT_EQ(recast_points[2], 2.0);
        EXPECT_EQ(recast_points[3], 3.0);
        EXPECT_EQ(recast_points[4], 4.0);
        EXPECT_EQ(recast_points[5], 5.0);
}
 
TEST(AlbHelper, organizedPointCloudMessage)
{
        float points[6] = { 0.0, 1.0, 2.0, 3.0, 4.0, 5.0 };
 
        // Test with an organized point cloud.
        sensor_msgs::PointCloud2 pointCloudOrganized;
        Helper::define_points_data(pointCloudOrganized, 2, 2, points, true);
 
        EXPECT_EQ(pointCloudOrganized.width, 1);
        EXPECT_EQ(pointCloudOrganized.height, 2);
        EXPECT_EQ(pointCloudOrganized.data.size(), 2 * 3 * sizeof(float));
}
 
TEST(AlbHelper, organizedPointCloudMessageRowwiseOrdering)
{
        constexpr uint32_t layers = 2;
        constexpr uint32_t points = 6;
        float data[points * 3]; // 3 numbers per points
        for (int i = 0; i < points; ++i) {
                data[3 * i] = float(i);
                data[3 * i + 1] = float(i);
                data[3 * i + 2] = float(i);
        }
 
        // Test with an organized point cloud.
        sensor_msgs::PointCloud2 pointCloudOrganized;
 
        Helper::define_points_data(pointCloudOrganized, layers, points, data, false);
 
        EXPECT_EQ(pointCloudOrganized.width, 3);
        EXPECT_EQ(pointCloudOrganized.height, 2);
        EXPECT_EQ(pointCloudOrganized.data.size(), points * 3 * sizeof(float));
 
        // The initial data is:
        // 0 2 4
        // 1 3 5
        // Colwise we should have: 0 1 2 3 4 5
        // Rowwise we should have: 0 2 4 1 3 5
        float expect[points] = { 0.f, 2.f, 4.f, 1.f, 3.f, 5.f };
        float *recast_points = reinterpret_cast<float *>(pointCloudOrganized.data.data());
        for (int i = 0; i < points; ++i) {
                EXPECT_EQ(recast_points[3 * i], expect[i]);
                EXPECT_EQ(recast_points[3 * i + 1], expect[i]);
                EXPECT_EQ(recast_points[3 * i + 2], expect[i]);
        }
}
 
TEST(AlbHelper, pointsFields)
{
        sensor_msgs::PointCloud2 pointCloud;
        Helper::define_point_fields(pointCloud);
 
        EXPECT_EQ(pointCloud.fields.size(), 3);
}