public PuzzleBot(IHost host)
{
_host = host;
_host.WriteLogMessage(c_componentName, "Initializing PuzzleBot comm. channels.");
_machine = new CncMachine(_host);
_upwardCamera = new OpenCV.CaptureEngine(
$"http://{_host.GetParam<string>("MachineHostName")}:{_host.GetParam<int>("UpCameraPort")}/?action=stream");
_downwardCamera = new OpenCV.CaptureEngine(
$"http://{_host.GetParam<string>("MachineHostName")}:{_host.GetParam<int>("DownCameraPort")}/?action=stream");
_upwardCameraView = _host.CreateCameraView("Upward Camera");
_downwardCameraView = _host.CreateCameraView("Downward Camera");
_downwardCameraIntrinsic = _host.GetParam<JObject>("IntrinsticCalib").ToMat();
_downwardCameraExtrinstic = _host.GetParam<JObject>("ExtrinsticCalib").ToMat();
_viewUpdateThread = new Thread(CameraViewUpdater);
_viewUpdateThread.Start();
_host.WriteLogMessage(c_componentName, "Press enter to home...");
_host.ReadLine();
_machine.PerformMechanicalHome();
AttachKeyHandlers();
MainControlLoop();
}
void Routine_CameraCalibration()
{
_host.WriteLogMessage(c_componentName, "Ensuring chessboard configured...");
var cbParams = CameraCalibration.EnsureChessboardConfigured(_host);
var finder = new ChessboardCornerFinder(cbParams.HorizontalSquareCount, cbParams.VerticalSquareCount);
List<Point<float>[]> calPoints = new List<OpenCV.Point<float>[]>();
int camWidth = 0;
int camHeight = 0;
Action findCorner = () => {
_downwardImageOverrideMode = true;
while (true) {
using (var img = _downwardCamera.TryGrabFrame()) {
camWidth = img.Columns;
camHeight = img.Rows;
var result = finder.TryFind(img);
_downwardCameraView.UpdateImage(img);
if (result != null) {
_host.WriteLogMessage(c_componentName, "Found corners!");
calPoints.Add(result);
Thread.Sleep(100);
break;
}
}
}
_downwardImageOverrideMode = false;
};
Action findCornerSet = () => {
findCorner();
Thread.Sleep(100);
findCorner();
Thread.Sleep(100);
findCorner();
Thread.Sleep(100);
};
_host.WriteLogMessage(c_componentName, "Jog machine over first calibration target and press enter.");
_host.ReadLine();
while (true) {
findCornerSet();
_host.WriteLogMessage(c_componentName, "Jog machine over next calibration target and press enter or (N) to stop.");
if (_host.ReadLine().Equals("N")) break;
}
Mat camMat;
Mat distCoeff;
var rms = CameraCalibration.Calibrate(cbParams, calPoints.ToArray(),
new Point<int>() { X = camWidth, Y = camHeight }, out camMat, out distCoeff);
_host.WriteLogMessage(c_componentName, $"Camera claibrated, {rms} error.");
_host.SaveParam("IntrinsticCalib", camMat.ToJObject());
_host.SaveParam("ExtrinsticCalib", distCoeff.ToJObject());
_host.WriteLogMessage(c_componentName, "Camera calibated!");
_downwardCameraExtrinstic = distCoeff;
_downwardCameraIntrinsic = camMat;
}
public static extern Mat Undistort(Mat img, Mat cameraMatrix, Mat distCoeffs);
public static extern bool TryFindChessboardCorners(Mat img, int patternWidth, int patternHeight, float[] cornerPoints);
public static extern void Mat_SaveImage(Mat handle, string filename);
public static extern int Mat_GetType(Mat handle);
public static extern void Mat_GetSteps(Mat handle, int[] steps);
public static extern void ComputeCalibration(int patternWidth, int patternHeight,
float mmPerSquare, int cameraWidth, int cameraHeight, float[] cornerPoints, int numBoards,
double* rms, Mat* distCoeffs, Mat* cameraMatrix);
public static extern int Mat_GetDims(Mat handle);
public static extern byte* Mat_GetData(Mat handle);
public static extern int Mat_GetColumns(Mat handle);
public static extern int Mat_GetChannels(Mat handle);
public static extern Mat Mat_DrawCrosshair(Mat handle);
public static extern void Mat_Destroy(Mat handle);
void Routine_ClearCalibrationData()
{
_downwardCameraIntrinsic = null;
_downwardCameraExtrinstic = null;
}
public static extern int Mat_GetRows(Mat handle);
public static extern bool CaptureEngine_TryGrabFrame(CaptureEngine handle, Mat* frame);