multisense_utilities_test.cc

Go to the documentation of this file.

 
#include <cmath>
 
#include <gtest/gtest.h>
 
#include <MultiSense/MultiSenseUtilities.hh>
 
using namespace multisense;
 
//
// Create a disparity image of a circular disk centered in the image
//
Image create_example_disparity_image(const CameraCalibration &left_calibration,
                                     const CameraCalibration &right_calibration,
                                     double obstacle_radius_m,
                                     double obstacle_distance_m,
                                     size_t width,
                                     size_t height)
{
    const double left_fx = left_calibration.P[0][0];
    const double left_cx = left_calibration.P[0][2];
    const double left_fy = left_calibration.P[1][1];
    const double left_cy = left_calibration.P[1][2];
 
    const double right_fx = right_calibration.P[0][0];
    const double right_cx = right_calibration.P[0][2];
    const double tx = right_calibration.P[0][3] / right_calibration.P[0][0];
 
    const double obstacle_distance_m2 = obstacle_distance_m * obstacle_distance_m;
    const double obstacle_radius_m2 = obstacle_radius_m * obstacle_radius_m;
 
    std::vector<uint8_t> data(width * height * sizeof(uint16_t), 0);
 
    uint16_t* raw_data = reinterpret_cast<uint16_t*>(data.data());
 
    for (size_t v = 0 ; v < height ; ++v)
    {
        for (size_t u = 0 ; u < width ; ++u)
        {
            // convert pixel to ray
            const double ray_x = (u / left_fx) - (left_cx / left_fx);
            const double ray_y = (v / left_fy) - (left_cy / left_fy);
 
            // scale ray so that z = obstacle_distance_m and test if we are within the disc radius compute our disparity
            if (((ray_x * ray_x * obstacle_distance_m2) + (ray_y * ray_y * obstacle_distance_m2) ) < obstacle_radius_m2)
            {
                // project into the right image. Note v will be the same
                const double right_u = ((right_fx * ray_x * obstacle_distance_m) + (right_cx * obstacle_distance_m) + (tx * right_fx)) / obstacle_distance_m;
 
                const uint16_t disparity = static_cast<uint16_t>((static_cast<double>(u) - right_u) * 16.0);
 
                raw_data[v * width + u] = disparity;
            }
        }
    }
 
    return Image{std::make_shared<const std::vector<uint8_t>>(data),
                 0,
                 width * height * sizeof(uint16_t),
                 Image::PixelFormat::MONO16,
                 static_cast<int>(width),
                 static_cast<int>(height),
                 {},
                 {},
                 DataSource::LEFT_DISPARITY_RAW,
                 left_calibration};
}
 
TEST(QMatrix, reproject)
{
    const float fx = 1500.0;
    const float fy = 1000.0;
    const float cx = 960.0;
    const float cy = 600.0;
    const float tx = -0.27;
 
    CameraCalibration left_calibration{
        {{{fx, 0.0, cx}, {0.0, fy, cy}, {0.0, 0.0, 1.0}}},
        {{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}},
        {{{fx, 0.0, cx, 0.0}, {0.0, fy, cy, 0.0}, {0.0, 0.0, 1.0, 0.0}}},
        CameraCalibration::DistortionType::NONE,
        {}};
 
    CameraCalibration right_calibration{
        {{{fx, 0.0, cx}, {0.0, fy, cy}, {0.0, 0.0, 1.0}}},
        {{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}},
        {{{fx, 0.0, cx, fx * tx}, {0.0, fy, cy, 0.0}, {0.0, 0.0, 1.0, 0.0}}},
        CameraCalibration::DistortionType::NONE,
        {}};
 
    QMatrix q{left_calibration, right_calibration};
 
    // Project a dummy point into the left/right camera
    const double x = 0.5;
    const double y = 1.5;
    const double z = 5.5;
 
    // Left image
    const double left_u = ((fx * x) + (cx * z)) / z;
    const double left_v = ((fy * y) + (cy * z)) / z;
 
    // right image
    const double right_u = ((fx * x) + (cx * z) + (tx * fx)) / z;
 
    const double disparity = left_u - right_u;
 
    const auto repojected_pt = q.reproject(Pixel{static_cast<size_t>(std::round(left_u)), static_cast<size_t>(std::round(left_v))}, disparity);
 
    const double epsilon = 1e-2;
    EXPECT_NEAR(x, repojected_pt.x, epsilon);
    EXPECT_NEAR(y, repojected_pt.y, epsilon);
    EXPECT_NEAR(z, repojected_pt.z, epsilon);
}
 
