aruco::CameraParameters aruco_ros::rosCameraInfo2ArucoCamParams(const sensor_msgs::CameraInfo& cam_info,
bool useRectifiedParameters)
{
cv::Mat cameraMatrix(3, 3, CV_32FC1);
cv::Mat distorsionCoeff(4, 1, CV_32FC1);
cv::Size size(cam_info.height, cam_info.width);
if ( useRectifiedParameters )
{
cameraMatrix.setTo(0);
cameraMatrix.at<float>(0,0) = cam_info.P[0]; cameraMatrix.at<float>(0,1) = cam_info.P[1]; cameraMatrix.at<float>(0,2) = cam_info.P[2];
cameraMatrix.at<float>(1,0) = cam_info.P[4]; cameraMatrix.at<float>(1,1) = cam_info.P[5]; cameraMatrix.at<float>(1,2) = cam_info.P[6];
cameraMatrix.at<float>(2,0) = cam_info.P[8]; cameraMatrix.at<float>(2,1) = cam_info.P[9]; cameraMatrix.at<float>(2,2) = cam_info.P[10];
for(int i=0; i<4; ++i)
distorsionCoeff.at<float>(i, 0) = 0;
}
else
{
for(int i=0; i<9; ++i)
cameraMatrix.at<float>(i%3, i-(i%3)*3) = cam_info.K[i];
for(int i=0; i<4; ++i)
distorsionCoeff.at<float>(i, 0) = cam_info.D[i];
}
return aruco::CameraParameters(cameraMatrix, distorsionCoeff, size);
}
aruco::CameraParameters aruco_ros::rosCameraInfo2ArucoCamParams(const sensor_msgs::CameraInfo& cam_info,
bool useRectifiedParameters)
{
cv::Mat cameraMatrix(3, 3, CV_32FC1);
cv::Mat distorsionCoeff(4, 1, CV_32FC1);
cv::Size size(cam_info.height, cam_info.width);
if ( useRectifiedParameters )
{
cameraMatrix.setTo(0);
cameraMatrix.at<float>(0,0) = cam_info.K[0];
cameraMatrix.at<float>(0,1) = cam_info.K[1];
cameraMatrix.at<float>(0,2) = cam_info.K[2];
cameraMatrix.at<float>(1,0) = cam_info.K[3];
cameraMatrix.at<float>(1,1) = cam_info.K[4];
cameraMatrix.at<float>(1,2) = cam_info.K[5];
cameraMatrix.at<float>(2,0) = cam_info.K[6];
cameraMatrix.at<float>(2,1) = cam_info.K[7];
cameraMatrix.at<float>(2,2) = cam_info.K[8];
for(int i=0; i<4; ++i)
distorsionCoeff.at<float>(i, 0) = 0;
}
else
{
for(int i=0; i<9; ++i)
cameraMatrix.at<float>(i%3, i-(i%3)*3) = cam_info.K[i];
if(cam_info.D.size() == 4)
{
for(int i=0; i<4; ++i)
distorsionCoeff.at<float>(i, 0) = cam_info.D[i];
}
else
{
ROS_WARN("length of camera_info D vector is not 4, assuming zero distortion...");
for(int i=0; i<4; ++i)
distorsionCoeff.at<float>(i, 0) = 0;
}
}
return aruco::CameraParameters(cameraMatrix, distorsionCoeff, size);
}
