QList<QImage> NonchipTilesetV2Interface::loadTileList(QString source){
QFile inputFile (source);
inputFile.open(QFile::ReadOnly);
quint8 tileSize;
inputFile.read( ( (char*) &tileSize ),1);
qDebug()<<"loading tileset with tileSize"<<tileSize;
int width = tileSize * 16;
int height = (inputFile.size()-1)/4/width;
qDebug()<<"calculated size:"<<width<<"x"<<height;
QImage inputImage(width, height, QImage::Format_ARGB32);
quint8 value[4];
for (int y = 0; y<height; ++y){
for (int x = 0; x<width; ++x){
inputFile.read( ( (char*) &value ),4);
inputImage.setPixel(x,y,qRgba(value[0],value[1],value[2],value[3]));
}
}
return parseInputImage(inputImage, tileSize);
}
const std::vector<unsigned char> &chilitags::ReadBits::operator()(const cv::Mat &inputImage, const Quad &corners)
{
cv::Mat_<cv::Point2f> cornersCopy(corners);
auto roi = cv::boundingRect(cornersCopy);
// Refine can actually provide corners outside the image
roi.x = cv::max(roi.x, 0);
roi.y = cv::max(roi.y, 0);
roi.width = cv::min(roi.width, inputImage.cols-roi.x);
roi.height = cv::min(roi.height, inputImage.rows-roi.y);
cv::Point2f origin = roi.tl();
for (int i : {0,1,2,3}) cornersCopy(i) -= origin;
cv::Matx33f transformation = cv::getPerspectiveTransform(NORMALIZED_CORNERS, cornersCopy);
cv::perspectiveTransform(mSamplePoints, mTransformedSamplePoints, transformation);
cv::Mat inputRoi = inputImage(roi);
uchar* sampleData = mSamples.ptr(0);
for (auto& transformedSamplePoint : mTransformedSamplePoints) {
*sampleData++ = inputRoi.at<uchar>(transformedSamplePoint);
}
cv::threshold(mSamples, mBits, -1, 1, cv::THRESH_BINARY | cv::THRESH_OTSU);
#ifdef DEBUG_ReadBits
for (int i = 0; i < DATA_SIZE; ++i)
{
for (int j = 0; j < DATA_SIZE; ++j)
{
std::cout << (int) mBits[i*DATA_SIZE + j];
}
std::cout << "\n";
}
std::cout << std::endl;
#define ZOOM_FACTOR 10
cv::Mat debugImage = inputImage.clone();
cv::Mat tag;
cv::resize(inputRoi, tag, cv::Size(0,0), ZOOM_FACTOR, ZOOM_FACTOR, cv::INTER_NEAREST);
cv::cvtColor(tag, tag, cv::COLOR_GRAY2BGR);
for (int i = 0; i < DATA_SIZE; ++i)
{
for (int j = 0; j < DATA_SIZE; ++j)
{
cv::Point2f position = mTransformedSamplePoints[i*DATA_SIZE + j];
cv::circle(tag, position * ZOOM_FACTOR, 1,
cv::Scalar(0,255,0),2);
cv::circle(tag, position * ZOOM_FACTOR, 3,
cv::Scalar::all(mBits[i*DATA_SIZE + j]*255),2);
}
}
cv::circle(tag, *cornersCopy[0] * ZOOM_FACTOR, 3, cv::Scalar(255,0,0),2);
cv::line(tag, *cornersCopy[0]*ZOOM_FACTOR, *cornersCopy[1]*ZOOM_FACTOR,cv::Scalar(255,0,0));
cv::line(tag, *cornersCopy[1]*ZOOM_FACTOR, *cornersCopy[2]*ZOOM_FACTOR,cv::Scalar(255,0,255));
cv::line(tag, *cornersCopy[2]*ZOOM_FACTOR, *cornersCopy[3]*ZOOM_FACTOR,cv::Scalar(255,0,255));
cv::line(tag, *cornersCopy[3]*ZOOM_FACTOR, *cornersCopy[0]*ZOOM_FACTOR,cv::Scalar(255,0,255));
cv::line(tag, *cornersCopy[2]*ZOOM_FACTOR, *cornersCopy[0]*ZOOM_FACTOR,cv::Scalar(255,0,255));
cv::line(tag, *cornersCopy[3]*ZOOM_FACTOR, *cornersCopy[1]*ZOOM_FACTOR,cv::Scalar(255,0,255));
cv::line(debugImage, *cornersCopy[0]+origin, *cornersCopy[1]+origin,cv::Scalar(255,0,255));
cv::line(debugImage, *cornersCopy[1]+origin, *cornersCopy[2]+origin,cv::Scalar(255,0,255));
cv::line(debugImage, *cornersCopy[2]+origin, *cornersCopy[3]+origin,cv::Scalar(255,0,255));
cv::line(debugImage, *cornersCopy[3]+origin, *cornersCopy[0]+origin,cv::Scalar(255,0,255));
cv::imshow("ReadBits-full", debugImage);
cv::imshow("ReadBits-tag", tag);
cv::waitKey(0);
#endif
return mBits;
}
const std::vector<unsigned char>& ReadBits::operator()(const cv::Mat &inputImage, const Quad &corners)
{
static const float TAG_SIZE = 2*TAG_MARGIN+DATA_SIZE;
static const Quad NORMALIZED_CORNERS = {
0.f, 0.f,
TAG_SIZE, 0.f,
TAG_SIZE, TAG_SIZE,
0.f, TAG_SIZE
};
cv::Mat_<cv::Point2f> cornersCopy(corners);
// Sometimes, the corners are refined into a concave quadrilateral
// which makes ReadBits crash
cv::Mat convexHull;
cv::convexHull(cornersCopy, convexHull, true);
if (convexHull.rows != 4) {
cornersCopy = convexHull;
cornersCopy.push_back(0.5f*(cornersCopy(0)+cornersCopy(1)));
}
auto roi = cv::boundingRect(cornersCopy);
// Refine can actually provide corners outside the image
roi.x = cv::max(roi.x, 0);
roi.y = cv::max(roi.y, 0);
roi.width = cv::min(roi.width, inputImage.cols-roi.x);
roi.height = cv::min(roi.height, inputImage.rows-roi.y);
cv::Point2f origin = roi.tl();
for (int i : {0,1,2,3}) cornersCopy(i) -= origin;
cv::Matx33f transformation = cv::getPerspectiveTransform(NORMALIZED_CORNERS, cornersCopy);
cv::perspectiveTransform(mSamplePoints, mTransformedSamplePoints, transformation);
cv::Mat inputRoi = inputImage(roi);
uchar* sampleData = mSamples.ptr(0);
for (auto& transformedSamplePoint : mTransformedSamplePoints) {
transformedSamplePoint.x = cv::max(cv::min(roi.width - 1, (int)std::round(transformedSamplePoint.x)), 0);
transformedSamplePoint.y = cv::max(cv::min(roi.height - 1, (int)std::round(transformedSamplePoint.y)), 0);
*sampleData++ = inputRoi.at<uchar>(transformedSamplePoint);
}
cv::threshold(mSamples, mBits, -1, 1, cv::THRESH_BINARY | cv::THRESH_OTSU);
#ifdef DEBUG_ReadBits
for (int i = 0; i < DATA_SIZE; ++i)
{
for (int j = 0; j < DATA_SIZE; ++j)
{
std::cout << (int) mBits[i*DATA_SIZE + j];
}
std::cout << "\n";
}
std::cout << std::endl;
#define ZOOM_FACTOR 10
cv::Mat debugImage = inputImage.clone();
cv::Mat tag;
cv::resize(inputRoi, tag, cv::Size(0,0), ZOOM_FACTOR, ZOOM_FACTOR, cv::INTER_NEAREST);
cv::cvtColor(tag, tag, cv::COLOR_GRAY2BGR);
for (int i = 0; i < DATA_SIZE; ++i)
{
for (int j = 0; j < DATA_SIZE; ++j)
{
cv::Point2f position = mTransformedSamplePoints[i*DATA_SIZE + j];
cv::circle(tag, position * ZOOM_FACTOR, 1,
cv::Scalar(0,255,0),2);
cv::circle(tag, position * ZOOM_FACTOR, 3,
cv::Scalar::all(mBits[i*DATA_SIZE + j]*255),2);
}
}
cv::circle(tag, *cornersCopy[0] * ZOOM_FACTOR, 3, cv::Scalar(255,0,0),2);
cv::line(tag, *cornersCopy[0]*ZOOM_FACTOR, *cornersCopy[1]*ZOOM_FACTOR,cv::Scalar(255,0,0));
cv::line(tag, *cornersCopy[1]*ZOOM_FACTOR, *cornersCopy[2]*ZOOM_FACTOR,cv::Scalar(255,0,255));
cv::line(tag, *cornersCopy[2]*ZOOM_FACTOR, *cornersCopy[3]*ZOOM_FACTOR,cv::Scalar(255,0,255));
cv::line(tag, *cornersCopy[3]*ZOOM_FACTOR, *cornersCopy[0]*ZOOM_FACTOR,cv::Scalar(255,0,255));
cv::line(tag, *cornersCopy[2]*ZOOM_FACTOR, *cornersCopy[0]*ZOOM_FACTOR,cv::Scalar(255,0,255));
cv::line(tag, *cornersCopy[3]*ZOOM_FACTOR, *cornersCopy[1]*ZOOM_FACTOR,cv::Scalar(255,0,255));
cv::line(debugImage, *cornersCopy[0]+origin, *cornersCopy[1]+origin,cv::Scalar(255,0,255));
cv::line(debugImage, *cornersCopy[1]+origin, *cornersCopy[2]+origin,cv::Scalar(255,0,255));
cv::line(debugImage, *cornersCopy[2]+origin, *cornersCopy[3]+origin,cv::Scalar(255,0,255));
cv::line(debugImage, *cornersCopy[3]+origin, *cornersCopy[0]+origin,cv::Scalar(255,0,255));
cv::imshow("ReadBits-full", debugImage);
cv::imshow("ReadBits-tag", tag);
cv::waitKey(0);
#endif
return mBits;
}
void ImageSourceThread::run(){
if (debug) {
cout << "ImageSourceThread::run: width " << *widthValue << endl;
cout << "ImageSourceThread::run: height " << *heightValue << endl;
cout << "ImageSourceThread::run: noise " << *noiseValue << endl;
cout << "ImageSourceThread::run: window flag " << *windowValue << endl;
cout << "ImageSourceThread::run: random flag " << *randomValue << endl;
cout << "ImageSourceThread::run: horizontal FoV " << *horizontalViewAngleValue << endl;
cout << "ImageSourceThread::run: vertical FoV " << *verticalViewAngleValue << endl;
cout << "ImageSourceThread::run: xShift " << *xShiftValue << endl;
cout << "ImageSourceThread::run: yShift " << *yShiftValue << endl;
}
/*
* copy the image data from file, either scale or extract sub-image, and add noise
*/
double noise;
double azimuth;
double elevation;
/* generate offsets for sub-image extraction */
if (*windowValue == 1) {
windowFlag = true;
if ( (*widthValue < inputImage.width()) && (*heightValue < inputImage.height()) ) {
if (*randomValue == 1) {
// random position of window
xOffset = (int)((float)(inputImage.width() - *widthValue)*((float)rand() / (float)(RAND_MAX)));
yOffset = (int)((float)(inputImage.height() - *heightValue)*((float)rand() / (float)(RAND_MAX)));
}
else {
// regular scan pattern: row major order, with x and y increment equal to the xShift and yShift value
// so that the window scans the complete image (except for borders at the right-hand side and bottom)
xOffset = xOffset + *xShiftValue;
if (xOffset > (inputImage.width() - *widthValue)) {
xOffset = 0;
yOffset = yOffset + *yShiftValue;
if (yOffset > (inputImage.height() - *heightValue)) {
yOffset = 0;
}
}
}
}
else {
xOffset = 0;
yOffset = 0;
}
}
else {
windowFlag = false;
}
if (debug) {
cout << "ImageSourceThread::run: xOffset " << xOffset << endl;
cout << "ImageSourceThread::run: yOffset " << yOffset << endl;
}
ImageOf<PixelRgb> &outputImage = imagePortOut->prepare();
outputImage.resize(*widthValue, *heightValue);
for (x=0; x<outputImage.width(); x++) {
for (y=0; y<outputImage.height(); y++) {
noise = ((float)rand() / (float)(RAND_MAX)); // 0-1
noise = 2 * (noise - 0.5); // -1 - +1
noise = noise * (*noiseValue); // -noiseValue - +noiseValue
// decide whether to copy directly or extract a sub-image
if (windowFlag == false) {
// scale the image
rgbPixel = inputImage((int)(x * ((float)inputImage.width()/(float)outputImage.width())),
(int)(y * ((float)inputImage.height()/(float)outputImage.height())));
}
else {
// extract a sub-image from the original image
rgbPixel = inputImage((int)(x + xOffset),
(int)(y + yOffset));
}
if (((double) rgbPixel.r + noise) < 0)
rgbPixel.r = 0;
else if (((double) rgbPixel.r + noise) > 255)
rgbPixel.r = 255;
else
rgbPixel.r = (unsigned char) (rgbPixel.r + noise);
if (((double) rgbPixel.g + noise) < 0)
rgbPixel.g = 0;
else if (((double) rgbPixel.g + noise) > 255)
rgbPixel.g = 255;
else
rgbPixel.g = (unsigned char) (rgbPixel.g + noise);
if (((double) rgbPixel.b + noise) < 0)
rgbPixel.b = 0;
else if (((double) rgbPixel.b + noise) > 255)
rgbPixel.b = 255;
else
rgbPixel.b = (unsigned char) (rgbPixel.b + noise);
outputImage(x,y) = rgbPixel;
}
}
imagePortOut->write();
/*
* Now write out the simulated gaze angles
*/
if (windowFlag == true) {
azimuth = ((xOffset * 2.0) - ((double)inputImage.width() - (double)outputImage.width())) *
(*horizontalViewAngleValue / (2.0 * (double)inputImage.width()));
elevation = ((yOffset * 2.0) - ((double)inputImage.height() - (double)outputImage.height())) *
(*verticalViewAngleValue / (2.0 * (double)inputImage.height()));
if (debug) {
cout << "ImageSourceThread::run: azimuth " << azimuth << endl;
cout << "ImageSourceThread::run: elevation " << elevation << endl;
}
VectorOf<double> &vctPos = gazePortOut->prepare();
vctPos.resize(5);
vctPos(0) = azimuth;
vctPos(1) = elevation;
vctPos(2) = (double)(int)'a'; // absolute (neck reference) coordinates are sent
vctPos(3) = (double)(int)'s'; // receiver module should do saccades
vctPos(4) = 0; // saccade index
// write output vector
gazePortOut->write();
}
/*
* Now write out the simulated encoder values
*/
VectorOf<double> &vctEnc = encoderPortOut->prepare();
vctEnc.resize(6);
vctEnc(0) = 0;
vctEnc(1) = 1;
vctEnc(2) = 2;
vctEnc(3) = 3;
vctEnc(4) = 4;
vctEnc(5) = 5;
// write output vector
encoderPortOut->write();
}
bool
CppDetEstCircle4 (vector < double >x, vector < double >y,
double cellsize, unsigned int maxR, unsigned int sigmacoef,
double epsion, unsigned int maxIter, unsigned int minnumfit,
vector < double >&detectedcircle,
vector < double >&mapfinalHTcircle,
vector < double >&iterationdata, vector < double >&ArcInfo,
vector < double >&finalx, vector < double >&finaly,
vector < double >&DebugInfo)
{
// based from 0.
// detectedcircle: [yc, xc, r], in unit of meter.
// mapfinalHTcircle: [rowc, colc, r], in unit of pixel.
if (x.size () != y.size ()) {
#ifdef MATLABPRINT
mexPrintf
("CppDetEstCircle ==> input x and y do not have the same length!\n");
#endif
detectedcircle.clear ();
mapfinalHTcircle.clear ();
iterationdata.clear ();
ArcInfo.clear ();
finalx.clear ();
finaly.clear ();
DebugInfo.clear ();
return false;
}
mxArray *mx_x, *mx_y, *mx_refmatrix;
double *tempGetPr;
vector < unsigned int >img, dim_img (2), dim_refmatrix (2);
vector < double >refmatrix;
// call CppGridPts to grid the points cloud into cellsize*cellsize cells
bool callflag;
callflag =
CppGridPts (x, y, cellsize, maxR, minnumfit, img, dim_img, refmatrix,
dim_refmatrix);
if (!callflag) {
#ifdef MATLABPRINT
mexPrintf ("CppDetEstCircle ==> calling CppGridPts failed!\n");
#endif
// maybe inputs are invalid!
detectedcircle.clear ();
mapfinalHTcircle.clear ();
iterationdata.clear ();
ArcInfo.clear ();
finalx.clear ();
finaly.clear ();
DebugInfo.clear ();
return false;
}
if (img.size () == 0 || refmatrix.size () == 0) {
// less than minnumfit points in this section
#ifdef DEBUG
mexPrintf
("CppDetEstCircle ==> calling CppGridPts ==> less than minnumfit points in this section!\n");
#endif
detectedcircle.clear ();
mapfinalHTcircle.clear ();
iterationdata.clear ();
iterationdata.resize (4, 0.0);
iterationdata[0] = 0.0;
iterationdata[1] = mxGetNaN ();
iterationdata[2] = mxGetNaN ();
iterationdata[3] = static_cast < double >(x.size ());
ArcInfo.clear ();
ArcInfo.resize (3, 0.0);
finalx.clear ();
finaly.clear ();
DebugInfo.clear ();
return true;
}
// convert x, y to row and col by MATLAB function: map2pix
// xpix, ypix: based from 0.
vector < double >xpix (x.size ()), ypix (y.size ());
mxArray *prhs[3], *plhs[2];
mx_x = mxCreateDoubleMatrix (x.size (), 1, mxREAL);
mx_y = mxCreateDoubleMatrix (y.size (), 1, mxREAL);
mx_refmatrix =
mxCreateDoubleMatrix (dim_refmatrix[0], dim_refmatrix[1], mxREAL);
tempGetPr = mxGetPr (mx_x);
for (unsigned int i = 0; i < x.size (); i++) {
tempGetPr[i] = x[i];
}
tempGetPr = mxGetPr (mx_y);
for (unsigned int i = 0; i < y.size (); i++) {
tempGetPr[i] = y[i];
}
tempGetPr = mxGetPr (mx_refmatrix);
for (unsigned int i = 0; i < refmatrix.size (); i++) {
tempGetPr[i] = refmatrix[i];
}
prhs[1] = mx_x;
prhs[2] = mx_y;
prhs[0] = mx_refmatrix;
mexCallMATLAB (2, plhs, 3, prhs, "map2pix");
tempGetPr = mxGetPr (plhs[1]);
for (unsigned int i = 0; i < x.size (); i++) {
xpix[i] = tempGetPr[i] - 1;
}
tempGetPr = mxGetPr (plhs[0]);
for (unsigned int i = 0; i < y.size (); i++) {
ypix[i] = tempGetPr[i] - 1;
}
mxDestroyArray (mx_x);
mxDestroyArray (mx_y);
vector < double >estcircle;
CppCircleFitting (xpix, ypix, estcircle);
if (estcircle.size () == 0) {
#ifdef DEBUG
mexPrintf ("CppDetEstCircle ==> CppCircleFitting return empty!\n");
#endif
detectedcircle.clear ();
mapfinalHTcircle.clear ();
iterationdata.clear ();
iterationdata.resize (4, 0.0);
iterationdata[0] = 0.0;
iterationdata[1] = mxGetNaN ();
iterationdata[2] = mxGetNaN ();
iterationdata[3] = static_cast < double >(x.size ());
ArcInfo.clear ();
ArcInfo.resize (3, 0.0);
finalx.clear ();
finaly.clear ();
DebugInfo.clear ();
return true;
}
if (estcircle[2] < 0) {
#ifdef DEBUG
mexPrintf ("CppDetEstCircle ==> CppCircleFitting return negative!\n");
#endif
detectedcircle.clear ();
mapfinalHTcircle.clear ();
iterationdata.clear ();
iterationdata.resize (4, 0.0);
iterationdata[0] = 0.0;
iterationdata[1] = mxGetNaN ();
iterationdata[2] = mxGetNaN ();
iterationdata[3] = static_cast < double >(x.size ());
ArcInfo.clear ();
ArcInfo.resize (3, 0.0);
finalx.clear ();
finaly.clear ();
DebugInfo.clear ();
return true;
}
vector < double >precircle (3, mxGetInf ());
vector < bool > ROIimage (img.size ());
vector < double >ROIxpix, ROIypix;
vector < unsigned int >inputImage (img.size ());
vector < unsigned int >edgeImage (img.size ());
vector < unsigned int >HTrow, HTcol, HTr, HTpeak;
vector < double >zeros (img.size (), 0.0);
vector < double >finalHTcircle, allRsigma;
unsigned int iterativecount = 0;
bool fitflag = true;
double centerchange, radiuschange, finalRsigma;
transform (img.begin (), img.end (), zeros.begin (), ROIimage.begin (),
std::not_equal_to < double >());
centerchange =
sqrt ((estcircle[0] - precircle[0]) * (estcircle[0] - precircle[0]) +
(estcircle[1] - precircle[1]) * (estcircle[1] - precircle[1]));
radiuschange = fabs (estcircle[2] - precircle[2]);
while (centerchange > epsion / cellsize || radiuschange > epsion / cellsize) {
// In this while loop, estcircle and precircle are both in unit of pixel.
// initially detect circle by Hough Transform.
inputImage.clear ();
inputImage.resize (img.size (), 0);
transform (img.begin (), img.end (), ROIimage.begin (),
inputImage.begin (), std::multiplies < unsigned int >());
//int debugcount=0;
//for (unsigned int i=0; i<img.size(); i++)
//{
// if ( (img[i]!=0) && ROIimage[i] )
// {
// inputImage[i]=img[i];
// debugcount++;
// }
//}
HTrow.clear ();
HTcol.clear ();
HTr.clear ();
HTpeak.clear ();
if (!
(CppIterCHT
(inputImage, dim_img, maxR, edgeImage, dim_img, HTrow, HTcol, HTr,
HTpeak)
)) {
#ifdef MATLABPRINT
mexPrintf ("CppDetEstCircle ==> calling CppIterCHT failed!\n");
#endif
return false;
}
if (HTrow.size () == 0) {
// no circle detected by CHT
fitflag = false;
break;
}
// select one circle from the above derived by CHT.
finalHTcircle.clear ();
allRsigma.clear ();
finalRsigma =
CppSelectHTCircle (edgeImage, dim_img, HTrow, HTcol, HTr, finalHTcircle,
allRsigma);
if (finalRsigma < 0) {
#ifdef MATLABPRINT
mexPrintf ("CppDetEstCircle ==> calling CppSelectHTCircle failed!\n");
#endif
return false;
}
// mark ROI
ROIimage.clear ();
if (!
(CppMarkROI
(edgeImage, dim_img, finalHTcircle, xpix, ypix, sigmacoef,
finalRsigma, ROIimage, ROIxpix, ROIypix)
)) {
#ifdef MATLABPRINT
mexPrintf ("CppDetEstCircle ==> calling CppMarkROI failed!\n");
#endif
return false;
}
if (ROIxpix.size () < minnumfit) {
#ifdef DEBUG
mexPrintf
("CppDetEstCircle ==> less than minnumfit points used to fit a circle!\n");
#endif
fitflag = false;
break;
}
precircle.clear ();
precircle.resize (estcircle.size (), 0.0);
copy (estcircle.begin (), estcircle.end (), precircle.begin ());
// fit circles using points in ROI
CppCircleFitting (ROIxpix, ROIypix, estcircle);
if (estcircle.size () == 0) {
#ifdef DEBUG
mexPrintf ("CppDetEstCircle ==> CppCircleFitting return empty!\n");
#endif
fitflag = false;
break;
}
if (estcircle[2] < 0) {
#ifdef DEBUG
mexPrintf ("CppDetEstCircle ==> CppCircleFitting return negative!\n");
#endif
fitflag = false;
break;
}
// calculate change compared with previous circle
centerchange =
sqrt ((estcircle[0] - precircle[0]) * (estcircle[0] - precircle[0]) +
(estcircle[1] - precircle[1]) * (estcircle[1] - precircle[1]));
radiuschange = fabs (estcircle[2] - precircle[2]);
iterativecount += 1;
if (iterativecount == maxIter) {
fitflag = true;
break;
}
}
// Now, we begin to check if estcircle(detectedcircle) is derived in while loop and if the derived is valid.
// We check it from 3 aspects.
// 1st check: whether circle is derived, see if fitflag is true
if (!fitflag) {
detectedcircle.clear ();
mapfinalHTcircle.clear ();
iterationdata.clear ();
iterationdata.resize (4, 0.0);
iterationdata[0] = static_cast < double >(iterativecount);
iterationdata[1] = centerchange * cellsize;
iterationdata[2] = radiuschange * cellsize;
iterationdata[3] = static_cast < double >(ROIxpix.size ());
ArcInfo.clear ();
ArcInfo.resize (3, 0.0);
finalx.clear ();
finaly.clear ();
DebugInfo.clear ();
return true;
}
//2nd check:
//see if the detectedcircle(or estcircle) locates within point
//cloud of trunk, if true, this horizontal point cloud section is
//unqualified, discard the detectedcircle. If in the 3*3 window
//centered at detectedcircle point density in every cell is greater
//than 1, we think the circle center locating within trunk.
double ptsdensity;
ptsdensity =
CppNeighborPtsDensity (img, dim_img, 3, estcircle[0], estcircle[1]);
if (ptsdensity < 0) {
#ifdef MATLABPRINT
mexPrintf ("CppDetEstCircle ==> calling CppNeighborPtsDensity failed!\n");
#endif
return false;
}
if (mxIsInf (ptsdensity)) {
// If ptsdensity is Inf, the derived center locates outside the rasterized image.
detectedcircle.clear ();
mapfinalHTcircle.clear ();
detectedcircle.resize (3, 0.0);
mapfinalHTcircle.resize (3, 0.0);
prhs[0] = mx_refmatrix;
prhs[1] = mxCreateDoubleMatrix (2, 1, mxREAL);
prhs[2] = mxCreateDoubleMatrix (2, 1, mxREAL);
tempGetPr = mxGetPr (prhs[1]);
tempGetPr[0] = estcircle[0] + 1;
tempGetPr[1] = finalHTcircle[0] + 1;
tempGetPr = mxGetPr (prhs[2]);
tempGetPr[0] = estcircle[1] + 1;
tempGetPr[1] = finalHTcircle[1] + 1;
mexCallMATLAB (2, plhs, 3, prhs, "pix2map");
tempGetPr = mxGetPr (plhs[0]); //xc
detectedcircle[1] = tempGetPr[0];
mapfinalHTcircle[1] = tempGetPr[1]; // xc
tempGetPr = mxGetPr (plhs[1]); //yc
detectedcircle[0] = tempGetPr[0];
mapfinalHTcircle[0] = tempGetPr[1]; // yc
detectedcircle[2] = estcircle[2] * cellsize;
mapfinalHTcircle[2] = finalHTcircle[2] * cellsize; // r
mxDestroyArray (prhs[1]);
mxDestroyArray (prhs[2]);
mxDestroyArray (plhs[0]);
mxDestroyArray (plhs[1]);
/*detectedcircle.assign(estcircle.begin(), estcircle.end());
mapfinalHTcircle.assign(finalHTcircle.begin(), finalHTcircle.end()); */
iterationdata.clear ();
iterationdata.resize (4, 0.0);
iterationdata[0] = static_cast < double >(iterativecount);
iterationdata[1] = mxGetNaN ();
iterationdata[2] = mxGetNaN ();
iterationdata[3] = static_cast < double >(ROIxpix.size ());
ArcInfo.clear ();
ArcInfo.resize (3, 0.0);
finalx.clear ();
finaly.clear ();
DebugInfo.clear ();
return true;
} else if (ptsdensity > 1) {
// If ptsdensity is greater than 1, the derived center is on the trunk, not valid!
detectedcircle.clear ();
mapfinalHTcircle.clear ();
iterationdata.clear ();
iterationdata.resize (4, 0.0);
iterationdata[0] = static_cast < double >(iterativecount);
iterationdata[1] = centerchange * cellsize;
iterationdata[2] = radiuschange * cellsize;
iterationdata[3] = static_cast < double >(ROIxpix.size ());
ArcInfo.clear ();
ArcInfo.resize (3, 0.0);
finalx.clear ();
finaly.clear ();
DebugInfo.clear ();
return true;
}
// 3rd check: Iteration (while loop) ends possibly without getting right circle but due to reaching maximum iteration count or failed HT.
// So here the estcircle derived from while loop is checked whether satisfying the right circle condition.
if (iterativecount == maxIter) {
if (centerchange > epsion / cellsize || radiuschange > epsion / cellsize) {
detectedcircle.clear ();
mapfinalHTcircle.clear ();
iterationdata.clear ();
iterationdata.resize (4, 0.0);
iterationdata[0] = static_cast < double >(maxIter);
iterationdata[1] = centerchange * cellsize;
iterationdata[2] = radiuschange * cellsize;
iterationdata[3] = static_cast < double >(ROIxpix.size ());
ArcInfo.clear ();
ArcInfo.resize (3, 0.0);
finalx.clear ();
finaly.clear ();
DebugInfo.clear ();
return true;
}
}
//After all checked, we can output the valid derived circle.
detectedcircle.clear ();
mapfinalHTcircle.clear ();
detectedcircle.resize (3, 0.0);
mapfinalHTcircle.resize (3, 0.0);
prhs[0] = mx_refmatrix;
prhs[1] = mxCreateDoubleMatrix (2, 1, mxREAL);
prhs[2] = mxCreateDoubleMatrix (2, 1, mxREAL);
tempGetPr = mxGetPr (prhs[1]);
tempGetPr[0] = estcircle[0] + 1;
tempGetPr[1] = finalHTcircle[0] + 1;
tempGetPr = mxGetPr (prhs[2]);
tempGetPr[0] = estcircle[1] + 1;
tempGetPr[1] = finalHTcircle[1] + 1;
mexCallMATLAB (2, plhs, 3, prhs, "pix2map");
tempGetPr = mxGetPr (plhs[0]); //xc
detectedcircle[1] = tempGetPr[0];
mapfinalHTcircle[1] = tempGetPr[1]; // xc
tempGetPr = mxGetPr (plhs[1]); //yc
detectedcircle[0] = tempGetPr[0];
mapfinalHTcircle[0] = tempGetPr[1]; // yc
detectedcircle[2] = estcircle[2] * cellsize;
mapfinalHTcircle[2] = finalHTcircle[2] * cellsize; // r
mxDestroyArray (prhs[1]);
mxDestroyArray (prhs[2]);
mxDestroyArray (plhs[0]);
mxDestroyArray (plhs[1]);
// convert ROIxpix, ROIypix to finalx, finaly
finalx.clear ();
finalx.resize (ROIxpix.size ());
finaly.clear ();
finaly.resize (ROIypix.size ());
prhs[0] = mx_refmatrix;
prhs[1] = mxCreateDoubleMatrix (ROIypix.size (), 1, mxREAL);
prhs[2] = mxCreateDoubleMatrix (ROIxpix.size (), 1, mxREAL);
tempGetPr = mxGetPr (prhs[1]);
for (unsigned int i = 0; i < ROIypix.size (); i++) {
tempGetPr[i] = ROIypix[i] + 1;
}
tempGetPr = mxGetPr (prhs[2]);
for (unsigned int i = 0; i < ROIxpix.size (); i++) {
tempGetPr[i] = ROIxpix[i] + 1;
}
mexCallMATLAB (2, plhs, 3, prhs, "pix2map");
tempGetPr = mxGetPr (plhs[0]); //finalx
for (unsigned int i = 0; i < ROIxpix.size (); i++) {
finalx[i] = tempGetPr[i];
}
tempGetPr = mxGetPr (plhs[1]); //finaly
for (unsigned int i = 0; i < ROIypix.size (); i++) {
finaly[i] = tempGetPr[i];
}
mxDestroyArray (prhs[1]);
mxDestroyArray (prhs[2]);
mxDestroyArray (plhs[0]);
mxDestroyArray (plhs[1]);
mxDestroyArray (mx_refmatrix);
iterationdata.clear ();
iterationdata.resize (4, 0.0);
iterationdata[0] = static_cast < double >(iterativecount);
iterationdata[1] = centerchange * cellsize;
iterationdata[2] = radiuschange * cellsize;
iterationdata[3] = static_cast < double >(ROIxpix.size ());
ArcInfo.clear ();
if (!CppArcInfo (ROIxpix, ROIypix, estcircle, ArcInfo)) {
#ifdef MATLABPRINT
mexPrintf ("CppDetEstCircle ==> calling CppArcInfo failed!\n");
#endif
return false;
}
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
// Because when rasterizing point clouds
// in the function CppGridPts the center of the first pixel,
// i.e. the pixel at [1,1] locates at (minx, maxy),
// we have to transform the angle in i-j(raster) coordinate system
// to x-y(point clouds) coordinate system.
// In x-y coordinate system, y axis points to north,
// so angle is presented in counterclockwise.
// In contrast, in i-j coordinate system,
// i axis (i.e. y axis in raster) points to south,
// so angle is presented in clockwise.
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
ArcInfo[0] = 2 * PI - ArcInfo[0];
ArcInfo[1] = 2 * PI - ArcInfo[1];
DebugInfo.clear ();
DebugInfo.resize (1);
DebugInfo[0] = finalRsigma * cellsize;
#ifdef DEBUG
mexPrintf ("CppDetEstCircle ==> finally points used to fit a circle: %d!\n",
ROIxpix.size ());
#endif
return true;
}
int main()
{
// Objects
CameraInput *camera = new CameraInput();
Control *controller = new Control();
Process *processer = new Process();
Tracking *tracker = new Tracking();
Serial_Communication *serial = new Serial_Communication("/dev/ttyUSB0", "/dev/ttyUSB1");
// Serial_Communication *serial = new Serial_Communication("/dev/ttyUSB0");// #### For testing with only one arduino
File_Handler *file_Handler = new File_Handler();
Window_Handler *window_Handler = new Window_Handler();
Menu *menu = new Menu();
// Threads
QThread *t1 = new QThread;
QThread *t2 = new QThread;
QThread *t3 = new QThread;
QThread *t4 = new QThread;
QThread *t5 = new QThread;
camera->moveToThread(t1);
processer->moveToThread(t2);
tracker->moveToThread(t3);
serial->moveToThread(t3);
controller->moveToThread(t4);
file_Handler->moveToThread(t5);
// Connect signals to slots. Whenever a signal is emitted in a function, its corresponding (connected) function will run.
qRegisterMetaType<cv::Mat>("cv::Mat");
//Signals calling from:
//Main thread
QObject::connect(menu, SIGNAL(startRecording(bool)), controller, SLOT(startRecording(bool)));
QObject::connect(menu, SIGNAL(stopRecording()), controller, SLOT(stopRecording()));
QObject::connect(menu, SIGNAL(displayMenu(cv::Mat)), window_Handler, SLOT(drawImage(cv::Mat)));
QObject::connect(menu, SIGNAL(requestDataFromFootController()), serial, SLOT(receiveDataFromFootControllerLoop()));
QObject::connect(menu, SIGNAL(startHighRep()), controller, SLOT(startDelayMode()));
QObject::connect(menu, SIGNAL(decreaseDelay()), controller, SLOT(decreaseDelay()));
QObject::connect(menu, SIGNAL(increaseDelay()), controller, SLOT(increaseDelay()));
QObject::connect(menu, SIGNAL(modeSwitch()), controller, SLOT(endMode()));
QObject::connect(menu, SIGNAL(startPlayback()), file_Handler, SLOT(readFromFile()));
QObject::connect(menu, SIGNAL(stopPlayback()), file_Handler, SLOT(stopVideo()));
QObject::connect(menu, SIGNAL(toggleSlowMotion()), file_Handler, SLOT(toggleSlowMotion()));
QObject::connect(menu, SIGNAL(toggleTracking()), controller, SLOT(toggleTracking()));
//Thread 1
QObject::connect(t1, SIGNAL(started()), camera, SLOT(captureImage()));
QObject::connect(camera, SIGNAL(capturedImage(cv::Mat)), controller, SLOT(inputImage(cv::Mat)));
//Thread 2
QObject::connect(t2, SIGNAL(started()), controller, SLOT(processerReady()));
QObject::connect(processer, SIGNAL(posXposY(int,int)), tracker, SLOT(position(int,int)));
QObject::connect(processer, SIGNAL(readyForWork()), controller, SLOT(processerReady()));
//Thread 3
QObject::connect(tracker, SIGNAL(directionAndSpeed(int,int)), serial, SLOT(sendDataToControlUnit(int,int)));
QObject::connect(serial, SIGNAL(fromFootController(char)), menu, SLOT(giveInput(char)));
//Thread 4
QObject::connect(t4, SIGNAL(started()), controller, SLOT(fileHandlerReadyToWrite()));
QObject::connect(controller, SIGNAL(imageToProcess(cv::Mat)), processer, SLOT(processImage(cv::Mat)));
QObject::connect(controller, SIGNAL(requestImage()), camera, SLOT(captureImage()));
QObject::connect(controller, SIGNAL(imageToRecord(cv::Mat)), file_Handler, SLOT(writeImage(cv::Mat)));
// QObject::connect(controller, SIGNAL(imageToShow(cv::Mat)), window_Handler, SLOT(drawImage(cv::Mat)));
QObject::connect(processer, SIGNAL(processedImage(cv::Mat)), window_Handler, SLOT(drawImage(cv::Mat)));
QObject::connect(controller, SIGNAL(stopMotor()), serial, SLOT(stopMotor()));
//Thread 5
QObject::connect(file_Handler, SIGNAL(showFrame(cv::Mat)), window_Handler, SLOT(drawImage(cv::Mat)));
QObject::connect(file_Handler, SIGNAL(readyToWrite()), controller, SLOT(fileHandlerReadyToWrite()));
QObject::connect(file_Handler, SIGNAL(timeout()), file_Handler, SLOT(playVideo()));
QObject::connect(file_Handler, SIGNAL(playbackEnded()), menu, SLOT(returnToLowRep()));
// Starting Threads
t1->start();
t2->start();
t3->start();
t4->start();
t5->start();
// menu->menu();
menu->inputHandler();
return 0;
}
int main(int argc, const char * argv[]) {
// insert code here...
//std::cout << "Hello, World!\n";
Palette c64palette(c64_colors, num_64_colors);
Ditherer* ditherer = Ditherer::createC64Ditherer();
const char* dirname = argv[1];
const char* outdirname = argv[2];
const char* out64fname = argv[3];
int index = 0;
char thisfname[256];
char nextfname[256];
char outfname[256];
unsigned char* c64frame = NULL;
FILE* fp = fopen(out64fname, "wb");
NetPort port(192,168,1,25,99998,99999);
Tools::Timer frameTimer;
frameTimer.start();
for (index = 1; index < 9999; index++)
{
// check if file can be opened
sprintf(thisfname, "%s/%04d.ppm", dirname, index);
sprintf(nextfname, "%s/%04d.ppm", dirname, index+1);
wait_for_file(nextfname);
printf("%s\n", thisfname);
// open image
Image inputImage(thisfname);
int imWidth = inputImage.getWidth();
int imHeight = inputImage.getHeight();
Image halfImage(inputImage, imWidth/2, imHeight);
Image* dithered = ditherer->createDitheredImageFromImageWithPalette(halfImage, c64palette);
C64Image* c64im = (C64Image*)dithered;
/*
// test - output to new ppm
sprintf(outfname, "%s/%04d.ppm", outdirname, index);
Image fullImage(*dithered, imWidth, imHeight);
fullImage.writePPM(outfname);
*/
int c64FrameSize = c64im->getC64FrameSize();
if (!c64frame)
{
c64frame = (unsigned char*)malloc(sizeof(unsigned char) * c64FrameSize);
}
float time = (float)index / 8.0;
c64im->getC64Frame(c64frame, time);
fwrite(c64frame, 1, c64FrameSize, fp);
float thisTime = frameTimer.getTime();
printf("pts %f, time %f\n", time, thisTime);
while (thisTime < time)
{
float diff = time - thisTime;
float usec = diff * 1000000.0;
usleep(usec);
thisTime = frameTimer.getTime();
}
// send bytes to port
port.send(&c64frame[4], 9216);
delete dithered;
}
fclose(fp);
return 0;
}
