319 lines
13 KiB
C++
319 lines
13 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.
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
#include "Extern/utility.hpp"
|
|
|
|
#include <opencv2/calib3d/calib3d.hpp>
|
|
#include <opencv2/imgproc/imgproc.hpp>
|
|
|
|
#include <iostream>
|
|
|
|
/******************************************************************/
|
|
/* FUNCTIONS TO DEVELOP */
|
|
/******************************************************************/
|
|
/**
|
|
* 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)
|
|
{
|
|
// it contains the value to return
|
|
bool found = false;
|
|
|
|
|
|
/******************************************************************/
|
|
// detect the chessboard --> see findChessboardCorners
|
|
/******************************************************************/
|
|
found = findChessboardCorners(rgbimage, boardSize, pointbuf);
|
|
|
|
// if a chessboard is found refine the position of the points in a window 11x11 pixel
|
|
// use the default value for the termination criteria --> TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 )
|
|
|
|
if ( found )
|
|
{
|
|
cv::Mat viewGrey; // it will contain the graylevel version of the image
|
|
|
|
/******************************************************************/
|
|
// convert the image in "rgbimage" to gray level and save it in "viewGrey"
|
|
// --> cvtColor with CV_BGR2GRAY option
|
|
/******************************************************************/
|
|
cv::cvtColor(rgbimage, viewGrey, CV_BGR2GRAY,1);
|
|
|
|
/******************************************************************/
|
|
// refine the corner location in "pointbuf" using "viewGrey"
|
|
// --> see cornerSubPix
|
|
/******************************************************************/
|
|
cv::cornerSubPix(viewGrey, pointbuf, boardSize, cv::Size{5,5}, cv::TermCriteria{CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,0.1});
|
|
|
|
}
|
|
|
|
|
|
return found;
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
*
|
|
* @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 )
|
|
{
|
|
|
|
// contains the points to project to draw the 3 axis
|
|
//******************************************************************/
|
|
// Add the four 3D points (Point3f) that we can use to draw
|
|
// the reference system to vertex3D. Use <scale> as unit
|
|
//******************************************************************/
|
|
float scalef = static_cast<float>(scale);
|
|
|
|
std::vector<cv::Point3f> vertex3D{cv::Point3f{0,0,0},
|
|
cv::Point3f{scalef,0,0},
|
|
cv::Point3f{0,scalef,0},
|
|
cv::Point3f{0,0,-scalef}
|
|
};
|
|
|
|
// contains the projected 3D points on the image
|
|
std::vector<cv::Point2f> imgRefPts;
|
|
|
|
//******************************************************************/
|
|
// Project the 3D points using myProjectPoints. Attention, check the
|
|
// flag alreadyUndistorted to see if we have to apply the distortion:
|
|
// if it is true we pass a 1x5 zero vector, otherwise the distortion
|
|
// parameter of cam
|
|
//******************************************************************/
|
|
cv::Mat undist = alreadyUndistorted ? cv::Mat::zeros(1,5,CV_32F) : cam.distCoeff;
|
|
|
|
myProjectPoints(vertex3D,projMat,cam.matK,undist,imgRefPts);
|
|
|
|
//******************************************************************/
|
|
// draw the line of the x-axis and put "X" at the end
|
|
//******************************************************************/
|
|
line(rgbimage, imgRefPts[0], imgRefPts[1], cv::Scalar{255,0,0}, thickness);
|
|
putText(rgbimage, "X", imgRefPts[1], cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar{255,0,0}, thickness);
|
|
|
|
//******************************************************************/
|
|
// draw the line of the y-axis and put "Y" at the end
|
|
//******************************************************************/
|
|
line(rgbimage, imgRefPts[0], imgRefPts[2], cv::Scalar{0,255,0}, thickness);
|
|
putText(rgbimage, "Y", imgRefPts[2], cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar{0,255,0}, thickness);
|
|
|
|
//******************************************************************/
|
|
// draw the line of the z-axis and put "Z" at the end
|
|
//******************************************************************/
|
|
line(rgbimage, imgRefPts[0], imgRefPts[3], cv::Scalar{0,0,255}, thickness);
|
|
putText(rgbimage, "Z", imgRefPts[3], cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar{0,0,255}, thickness);
|
|
|
|
}
|
|
|
|
/**
|
|
* Wrapper around the original opencv's projectPoints
|
|
*
|
|
* @param objectPoints the 3D points
|
|
* @param poseMat the pose matrix
|
|
* @param cameraMatrix the calibration matrix
|
|
* @param distCoeffs the distortion coeffi
|
|
* @param imagePoints
|
|
*/
|
|
void myProjectPoints( cv::InputArray objectPoints, const cv::Mat &poseMat, cv::InputArray cameraMatrix, cv::InputArray distCoeffs, cv::OutputArray imagePoints)
|
|
{
|
|
cv::Mat rvec;
|
|
Rodrigues( poseMat.colRange(0,3), rvec );
|
|
// projectPoints( Mat( vertex3D.t( ) ).reshape( 3, 1 ), rvec, Tvec, K, dist, imgRefPts );
|
|
projectPoints( objectPoints, rvec, poseMat.col(3), cameraMatrix, distCoeffs, imagePoints );
|
|
}
|
|
|
|
|
|
/**
|
|
* 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 )
|
|
{
|
|
corners.resize(0);
|
|
corners.reserve( boardSize.height* boardSize.width );
|
|
|
|
for( int i = 0; i < boardSize.height; i++ )
|
|
for( int j = 0; j < boardSize.width; j++ )
|
|
{
|
|
/******************************************************************/
|
|
// create a Point2f(x,y) according to the position j,i and a square
|
|
// size of squareSize. Add it to corners (using push_back...)
|
|
/******************************************************************/
|
|
corners.push_back(cv::Point2f{squareSize*j,squareSize*i});
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* 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 )
|
|
{
|
|
|
|
cv::Mat temp;
|
|
|
|
// H = [h1, h2, h3] = lambda * K [r1,r2,t]
|
|
|
|
//******************************************************************/
|
|
//temp contains inv(K)*H
|
|
//******************************************************************/
|
|
temp = matK.inv()*H;
|
|
|
|
cv::Mat r1, r2, r3, t;
|
|
|
|
//******************************************************************/
|
|
// get r1 and r2 from temp
|
|
//******************************************************************/
|
|
r1 = temp.col(0);
|
|
r2 = temp.col(1);
|
|
|
|
//******************************************************************/
|
|
// compute lambda
|
|
//******************************************************************/
|
|
double lambda = 1 / norm(r1);
|
|
|
|
//******************************************************************/
|
|
// normalize r1 and r2
|
|
//******************************************************************/
|
|
r1 *= lambda;
|
|
r2 *= lambda;
|
|
|
|
//******************************************************************/
|
|
// compute r3
|
|
//******************************************************************/
|
|
r3 = r1.cross(r2);
|
|
|
|
//******************************************************************/
|
|
// compute t
|
|
//******************************************************************/
|
|
t = temp.col(2) * lambda;
|
|
|
|
//******************************************************************/
|
|
// create a 3x4 matrix (float) for poseMat
|
|
//******************************************************************/
|
|
poseMat = cv::Mat{3,4, CV_32F};
|
|
|
|
//******************************************************************/
|
|
// fill the columns of poseMat with r1 r2 r3 and t
|
|
//******************************************************************/
|
|
r1.copyTo(poseMat.col(0));
|
|
r2.copyTo(poseMat.col(1));
|
|
r3.copyTo(poseMat.col(2));
|
|
t.copyTo(poseMat.col(3));
|
|
|
|
// std::cout << "----------------------------" << std::endl;
|
|
// std::cout << "lambda = " << lambda << std::endl;
|
|
// std::cout << "----------------------------" << std::endl;
|
|
// std::cout << r1 << std::endl;
|
|
// std::cout << "----------------------------" << std::endl;
|
|
// std::cout << poseMat << std::endl;
|
|
// std::cout << "----------------------------" << std::endl;
|
|
|
|
}
|
|
|
|
|
|
/******************************************************************************/
|
|
//KLT ONLY
|
|
|
|
/**
|
|
* 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 )
|
|
{
|
|
corners.resize(0);
|
|
corners.reserve( boardSize.height* boardSize.width );
|
|
for( int i = 0; i < boardSize.height; i++ )
|
|
for( int j = 0; j < boardSize.width; j++ )
|
|
{
|
|
/******************************************************************/
|
|
// create a Point3f(x,y,0) according to the position j,i and a square
|
|
// size of squareSize. Add it to corners (using push_back...)
|
|
/******************************************************************/
|
|
corners.push_back(cv::Point3f{squareSize*j, squareSize*i, 0});
|
|
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* 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::Mat currR, currT;
|
|
cv::solvePnPRansac( objectPoints, imagePoints, cameraMatrix, distCoeffs, currR, currT, false, 100, 4, 100, inliers );
|
|
|
|
poseMat = cv::Mat(3, 4, CV_32F );
|
|
|
|
cv::Mat Rot;
|
|
Rodrigues( currR, Rot );
|
|
#if CV_MINOR_VERSION < 4
|
|
// apparently older versions does not support direct copy
|
|
cv::Mat temp;
|
|
Rot.convertTo( temp, CV_32F );
|
|
cv::Mat a1 = poseMat.colRange(0,3);
|
|
temp.copyTo( a1 );
|
|
a1 = poseMat.col(3);
|
|
currT.convertTo( temp, CV_32F );
|
|
temp.copyTo( a1 );
|
|
#else
|
|
Rot.copyTo( poseMat.colRange(0,3) );
|
|
currT.copyTo( poseMat.col(3) );
|
|
#endif
|
|
}
|