TEST(create_depth_image, mono_and_float)
{
    const float fx = 1000.0;
    const float fy = 1000.0;
    const float cx = 960.0;
    const float cy = 600.0;
    const float tx = -0.27;
    const float aux_tx = -0.033;
 
    CameraCalibration left_calibration{
        {{{fx, 0.0, cx}, {0.0, fy, cy}, {0.0, 0.0, 1.0}}},
        {{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}},
        {{{fx, 0.0, cx, 0.0}, {0.0, fy, cy, 0.0}, {0.0, 0.0, 1.0, 0.0}}},
        CameraCalibration::DistortionType::NONE,
        {}};
 
    CameraCalibration right_calibration{
        {{{fx, 0.0, cx}, {0.0, fy, cy}, {0.0, 0.0, 1.0}}},
        {{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}},
        {{{fx, 0.0, cx, fx * tx}, {0.0, fy, cy, 0.0}, {0.0, 0.0, 1.0, 0.0}}},
        CameraCalibration::DistortionType::NONE,
        {}};
 
    CameraCalibration aux_calibration{
        {{{fx, 0.0, cx}, {0.0, fy, cy}, {0.0, 0.0, 1.0}}},
        {{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}},
        {{{fx, 0.0, cx, fx * aux_tx}, {0.0, fy, cy, 0.0}, {0.0, 0.0, 1.0, 0.0}}},
        CameraCalibration::DistortionType::NONE,
        {}};
 
    const double disk_distance_m = 4.0;
 
    const auto disparity_image = create_example_disparity_image(left_calibration, right_calibration, 3.0, disk_distance_m, 1920, 1200);
 
    ImageFrame frame{0, {}, StereoCalibration{left_calibration, right_calibration, aux_calibration}, {}, {}, {}, {}, {}, {}};
    frame.add_image(disparity_image);
 
    const float invalid = 7000.0;
 
    const auto float_depth_image = create_depth_image(frame,
                                                      Image::PixelFormat::FLOAT32,
                                                      DataSource::LEFT_DISPARITY_RAW,
                                                      false,
                                                      invalid);
 
    const auto aux_float_depth_image = create_depth_image(frame,
                                                          Image::PixelFormat::FLOAT32,
                                                          DataSource::LEFT_DISPARITY_RAW,
                                                          true,
                                                          invalid);
 
    const auto mono16_depth_image = create_depth_image(frame,
                                                       Image::PixelFormat::MONO16,
                                                       DataSource::LEFT_DISPARITY_RAW,
                                                       false,
                                                       invalid);
 
    ASSERT_TRUE(float_depth_image);
    ASSERT_TRUE(mono16_depth_image);
    ASSERT_TRUE(aux_float_depth_image);
 
    for (int v = 0 ; v < disparity_image.height ; ++v)
    {
        for (int u = 0 ; u < disparity_image.width ; ++u)
        {
            if (disparity_image.at<uint16_t>(u, v).value() == 0)
            {
                EXPECT_DOUBLE_EQ(float_depth_image->at<float>(u, v).value(), invalid);
                EXPECT_EQ(mono16_depth_image->at<uint16_t>(u, v).value(), invalid);
            }
            else
            {
                EXPECT_DOUBLE_EQ(float_depth_image->at<float>(u, v).value(), disk_distance_m);
 
                // depths are in mm
                EXPECT_EQ(mono16_depth_image->at<uint16_t>(u, v).value(), static_cast<uint16_t>(disk_distance_m * 1000));
 
                // compute our aux pixel location for this depth measurement
                const int aux_u = u - static_cast<int>((aux_tx/ tx) * disparity_image.at<uint16_t>(u, v).value() / 16.0);
                EXPECT_EQ(aux_float_depth_image->at<float>(aux_u, v).value(), disk_distance_m);
            }
        }
    }
}
 
TEST(get_aux_3d_point, basic_tests)
{
    const float fx = 1000.0;
    const float fy = 1000.0;
    const float cx = 960.0;
    const float cy = 600.0;
    const float tx = -0.27;
    const float aux_tx = -0.033;
 
    CameraCalibration left_calibration{
        {{{fx, 0.0, cx}, {0.0, fy, cy}, {0.0, 0.0, 1.0}}},
        {{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}},
        {{{fx, 0.0, cx, 0.0}, {0.0, fy, cy, 0.0}, {0.0, 0.0, 1.0, 0.0}}},
        CameraCalibration::DistortionType::NONE,
        {}};
 
    CameraCalibration right_calibration{
        {{{fx, 0.0, cx}, {0.0, fy, cy}, {0.0, 0.0, 1.0}}},
        {{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}},
        {{{fx, 0.0, cx, fx * tx}, {0.0, fy, cy, 0.0}, {0.0, 0.0, 1.0, 0.0}}},
        CameraCalibration::DistortionType::NONE,
        {}};
 
    CameraCalibration aux_calibration{
        {{{fx, 0.0, cx}, {0.0, fy, cy}, {0.0, 0.0, 1.0}}},
        {{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}},
        {{{fx, 0.0, cx, fx * aux_tx}, {0.0, fy, cy, 0.0}, {0.0, 0.0, 1.0, 0.0}}},
        CameraCalibration::DistortionType::NONE,
        {}};
 
    const double disk_distance_m = 4.0;
 
    const auto disparity_image = create_example_disparity_image(left_calibration, right_calibration, 3.0, disk_distance_m, 1920, 1200);
 
    ImageFrame frame{0, {}, StereoCalibration{left_calibration, right_calibration, aux_calibration}, {}, {}, {}, {}, {}, {}};
    frame.add_image(disparity_image);
 
    const auto invalid_point = get_aux_3d_point(frame, Pixel{0, 0}, 100, 0.01);
 
    ASSERT_FALSE(invalid_point);
 
    const auto valid_point = get_aux_3d_point(frame, Pixel{static_cast<size_t>(cx), static_cast<size_t>(cy)}, 1000, 0.5);
 
    ASSERT_TRUE(valid_point);
 
    ASSERT_DOUBLE_EQ(valid_point->z, disk_distance_m);
}
 
