test.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
 
#include <mrpt/system/CTimeLogger.h>
#include <mrpt/gui/CDisplayWindow3D.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 (1)
                {
                        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 (std::exception& e)
        {
                std::cout << "MRPT exception caught: " << e.what() << std::endl;
                return -1;
        }
        catch (...)
        {
                printf("Untyped exception!!");
                return -1;
        }
}