151 lines
5.4 KiB
C++
151 lines
5.4 KiB
C++
////////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// Paella
|
|
// Copyright (C) 2015 - Thomas FORGIONE, Emilie JALRAS, Marion LENFANT, Thierry MALON, Amandine PAILLOUX
|
|
// Authors :
|
|
// Thomas FORGIONE
|
|
// Emilie JALRAS
|
|
// Marion LENFANT
|
|
// Thierry MALON
|
|
// Amandine PAILLOUX
|
|
// Simone GASPARINI
|
|
//
|
|
// This file is part of the project Paella
|
|
// This software is provided 'as-is', without any express or implied warranty.
|
|
// In no event will the authors be held liable for any damages arising from the use of this software.
|
|
//
|
|
// Permission is granted to anyone to use this software for any purpose,
|
|
// including commercial applications, and to alter it and redistribute it freely,
|
|
// subject to the following restrictions:
|
|
//
|
|
// 1. The origin of this software must not be misrepresented;
|
|
// you must not claim that you wrote the original software.
|
|
// If you use this software in a product, an acknowledgment
|
|
// in the product documentation would be appreciated but is not required.
|
|
//
|
|
// 2. Altered source versions must be plainly marked as such,
|
|
// and must not be misrepresented as being the original software.
|
|
//
|
|
// 3. This notice may not be removed or altered from any source distribution.
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
#ifndef _UTILITY_HPP_
|
|
#define _UTILITY_HPP_
|
|
|
|
#include "Camera.hpp"
|
|
|
|
#include <opencv2/core/core.hpp>
|
|
|
|
#include <iostream>
|
|
|
|
#define DEBUGGING 1
|
|
|
|
#if DEBUGGING
|
|
#define PRINTVAR( a ) std::cout << #a << " = " << a << std::endl << std::endl;
|
|
#else
|
|
#define PRINTVAR( a )
|
|
#endif
|
|
|
|
/**
|
|
* \ingroup calibration
|
|
* Detect a chessboard in a given image
|
|
*
|
|
* @param[in] rgbimage The rgb image to process
|
|
* @param[out] pointbuf the set of 2D image corner detected on the chessboard
|
|
* @param[in] boardSize the size of the board in terms of corners (width X height)
|
|
* @return true if the chessboard is detected inside the image, false otherwise
|
|
*/
|
|
bool detectChessboard( const cv::Mat &rgbimage, std::vector<cv::Point2f> &pointbuf, const cv::Size &boardSize);
|
|
|
|
/**
|
|
* \ingroup calibration
|
|
* Decompose the homography into its components R and t
|
|
*
|
|
* @param[in] H The homography H = [r1 r2 t]
|
|
* @param[in] matK The 3x3 calibration matrix K
|
|
* @param[out] poseMat the 3x4 pose matrix [R t]
|
|
*/
|
|
void decomposeHomography( const cv::Mat &H, const cv::Mat& matK, cv::Mat& poseMat );
|
|
|
|
/**
|
|
*
|
|
* \ingroup calibration
|
|
* @param[in,out] rgbimage The image on which to draw the reference system
|
|
* @param[in] cam The camera
|
|
* @param[in] projMat The projection matrix of the camera
|
|
* @param[in] thickness The thickness of the line
|
|
* @param[in] scale A scale factor for the unit vectors to draw
|
|
* @param[in] alreadyUndistorted A boolean value that tells if the input image rgbimage is already undistorted or we are working on a distorted image
|
|
*/
|
|
void drawReferenceSystem( cv::Mat &rgbimage, const Camera& cam, const cv::Mat &projMat, const int &thickness, const double &scale, const bool alreadyUndistorted = true );
|
|
|
|
/**
|
|
* \ingroup calibration
|
|
* Wrapper around the original opencv's projectPoints
|
|
*
|
|
* @param[in] objectPoints the 3D points
|
|
* @param[in] poseMat the pose matrix
|
|
* @param[in] cameraMatrix the calibration matrix
|
|
* @param[in] distCoeffs the distortion coefficients
|
|
* @param[out] imagePoints the projected points
|
|
*/
|
|
void myProjectPoints( cv::InputArray objectPoints, const cv::Mat &poseMat, cv::InputArray cameraMatrix, cv::InputArray distCoeffs, cv::OutputArray imagePoints);
|
|
|
|
/**
|
|
* \ingroup calibration
|
|
* Wrapper around the original opencv's solvePnPRansac
|
|
*
|
|
* @param[in] objectPoints the 3D points
|
|
* @param[in] imagePoints the image points
|
|
* @param[in] cameraMatrix the calibration matrix
|
|
* @param[in] distCoeffs the distortion coefficients
|
|
* @param[out] poseMat the pose matrix
|
|
* @param[out] inliers the list of indices of the inliers points
|
|
*/
|
|
void mySolvePnPRansac(cv::InputArray objectPoints, cv::InputArray imagePoints, cv::InputArray cameraMatrix, cv::InputArray distCoeffs, cv::Mat &poseMat, cv::OutputArray inliers=cv::noArray());
|
|
|
|
/**
|
|
* \ingroup calibration
|
|
* Generate the set of 3D points of a chessboard
|
|
*
|
|
* @param[in] boardSize the size of the board in terms of corners (width X height)
|
|
* @param[in] squareSize the size in mm of the each square of the chessboard
|
|
* @param[out] corners the set of 2D points on the chessboard
|
|
*/
|
|
void calcChessboardCorners( const cv::Size &boardSize, const float &squareSize, std::vector<cv::Point2f>& corners);
|
|
|
|
|
|
/**
|
|
* \ingroup calibration
|
|
* Generate the set of 3D points of a chessboard
|
|
*
|
|
* @param[in] boardSize the size of the board in terms of corners (width X height)
|
|
* @param[in] squareSize the size in mm of the each square of the chessboard
|
|
* @param[out] corners the set of 3D points on the chessboard
|
|
*/
|
|
void calcChessboardCorners3D( const cv::Size &boardSize, const float &squareSize, std::vector<cv::Point3f>& corners );
|
|
|
|
/**
|
|
* \ingroup calibration
|
|
* Filter a generic vector against a list of index of the elements to be deleted,
|
|
*
|
|
* @param[in,out] inout the vector to filter
|
|
* @param[in] idx list of indices of the element to remove
|
|
*/
|
|
template<typename T>
|
|
void filterVector( std::vector<T> &inout, const std::vector<int> &idx )
|
|
{
|
|
std::vector<T> temp;
|
|
temp.reserve( idx.size() );
|
|
|
|
for( size_t i = 0; i < idx.size(); ++i )
|
|
{
|
|
assert( idx[i] < inout.size() );
|
|
temp.push_back( inout[ idx[i] ] );
|
|
}
|
|
|
|
inout.clear(); //necessary??
|
|
inout = temp;
|
|
}
|
|
|
|
#endif /*_UTILITY_HPP_*/
|