void CameraCalibration::ReadIntrinsicMatrix( const sensor_msgs::CameraInfo& info )
{
intrinsicMatrix = cv::Matx33d::eye();
intrinsicMatrix(0,0) = GetFx( info );
intrinsicMatrix(1,1) = GetFy( info );
intrinsicMatrix(0,2) = GetCx( info );
intrinsicMatrix(1,2) = GetCy( info );
}
void CVToGLConversion::intrinsicMatrix(const cv::Matx33f& K, cv::Size windowsize,
const float& near_, const float& far_, float(&glprojection)[16])
{
glm::mat4 m = glm::make_mat4(glprojection);
intrinsicMatrix(K, windowsize, near_, far_, m);
memcpy(glprojection, glm::value_ptr(m), 16 * sizeof(float));
}
void CVToGLConversion::intrinsicMatrix(const cv::Matx33d& K, cv::Size windowsize,
const double& near_, const double& far_, double(&glprojection)[16])
{
glm::dmat4 m = glm::make_mat4(glprojection);
intrinsicMatrix(K, windowsize, near_, far_, m);
memcpy(glprojection, glm::value_ptr(m), 16 * sizeof(double));
}
void PhotoCamera::buildProjection()
{
float fov = (2 * std::atan((getHeight()/2.0)/-intrinsicMatrix(1,1)));
float ratio = getWidth()/(getHeight() * pixelSize(0)/pixelSize(1));
float f = 1/ tan(fov/2);
float near = 0.1;
float far = 100000;
// cout << "fov: " << fov << endl;
// cout << "ratio: " << ratio << endl;
Eigen::Matrix4f perspective = Eigen::Matrix4f::Zero();
perspective(0,0) = f/ratio;
perspective(1,1) = f;
perspective(2,2) = (far + near)/(near - far);
perspective(3,2) = -1;
perspective(2,3) = (2 * far * near)/(near - far);
projectionMatrix = perspective;
}