TEST(create_bgr_from_ycbcr420, gray_image)
{
    const float fx = 1000.0;
    const float fy = 1000.0;
    const float cx = 960.0;
    const float cy = 600.0;
 
    CameraCalibration aux_calibration{
        {{{fx, 0.0, cx}, {0.0, fy, cy}, {0.0, 0.0, 1.0}}},
        {{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}},
        {{{fx, 0.0, cx, 0.0}, {0.0, fy, cy, 0.0}, {0.0, 0.0, 1.0, 0.0}}},
        CameraCalibration::DistortionType::NONE,
        {}};
 
    const size_t width = 1920;
    const size_t height = 1200;
 
    const uint8_t val = 42;
 
    std::vector<uint8_t> y_data(width * height, val);
 
    // Values of 128 for cb/cr result in 0 values for the corresponding color pixels
    std::vector<uint8_t> cbcr_data(width * height/2, 128);
 
    const Image y{std::make_shared<const std::vector<uint8_t>>(std::move(y_data)),
                  0,
                  width * height,
                  Image::PixelFormat::MONO8,
                  static_cast<int>(width),
                  static_cast<int>(height),
                  {},
                  {},
                  DataSource::AUX_LUMA_RAW,
                  aux_calibration};
 
    const Image cbcr{std::make_shared<const std::vector<uint8_t>>(std::move(cbcr_data)),
                     0,
                     width / 2 * height / 2,
                     Image::PixelFormat::MONO16,
                     static_cast<int>(width/2),
                     static_cast<int>(height/2),
                     {},
                     {},
                     DataSource::AUX_CHROMA_RAW,
                     aux_calibration};
 
    const auto bgr_image = create_bgr_from_ycbcr420(y, cbcr, DataSource::AUX_RAW);
 
    ASSERT_TRUE(bgr_image);
    ASSERT_EQ(bgr_image->width , static_cast<int>(width));
    ASSERT_EQ(bgr_image->height , static_cast<int>(height));
 
    for (size_t h = 0 ; h < height ; ++h)
    {
        for (size_t w = 0 ; w < width ; ++w)
        {
            const auto pixel = bgr_image->at<std::array<uint8_t, 3>>(w, h);
            ASSERT_TRUE(pixel);
            ASSERT_EQ(pixel->at(0), val);
            ASSERT_EQ(pixel->at(1), val);
            ASSERT_EQ(pixel->at(2), val);
        }
    }
}
 
TEST(create_pointcloud, basic_tests)
{
    const float fx = 1000.0;
    const float fy = 1000.0;
    const float cx = 960.0;
    const float cy = 600.0;
    const float tx = -0.27;
 
    CameraCalibration left_calibration{
        {{{fx, 0.0, cx}, {0.0, fy, cy}, {0.0, 0.0, 1.0}}},
        {{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}},
        {{{fx, 0.0, cx, 0.0}, {0.0, fy, cy, 0.0}, {0.0, 0.0, 1.0, 0.0}}},
        CameraCalibration::DistortionType::NONE,
        {}};
 
    CameraCalibration right_calibration{
        {{{fx, 0.0, cx}, {0.0, fy, cy}, {0.0, 0.0, 1.0}}},
        {{{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}},
        {{{fx, 0.0, cx, fx * tx}, {0.0, fy, cy, 0.0}, {0.0, 0.0, 1.0, 0.0}}},
        CameraCalibration::DistortionType::NONE,
        {}};
 
    const double disk_distance_m = 4.0;
 
    const auto disparity_image = create_example_disparity_image(left_calibration, right_calibration, 3.0, disk_distance_m, 1920, 1200);
 
    ImageFrame frame{0, {}, StereoCalibration{left_calibration, right_calibration, std::nullopt}, {}, {}, {}, {}, {}, {}};
    frame.add_image(disparity_image);
 
    const auto point_cloud = create_pointcloud(frame, disk_distance_m + 3.0);
 
    ASSERT_TRUE(point_cloud);
    ASSERT_TRUE(!point_cloud->cloud.empty());
 
    for (const auto &point : point_cloud->cloud)
    {
        EXPECT_TRUE(point.z == disk_distance_m);
    }
 
    const auto point_cloud_empty = create_pointcloud(frame, disk_distance_m - 0.1);
 
    ASSERT_TRUE(point_cloud_empty);
    ASSERT_TRUE(point_cloud_empty->cloud.empty());
}