143 lines
5.5 KiB
C++
143 lines
5.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/Camera.hpp"
|
||
|
||
#include <iostream>
|
||
|
||
/**
|
||
* Initialize the camera loading the internal parameters from the given file
|
||
*
|
||
* @param[in] calibFilename the calibration file
|
||
* @return true if success
|
||
*/
|
||
bool Camera::init( std::string calibFilename )
|
||
{
|
||
/******************************************************************/
|
||
// open the file storage with the given filename
|
||
/******************************************************************/
|
||
cv::FileStorage fs;
|
||
|
||
/******************************************************************/
|
||
// check if the file storage has been opened correclty
|
||
/******************************************************************/
|
||
if( !fs.open(calibFilename, cv::FileStorage::READ))
|
||
{
|
||
std::cerr << "Cannot open calib file" << std::endl;
|
||
return false;
|
||
}
|
||
|
||
|
||
/******************************************************************/
|
||
// load the camera_matrix in matK
|
||
/******************************************************************/
|
||
fs["camera_matrix"] >> matK;
|
||
|
||
/******************************************************************/
|
||
// load the distortion_coefficients in distCoeff
|
||
/******************************************************************/
|
||
fs["distortion_coefficients"] >> distCoeff;
|
||
|
||
/******************************************************************/
|
||
// load image_width and image_height in imageSize.[width|height]
|
||
/******************************************************************/
|
||
fs["image_width"] >> imageSize.width;
|
||
fs["image_height"] >> imageSize.height;
|
||
|
||
// std::cout << matK << std::endl;
|
||
// std::cout << distCoeff << std::endl;
|
||
|
||
return true;
|
||
}
|
||
|
||
|
||
/**
|
||
* Return the OpenGL projection matrix for the left camera
|
||
* @param[out] proj the OGL projection matrix (ready to be passed, ie in col major format)
|
||
* @param znear near clipping plane
|
||
* @param zfar far clipping plane
|
||
* \note using http://strawlab.org/2011/11/05/augmented-reality-with-OpenGL/
|
||
*/
|
||
void Camera::getOGLProjectionMatrix( float *proj, const float znear, const float zfar ) const
|
||
{
|
||
|
||
|
||
// With window_coords==’y down’, we have:
|
||
//
|
||
// [2*K00/width, -2*K01/width, (width - 2*K02 + 2*x0)/width, 0]
|
||
// [ 0, 2*K11/height, (-height + 2*K12 + 2*y0)/height, 0]
|
||
// [ 0, 0, (-zfar - znear)/(zfar - znear), -2*zfar*znear/(zfar - znear)]
|
||
// [ 0, 0, -1, 0]
|
||
|
||
// //corrected with artoolkitpluss src/Tracker::349
|
||
// proj[0] = (float)(2*matK.at<double>(0,0))/imageSize.width;
|
||
// proj[1*4 + 0] = (float)(2*matK.at<double>(0,1))/imageSize.width;
|
||
// proj[2*4 + 0] = -(float)(imageSize.width - 2*matK.at<double>(0,2) )/imageSize.width;
|
||
// proj[3*4 + 0] = 0;
|
||
|
||
// proj[0*4 + 1] = 0;
|
||
// // minus -(float)(2*matK.at<double>(1,1))/imageSize.height;
|
||
// proj[1*4 + 1] = -(float)(2*matK.at<double>(1,1))/imageSize.height;
|
||
// proj[2*4 + 1] = (float)(-imageSize.height + 2*matK.at<double>(1,2) )/imageSize.height;
|
||
// proj[3*4 + 1] = 0;
|
||
|
||
// proj[0*4 + 2] = 0;
|
||
// proj[1*4 + 2] = 0;
|
||
// proj[2*4 + 2] = (zfar + znear)/(zfar - znear);
|
||
// proj[3*4 + 2] = -2*zfar*znear/(zfar - znear);
|
||
|
||
// proj[0*4 + 3] = 0;
|
||
// proj[1*4 + 3] = 0;
|
||
// proj[2*4 + 3] = 1;
|
||
// proj[3*4 + 3] = 0;
|
||
|
||
//corrected with artoolkitpluss src/Tracker::349
|
||
proj[0] = (float)(2*matK.at<double>(0,0))/imageSize.width;
|
||
proj[1*4 + 0] = (float)(2*matK.at<double>(0,1))/imageSize.width;
|
||
proj[2*4 + 0] = -(float)(imageSize.width - 2*matK.at<double>(0,2) )/imageSize.width;
|
||
proj[3*4 + 0] = 0;
|
||
|
||
proj[0*4 + 1] = 0;
|
||
// minus -(float)(2*matK.at<double>(1,1))/imageSize.height;
|
||
proj[1*4 + 1] = -(float)(2*matK.at<double>(1,1))/imageSize.height;
|
||
proj[2*4 + 1] = -(float)(-imageSize.height + 2*matK.at<double>(1,2) )/imageSize.height;
|
||
proj[3*4 + 1] = 0;
|
||
|
||
proj[0*4 + 2] = 0;
|
||
proj[1*4 + 2] = 0;
|
||
proj[2*4 + 2] = (zfar + znear)/(zfar - znear);
|
||
proj[3*4 + 2] = -2*zfar*znear/(zfar - znear);
|
||
|
||
proj[0*4 + 3] = 0;
|
||
proj[1*4 + 3] = 0;
|
||
proj[2*4 + 3] = 1;
|
||
proj[3*4 + 3] = 0;
|
||
}
|