360toPerspective/cpp/src/tests.cpp

#include <iostream>
#include <Camera.hpp>
#include <Image.hpp>
#include <Slicer360ToPerspective.hpp>
#include <frame_conversions.hpp>

#include <test_framework.hpp>

int main() {
    std::cout.precision(16);
    std::cout << "Running Unit Tests\n";
    
    if(0){// frame transforms
        std::cout << "frame transforms\n";
        // PRINT_VAR(sph2equirectangular_d(Eigen::Vector3d(1.0, 0., 0.), 100, 50).transpose());
        // PRINT_VAR(sph2equirectangular_d(Eigen::Vector3d(1.0, 0., 1.), 100, 50).transpose());
        // PRINT_VAR(sph2equirectangular_d(Eigen::Vector3d(1.0, M_PI/2.0, 0.), 100, 50).transpose());
        // PRINT_VAR(sph2equirectangular_d(Eigen::Vector3d(1.0, M_PI/2.0, M_PI/2.0), 100, 50).transpose());
        // PRINT_VAR(sph2equirectangular_d(Eigen::Vector3d(1.0, M_PI/2.0, M_PI), 100, 50).transpose());
        // PRINT_VAR(sph2equirectangular_d(Eigen::Vector3d(1.0, M_PI/2.0, -M_PI/2.0), 100, 50).transpose());

        // PRINT_VAR(sph2equirectangular_d(Eigen::Vector3d(1.0, -M_PI/2.0, 0.), 100, 50).transpose());
        // PRINT_VAR(sph2equirectangular_d(Eigen::Vector3d(1.0, M_PI/2.0, -M_PI/2.0), 100, 50).transpose());
        // PRINT_VAR(sph2equirectangular_d(Eigen::Vector3d(1.0, -M_PI/2.0, -M_PI), 100, 50).transpose());
        // PRINT_VAR(sph2equirectangular_d(Eigen::Vector3d(1.0, M_PI/2.0, -2.5*M_PI), 100, 50).transpose());

        std::cout << "Testing equirectangular coordinates.\n";

        int i = 0;
        int j = 0;
        int width = 10;
        int height = 5;

        if(1) { std::cout << "\nTesting equirectangular2sph.\n";
            for(i = -2*height ; i < 2*height ; i++) {
                for(j = -2*width ; j < 2*width ; j++) {
                    Eigen::Vector3d p_sph = equirectangular2sph(i, j, width, height);
                    // std::cout << "i: " << i << ", j: " << j << ", p_sph: " << p_sph.transpose() << "\n";
                    EXPECT_DOUBLE_EQ(p_sph[0], 1.0);
                    EXPECT_LT(p_sph[1], M_PI+TOL_EQ_TEST);
                    EXPECT_GE(p_sph[1], 0.0);
                    EXPECT_LT(p_sph[2], 2.0*M_PI);
                    EXPECT_GE(p_sph[2], 0.0);
                }
            }
        }

        if(1) { std::cout << "\nTesting roundtrip.\n";
            for(i = 0 ; i < height ; i++) {
                for(j = 0 ; j < width ; j++) {
                    Eigen::Vector3d p_sph = equirectangular2sph(i, j, width, height);
                    Eigen::Vector2d p_equirectangular = sph2equirectangular_d(p_sph, width, height);
                    // std::cout << "i: " << i << ", j: " << j << ", p_sph: " << p_sph.transpose() << ", p_equirectangular: " << p_equirectangular.transpose() << ((p_equirectangular[0] > height) ? "I OUT OF BOUNDS " : "") << ((p_equirectangular[1] > width) ? "J OUT OF BOUNDS " : "") << "\n";
                    EXPECT_DOUBLE_EQ(p_equirectangular[0], i);
                    EXPECT_DOUBLE_EQ(p_equirectangular[1], j);
                    EXPECT_LT(p_equirectangular[0], height);
                    EXPECT_GE(p_equirectangular[0], 0.0);
                    EXPECT_LT(p_equirectangular[1], width);
                    EXPECT_GE(p_equirectangular[1], 0.0);
                }
            }
        }

        if(1) { std::cout << "\nTesting sph2equirectangular_d out of bounds.\n";
            width = 3600;
            height = 1801;
            double theta = 0.0;
            double phi   = M_PI/4.0;
            
            std::cout << "\nTesting sph2equirectangular_d out of bounds in THETA.\n";
            for(theta = -4.0*M_PI ; theta < 4.0*M_PI ; theta += M_PI/10.0) {
                auto p_equi = sph2equirectangular_d(Eigen::Vector3d(1.0, theta, phi), width, height).transpose();
                // std::cout << "theta: " << theta << ", phi: " << phi << ", p_equirectangular: " << p_equi << "\n";
                EXPECT_EQ(p_equi[0] < 0, false);
                EXPECT_EQ(p_equi[0] >= height, false);
            }

            std::cout << "\nTesting sph2equirectangular_d out of bounds in PHI.\n";
            theta = M_PI/2.0;
            for(phi = -4.0*M_PI ; phi < 4.0*M_PI ; phi += M_PI/10.0) {
                auto p_equi = sph2equirectangular_d(Eigen::Vector3d(1.0, theta, phi), width, height).transpose();
                // std::cout << "theta: " << theta << ", phi: " << phi << ",\tp_equirectangular: " << p_equi << "\n";
                EXPECT_EQ(p_equi[1] < 0, false);
                EXPECT_EQ(p_equi[1] >= width, false);
            }
        }

        if(1) { std::cout << "\nTesting fmod.\n";
            for(double x = -4.0*M_PI ; x < 4.0*M_PI ; x += M_PI/10.0) {
                // std::cout << x << "\t" << std::fmod(x, 2.0*M_PI) << "\t" << frame_conversions::fmod_pos(x, 2.0*M_PI) << "\n";
                EXPECT_EQ(frame_conversions::fmod_pos(x, 2.0*M_PI) >= 0.0, true);
                EXPECT_EQ(frame_conversions::fmod_pos(x, 2.0*M_PI) < 2.0*M_PI, true);
            }
        }
    }

    if(0){// Camera
        std::cout << "Camera\n";
        {// Constructor, accessors and setters
            std::cout << "Constructor, accessors and setters\n";
            
            // Create a camera object
            Camera camera;

            // Check constructor values using the getters
            Eigen::Matrix3d default_R;// Rotation matrix with default fwd and up vectors.
            default_R <<  0, 0, -1,
                         -1, 0,  0,
                          0, 1,  0;
            EXPECT_EQ(camera.GetForward(), Eigen::Vector3d(1, 0, 0));
            EXPECT_EQ(camera.GetUp(), Eigen::Vector3d(0, 0, 1));
            EXPECT_DOUBLE_EQ(camera.GetFOV(), 90.0*M_PI/180.0);
            EXPECT_MAT_EQ(camera.GetR(), default_R);
            EXPECT_MAT_EQ(camera.GetR_T(), default_R.transpose());

            // Check target computation
            Eigen::Vector3d target(1, 1, 1);
            camera.SetTarget(target);

            Eigen::Matrix3d target_R;
            target_R <<  0.7071067811865476, -0.4082482904638631, -0.5773502691896258,
                        -0.7071067811865476, -0.4082482904638631, -0.5773502691896258,
                        -0.                ,  0.8164965809277261, -0.5773502691896258;

            EXPECT_VEC_EQ(camera.GetForward(), Eigen::Vector3d(0.57735027, 0.57735027, 0.57735027));
            EXPECT_MAT_EQ(camera.GetR(), target_R);

            // Set some values using the setters
            Eigen::Vector3d fwd(1, 0, 0);
            Eigen::Vector3d up(0, 1, 0);
            Eigen::Matrix3d R;
            R << 0, 0, 1,
                 1, 0, 0,
                 0, 1, 0;
            double fov = M_PI / 3.0;

            camera.SetForward(fwd)
                  .SetUp(up)
                  .SetR(R)
                  .SetFOV(fov);

            // Check the values using the getters
            EXPECT_EQ(camera.GetForward(), fwd);
            EXPECT_EQ(camera.GetUp(), up);
            EXPECT_EQ(camera.GetR(), R);
            EXPECT_DOUBLE_EQ(camera.GetFOV(), fov);

            // Test point projections
            camera.SetFOV(20*DEG2RAD);
            camera.SetUp(Eigen::Vector3d(0, 0, 1));
            camera.SetTarget(Eigen::Vector3d(1.,.5,.3));

            EXPECT_VEC_EQ(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d( 0.0,  0.0)), Eigen::Vector3d(0.0, 0.0, -1.0));
            EXPECT_VEC_EQ(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d( 1.0,  0.0)), Eigen::Vector3d(0.1736481776669304,                    0, -0.9848077530122081));
            EXPECT_VEC_EQ(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d( 1.0,  1.0)), Eigen::Vector3d(0.1710878697460355,   0.1710878697460355, -0.970287525247814));
            EXPECT_VEC_EQ(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d( 1.0, -1.0)), Eigen::Vector3d(0.1710878697460355,  -0.1710878697460355, -0.970287525247814));
            EXPECT_VEC_EQ(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d(-1.0, -1.0)), Eigen::Vector3d(-0.1710878697460355, -0.1710878697460355, -0.970287525247814));

            EXPECT_VEC_EQ(camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(0.0, 0.0)), Eigen::Vector3d(0.86386842558136, 0.43193421279068, 0.259160527674408));
            EXPECT_VEC_EQ(camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(1.0, 0.0)), Eigen::Vector3d(0.9284021489814165, 0.2700565097745997, 0.2552232969284919));
            EXPECT_VEC_EQ(camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(1.0, 1.0)), Eigen::Vector3d(0.8750553718495241, 0.2462456324858795, 0.416702753385336));
            EXPECT_VEC_EQ(camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(1.0, -1.0)), Eigen::Vector3d(0.9543717844957083, 0.2859038388089716, 0.08621770069290194));
            EXPECT_VEC_EQ(camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(-1.0, -1.0)), Eigen::Vector3d(0.8013461417446023, 0.5919551243111837, 0.08621770069290194));

            EXPECT_VEC_EQ(camera.ProjectPointToSensorCameraFrame(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d(0.0, 0.0))), Eigen::Vector3d(0.0, 0.0, -1.0/tan(camera.GetFOV()/2.0)));
            EXPECT_VEC_EQ(camera.ProjectPointToSensorCameraFrame(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d(1.0, 1.0))), Eigen::Vector3d(1.0, 1.0, -1.0/tan(camera.GetFOV()/2.0)));
            EXPECT_VEC_EQ(camera.ProjectPointToSensorCameraFrame(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d(-1.0, 0.5))), Eigen::Vector3d(-1.0, 0.5, -1.0/tan(camera.GetFOV()/2.0)));

            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 0.0,  0.0, -1.0/tan(camera.GetFOV()/2.0))), true);
            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 1.0,  0.0, -1.0/tan(camera.GetFOV()/2.0))), true);
            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 1.0,  1.0, -1.0/tan(camera.GetFOV()/2.0))), true);
            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 1.0, -1.0, -1.0/tan(camera.GetFOV()/2.0))), true);
            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d(-1.0, -1.0, -1.0/tan(camera.GetFOV()/2.0))), true);
            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d(-1.5, -0.5, -1.0/tan(camera.GetFOV()/2.0))), false);
            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 1.5, -0.5, -1.0/tan(camera.GetFOV()/2.0))), false);
            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 0.5, -1.5, -1.0/tan(camera.GetFOV()/2.0))), false);
            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 0.5,  1.5, -1.0/tan(camera.GetFOV()/2.0))), false);
            EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 0.5, -0.5, +1.0/tan(camera.GetFOV()/2.0))), false);

            // Check static methods
            unsigned int width = 640, height = 480;
            EXPECT_VEC_EQ(Camera::PixelToNormalizedCoordinates(       0,       0, width, height), Eigen::Vector2d(-1.0, 1.0));
            EXPECT_VEC_EQ(Camera::PixelToNormalizedCoordinates(height-1,       0, width, height), Eigen::Vector2d(-1.0, -1.0));
            EXPECT_VEC_EQ(Camera::PixelToNormalizedCoordinates(height-1, width-1, width, height), Eigen::Vector2d(1.0, -1.0));
            EXPECT_VEC_EQ(Camera::PixelToNormalizedCoordinates(       0, width-1, width, height), Eigen::Vector2d(1.0, 1.0));
        }
    }

    if(0){// Image
        std::cout << "Image\n";
        {
            std::cout << "Constructors\n";
            Image image1(640, 480);
            EXPECT_EQ(image1.GetWidth(), 640);
            EXPECT_EQ(image1.GetHeight(), 480);
            EXPECT_EQ(image1.GetDepth(), 1);

            Image image2(1080, 720, 3);
            EXPECT_EQ(image2.GetWidth(), 1080);
            EXPECT_EQ(image2.GetHeight(), 720);
            EXPECT_EQ(image2.GetDepth(), 3);
        }

        {
            std::cout << "Pixel accessors\n";
            Image image(20, 10, 3);
            image.SetPixelValue(5, 7, 0, 69);
            image.SetPixelValue(5, 7, 1, 100);
            image.SetPixelValue(5, 7, 2, 42);
            EXPECT_EQ(image.GetPixelValue(5, 7, 0), 69);
            EXPECT_EQ(image.GetPixelValue(5, 7, 1), 100);
            EXPECT_EQ(image.GetPixelValue(5, 7, 2), 42);

            // Test reference accessor
            image.PixelValue(5, 7, 0) = 13;
            image.PixelValue(5, 7, 1) = 37;
            image.PixelValue(5, 7, 2) = 74;
            EXPECT_EQ(image.PixelValue(5, 7, 0), 13);
            EXPECT_EQ(image.PixelValue(5, 7, 1), 37);
            EXPECT_EQ(image.PixelValue(5, 7, 2), 74);

            // Test Vector accessors
            EXPECT_EQ(image.GetPixel(5, 7), Eigen::Vector3i(13, 37, 74));
            EXPECT_EQ(image.GetPixel(Eigen::Vector2i(5, 7)), Eigen::Vector3i(13, 37, 74));

            image.SetPixel(5, 7, Eigen::Vector3i(1, 2, 3));
            EXPECT_EQ(image.GetPixel(5, 7), Eigen::Vector3i(1, 2, 3));
            
            image.SetPixel(Eigen::Vector2i(6, 8), Eigen::Vector3i(3, 2, 1));
            EXPECT_EQ(image.GetPixel(Eigen::Vector2i(6, 8)), Eigen::Vector3i(3, 2, 1));
        }

        {
            std::cout << "Test Fill(rgb)\n";
            Image image(3, 2, 3);
            image.Fill(Eigen::Vector3i(1, 2, 3));
            for (int i = 0; i < image.GetHeight(); ++i) {
                for (int j = 0; j < image.GetWidth(); ++j) {
                    EXPECT_EQ(image.GetPixel(i, j), Eigen::Vector3i(1, 2, 3));
                }
            }
        }

        {
            std::cout << "Test Fill(uchar)\n";
            Image image(3, 2, 3);
            image.Fill(69);
            for (int i = 0; i < image.GetHeight(); ++i) {
                for (int j = 0; j < image.GetWidth(); ++j) {
                    EXPECT_EQ(image.GetPixel(i, j), Eigen::Vector3i(69, 69, 69));
                }
            }
        }

        {
            std::cout << "Image loading\n";
            Image image("../test_img_4x3.png");
            EXPECT_EQ(image.GetWidth(), 4);
            EXPECT_EQ(image.GetHeight(), 3);
            EXPECT_EQ(image.GetDepth(), 3);

            for(int i = 0; i < image.GetHeight(); ++i) {
                for(int j = 0; j < image.GetWidth(); ++j) {
                    auto pixel = image.GetPixel(i, j);
                    std::cout << i << " " << j << " (" << pixel(0)/255.0 << ", " << pixel(1)/255.0 << ", " << pixel(2)/255.0 << ")\n";
                }
            }

            for(int i = 0 ; i < image.GetWidth()*image.GetHeight()*image.GetDepth() ; i++)
                std::cout << i << "\t" << (int)(image[i]) << "\t" << (image[i]/255.0) << "\n";
        }

        {
            std::cout << "Image saving\n";
            Image image(512, 256, 3);
            for(int i = 0; i < image.GetHeight(); ++i) {
                for(int j = 0; j < image.GetWidth(); ++j) {
                    image.SetPixel(i, j, Eigen::Vector3i(127, i%256, j%256));
                }
            }
            image.Save("test_img_512x256.png");
            image.Save("test_img_512x256.jpg");
            image.Save("test_img_512x256.bmp");
            image.Save("test_img_512x256.tga");
        }

        {
            std::cout << "Bilinear interpolation\n";
            Image image1(3, 3, 3);      // Source image
            Image image2(512, 512, 3);  // Interpolated image (nearest neighbor)
            Image image3(512, 512, 3);  // Interpolated image (bilinear)
            image1.SetPixel(0, 0, Eigen::Vector3i(255, 127, 0));
            image1.SetPixel(0, 1, Eigen::Vector3i(127, 192, 64));
            image1.SetPixel(0, 2, Eigen::Vector3i(100, 100, 50));
            image1.SetPixel(1, 0, Eigen::Vector3i(30, 0, 210));
            image1.SetPixel(1, 1, Eigen::Vector3i(192, 64, 0));
            image1.SetPixel(1, 2, Eigen::Vector3i(64, 150, 192));
            image1.SetPixel(2, 0, Eigen::Vector3i(100, 50, 150));
            image1.SetPixel(2, 1, Eigen::Vector3i(50, 120, 190));
            image1.SetPixel(2, 2, Eigen::Vector3i(240, 50, 15));

            for(int i = 0; i < image2.GetHeight(); ++i) {
                for(int j = 0; j < image2.GetWidth(); ++j) {
                    Eigen::Vector2d pos_float((double)i/image2.GetHeight()*(image1.GetHeight()-1), (double)j/image2.GetWidth()*(image1.GetWidth()-1));
                    image2.SetPixel(i, j, image1.GetPixelInterp(pos_float, Image::InterpMethod::NEAREST));
                    image3.SetPixel(i, j, image1.GetPixelInterp(pos_float, Image::InterpMethod::BILINEAR));
                }
            }

            image1.Save("interp_src_3x3.png");
            image2.Save("interp_nearest_512x512.png");
            image3.Save("interp_bilinear_512x512.png");
        }

        {
            std::cout << "Grayscale conversion\n";
            Image image("../venise.jpg");
            Image image_gray = image.Grayscale();
            Image image_luma = image.LumaREC709();
            image_gray.Save("venise_gray.jpg");
            image_luma.Save("venise_luma.jpg");

            std::cout << "Colormap application\n";
            Image image_parula = image_luma.Colorized(Image::Colormap::PARULA);
            image_parula.Save("venise_parula.jpg");
        }

        {
            std::cout << "Image normalization\n";
            Image image("../venise.jpg");
            image.HistogramNormalize(5);
            image.Save("venise_normalized.jpg");
        }
    }

    if(1){// Test Slicer360ToPerspective
        if(0) { std::cout << "Coverage analysis\n";
            Slicer360ToPerspective slicer;

            double fov = 90.0*DEG2RAD;
            int width = 500;
            slicer.cameras.push_back(Camera().SetFOV(fov).SetTarget(Eigen::Vector3d(1., 0., 0.)));
            slicer.cameras.push_back(Camera().SetFOV(fov).SetTarget(Eigen::Vector3d(-1., -1., -1.)));
            slicer.cameras.push_back(Camera().SetFOV(fov/2).SetTarget(Eigen::Vector3d(1., 0.2, -0.3)));
            Image coverage_raw = slicer.ComputeCoverage(width, true);
            Image coverage_col = slicer.ComputeCoverage(width, false);

            coverage_raw.Save("coverage_raw.png");
            coverage_col.Save("coverage_col.png");
        }

        if(1){ std::cout << "Perspective Projection\n";
            double Mpx   = 1e6;         // Image size in Mpx
            double ratio = 16./9.;      // Image ratio
            int width    = sqrt(Mpx)*sqrt(ratio);
            int height   = sqrt(Mpx)/sqrt(ratio);

            Slicer360ToPerspective slicer = Slicer360ToPerspective(Image("../venise.jpg")).SetOutputImageSize(width, height).SetInterpolationMethod(Image::InterpMethod::BILINEAR);
            Camera camera = Camera().SetFOV(90.0*DEG2RAD).SetTarget(Eigen::Vector3d(1., 0., 0.));
            slicer.cameras.push_back(camera);

            slicer.ProjectToCamera(camera, true).Save("venise_proj_01.png");

            // measure performance
            double t0, t1;
            t0 = omp_get_wtime();
            slicer.ProjectToCamera(camera, false);
            t1 = omp_get_wtime();
            std::cout << "Projection time single-threaded BILINEAR : " << t1-t0 << " s\n";
            t0 = omp_get_wtime();
            slicer.ProjectToCamera(camera, true);
            t1 = omp_get_wtime();
            std::cout << "Projection time multi-threaded BILINEAR : " << t1-t0 << " s\n";
            
            slicer.SetInterpolationMethod(Image::InterpMethod::NEAREST);

            t0 = omp_get_wtime();
            slicer.ProjectToCamera(camera, false);
            t1 = omp_get_wtime();
            std::cout << "Projection time single-threaded NEAREST : " << t1-t0 << " s\n";
            t0 = omp_get_wtime();
            slicer.ProjectToCamera(camera, true);
            t1 = omp_get_wtime();
            std::cout << "Projection time multi-threaded NEAREST : " << t1-t0 << " s\n";
        }
    }

    PRINT_TESTS_SUMMARY();

    return 0;
}