private void InitCameraInfoData()
{
if (CameraInfoData != null)
{
return;
}
var vFOV = SensorCamera.fieldOfView * Mathf.Deg2Rad;
var hFOV = 2 * Mathf.Atan(Mathf.Tan(SensorCamera.fieldOfView * Mathf.Deg2Rad / 2) * SensorCamera.aspect);
double fx = (double)(SensorCamera.pixelWidth / (2.0f * Mathf.Tan(0.5f * hFOV)));
double fy = (double)(SensorCamera.pixelHeight / (2.0f * Mathf.Tan(0.5f * vFOV)));
double cx = SensorCamera.pixelWidth / 2.0f;
double cy = SensorCamera.pixelHeight / 2.0f;
CameraInfoData = new CameraInfoData();
CameraInfoData.Width = SensorCamera.pixelWidth;
CameraInfoData.Height = SensorCamera.pixelHeight;
CameraInfoData.FocalLengthX = fx;
CameraInfoData.FocalLengthY = fx;
CameraInfoData.PrincipalPointX = cx;
CameraInfoData.PrincipalPointY = cy;
if (Distorted)
{
CameraInfoData.DistortionParameters = DistortionParameters.ToArray();
}
else
{
CameraInfoData.DistortionParameters = new float[4] {
0.0f, 0.0f, 0.0f, 0.0f
};
}
}
public static Ros.CameraInfo ConvertFrom(CameraInfoData data)
{
return(new Ros.CameraInfo()
{
header = new Ros.Header()
{
seq = data.Sequence,
stamp = ConvertTime(data.Time),
frame_id = data.Frame,
},
height = (uint)data.Height,
width = (uint)data.Width,
distortion_model = "plumb_bob",
D = new double[5]
{
(double)data.DistortionParameters[0],
(double)data.DistortionParameters[1],
0.0,
0.0,
(double)data.DistortionParameters[2],
},
K = new double[9]
{
data.FocalLengthX, 0.0, data.PrincipalPointX,
0.0, data.FocalLengthY, data.PrincipalPointY,
0.0, 0.0, 1.0,
},
R = new double[9]
{
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0,
},
P = new double[12]
{
data.FocalLengthX, 0.0, data.PrincipalPointX, 0.0,
0.0, data.FocalLengthY, data.PrincipalPointY, 0.0,
0.0, 0.0, 1.0, 0.0,
},
binning_x = 0,
binning_y = 0,
roi = new Ros.RegionOfInterest()
{
x_offset = 0,
y_offset = 0,
width = 0,
height = 0,
do_rectify = false,
}
});
}
public static Snowball.CameraInfo ConvertFrom(CameraInfoData data)
{
return(new Snowball.CameraInfo()
{
header = new Snowball.Header()
{
stamp = new Snowball.Time(data.Time),
frame_id = data.frame_id,
},
height = data.Height,
width = data.Width,
d = data.d,
k = data.k,
r = data.r,
p = data.p
});
}
