test.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/system/filesystem.h>  // for ASSERT_FILE_EXISTS_
#include <mrpt/vision/chessboard_stereo_camera_calib.h>
#include <iostream>

using namespace mrpt;
using namespace mrpt::gui;
using namespace mrpt::vision;
using namespace mrpt::opengl;
using namespace mrpt::system;
using namespace mrpt::vision;
using namespace std;

// ------------------------------------------------------
//              TestStereoCalibrate
// ------------------------------------------------------
int TestStereoCalibrate(int argc, char** argv)
{
    CTimeLogger timlog;

    // Parse optional arguments:
    if (argc == 1 || ((argc - 1) & 1) != 0)
    {
        cout << "Usage:\n"
             << argv[0] << "left_image1 right_image1 [L2 R2] [L3 R3] [...]\n";
        return -1;
    }

    // The stereo calibration structures:
    TCalibrationStereoImageList calib_imgs;
    TStereoCalibResults calib_result;
    TStereoCalibParams calib_params;

    // ============ Set parameters ============
    calib_params.check_size_x = 7;
    calib_params.check_size_y = 9;
    calib_params.check_squares_length_X_meters = 22.83e-3;
    calib_params.check_squares_length_Y_meters = 24.31e-3;
    // calib_params.maxIters = 300;
    // calib_params.verbose = true;
    calib_params.optimize_k1 = true;
    calib_params.optimize_k2 = true;

    // Load images:
    const size_t nPairs = (argc >> 1);
    for (size_t i = 0; i < nPairs; i++)
    {
        const string sImgL = argv[1 + 2 * i + 0];
        const string sImgR = argv[1 + 2 * i + 1];
        ASSERT_FILE_EXISTS_(sImgL);
        ASSERT_FILE_EXISTS_(sImgR);

        calib_imgs.resize(calib_imgs.size() + 1);
        TImageStereoCalibData& stereo_dat = *calib_imgs.rbegin();

#if 1
        // Load all images in memory:
        if (!stereo_dat.left.img_original.loadFromFile(sImgL))
            THROW_EXCEPTION_FMT("Error loading: %s", sImgL.c_str());
        if (!stereo_dat.right.img_original.loadFromFile(sImgR))
            THROW_EXCEPTION_FMT("Error loading: %s", sImgR.c_str());
#else
        // Don't load images in memory until really needed.
        stereo_dat.left.img_original.setExternalStorage(sImgL);
        stereo_dat.right.img_original.setExternalStorage(sImgR);
#endif
    }

    // Run calibration:
    bool res = mrpt::vision::checkerBoardStereoCalibration(
        calib_imgs, calib_params, calib_result);

    if (!res)
    {
        std::cout << "Calibration returned an error status.\n";
        return -1;
    }
    else
    {
        // Calibration was OK:

        // Show detected corners:
        if (true)
        {
            mrpt::gui::CDisplayWindow3D win("Calibration results", 1000, 480);

            mrpt::opengl::COpenGLViewport::Ptr view1, view2;
            {
                mrpt::opengl::COpenGLScene::Ptr& scene =
                    win.get3DSceneAndLock();
                view1 = scene->getViewport("main");
                view2 = scene->createViewport("right");

                // Split viewing area into two halves:
                view1->setViewportPosition(0, 0, 0.5, 1.0);
                view2->setViewportPosition(0.5, 0, 0.5, 1.0);

                win.unlockAccess3DScene();
            }

            for (size_t i = 0; i < nPairs; i++)
            {
                win.get3DSceneAndLock();

                view1->setImageView(
                    calib_imgs[i].left.img_rectified);  // img_checkboard );
                view2->setImageView(
                    calib_imgs[i].right.img_rectified);  // img_checkboard );

                win.setWindowTitle(mrpt::format(
                    "Detected corners: %u / %u",
                    static_cast<unsigned int>(i + 1),
                    static_cast<unsigned int>(nPairs)));

                win.unlockAccess3DScene();
                win.repaint();

                win.waitForKey();
            }
        }  // end show detected corners

        return 0;
    }
}

// ------------------------------------------------------
//                      MAIN
// ------------------------------------------------------
int main(int argc, char** argv)
{
    try
    {
        return TestStereoCalibrate(argc, argv);
    }
    catch (const std::exception& e)
    {
        std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
        return -1;
    }
    catch (...)
    {
        printf("Untyped exception!!");
        return -1;
    }
}