private void LoadCalib(_emPosition emPosition)
{
string strFName = String.Format("{0:s}\\steoreo_calibP{1:d}.json", ASDef._INI_PATH, emPosition + 1);
try
{
m_pStereoCamera[(int)emPosition] = JsonUtils.Read <StereoCamera>(Json.ReadFromFile(strFName));
}
catch (Exception ex)
{
LotusAPI.Logger.Error(ex.Message);
}
strFName = String.Format("{0:s}\\robot_camera_calibresultP{1:d}.json", ASDef._INI_PATH, emPosition + 1);
try
{
var j = Json.ReadFromFile(strFName);
m_pX[(int)emPosition] = JsonUtils.Read <Matrix44d>(j["X"]);
m_pCamPose[(int)emPosition] = JsonUtils.Read <Matrix44d>(j["CamPose"]);
}
catch (Exception ex)
{
LotusAPI.Logger.Error(ex.Message);
}
}
public float FindXYZ(_emPosition emPosition, ref _stMeasure stMeasure)
{
Point2f p0 = new Point2f((float)stMeasure.pstHole[0].dX, (float)stMeasure.pstHole[0].dY);
Point2f p1 = new Point2f((float)stMeasure.pstHole[1].dX, (float)stMeasure.pstHole[1].dY);
float fRMS;
var center = m_pStereoCamera[(int)emPosition].Triangulate(p0, p1, out fRMS);
Vector4d vcenter = m_pCamPose[(int)emPosition] * LotusAPI.Math.Utils.ToHomogeneous(LotusAPI.Math.Utils.ToVector3d(center));
Vector3d vxyz = LotusAPI.Math.Utils.FromHomogeneous(vcenter);
stMeasure.stShift.dX = vxyz.X;
stMeasure.stShift.dY = vxyz.Y;
stMeasure.stShift.dZ = vxyz.Z;
return(fRMS);
}
