107 lines
4.5 KiB
C++
107 lines
4.5 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/ChessboardCameraTracker.hpp"
|
|
#include "Extern/utility.hpp"
|
|
|
|
#include <opencv2/highgui/highgui.hpp>
|
|
#include <opencv2/calib3d/calib3d.hpp>
|
|
#include <opencv2/imgproc/imgproc.hpp>
|
|
|
|
#include <iostream>
|
|
|
|
/**
|
|
* It detects a chessboard inside an image and if found it returns the pose of the camera wrt the chessboard
|
|
*
|
|
* @param[in,out] img the original image
|
|
* @param[out] pose the pose of the camera
|
|
* @param[in] cam the camera
|
|
* @param[in] boardSize the size of the chessboard to detect
|
|
* @return true if the chessboard has been found
|
|
*/
|
|
bool ChessboardCameraTracker::process( cv::Mat &img, cv::Mat &pose, const Camera & cam, const cv::Size &boardSize )
|
|
{
|
|
cv::Mat undistorted_img, H;
|
|
|
|
// true if the chessboard is found
|
|
bool found = false;
|
|
|
|
// contains the points detected on the chessboard
|
|
std::vector<cv::Point2f> corners;
|
|
|
|
//******************************************************************/
|
|
// undistort the input image. view at the end must contain the undistorted version
|
|
// of the image.
|
|
//******************************************************************/
|
|
cv::undistort(img, undistorted_img, cam.matK, cam.distCoeff);
|
|
// cv::imshow("Image View", undistorted_img);
|
|
//******************************************************************/
|
|
// detect the chessboard
|
|
//******************************************************************/
|
|
found = detectChessboard(undistorted_img, corners, boardSize);
|
|
|
|
// cout << ( (!found ) ? ( "No " ) : ("") ) << "chessboard detected!" << endl;
|
|
// cv::drawChessboardCorners(undistorted_img, boardSize, corners, found);
|
|
|
|
//******************************************************************/
|
|
// if a chessboard is found estimate the homography and rectify the image
|
|
//******************************************************************/
|
|
if( found )
|
|
{
|
|
|
|
// contains the points on the chessboard in the chessboard reference system
|
|
std::vector<cv::Point2f> refCorners;
|
|
|
|
//******************************************************************/
|
|
// generate the coordinates of the points on the chessboard in the chessboard reference system
|
|
//******************************************************************/
|
|
calcChessboardCorners(boardSize, 25, refCorners);
|
|
|
|
//******************************************************************/
|
|
// estimate the homography
|
|
// --> see findHomography
|
|
// http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?highlight=homography#findhomography
|
|
//******************************************************************/
|
|
H = cv::findHomography(refCorners, corners, CV_RANSAC);
|
|
|
|
|
|
// cout << "H = " << H << endl << endl;
|
|
// cout << "corners =" << corners << endl << endl;
|
|
// cout << "ptsOb =" << objectPoints << endl << endl;
|
|
|
|
//******************************************************************/
|
|
// decompose the homography
|
|
//******************************************************************/
|
|
decomposeHomography(H, cam.matK, pose);
|
|
}
|
|
|
|
return found;
|
|
}
|