Robot::Robot()
{
m_pArm = new Arm();
m_pCamera = new Camera();
logInit();
// Generated RobotContext
m_pRobotContext = new RobotContext(*m_pCamera, *m_pArm);
m_pRobotContext->SetObserver(SmQtObserver::GetInstance());
QObject::connect(m_pRobotContext,
SIGNAL(end()),
this,
SLOT(contextEnd()));
QObject::connect(m_pArm,
SIGNAL(calibrationDone()),
m_pRobotContext,
SLOT(evArmCalibrationDone()));
QObject::connect(m_pCamera,
SIGNAL(calibrationDone()),
m_pRobotContext,
SLOT(evCameraCalibrationDone()));
}
//------------------------------------------------------------------------------------------------
void CalibrationWidget::doCalibration(
const QString& filePath, const std::vector<int>& filePathModelIndices)
{
namespace pl = std::placeholders;
auto f = std::bind(&ProgressState::emitSignals, calibrationState, pl::_1, pl::_2, pl::_3);
try
{
calibTool.calibrateCamera(f);
}
catch (const std::runtime_error& e)
{
emit calibrationDone(false, QString::fromStdString(e.what()));
return;
}
catch (const cv::Exception& e)
{
emit calibrationDone(false, QString::fromStdString(e.what()));
return;
}
catch (...)
{
emit calibrationDone(false, trUtf8("Unknown exception."));
return;
}
if (calibTool.isStopRequested())
return;
calibTool.saveCameraParameters(filePath.toStdString());
const std::vector<libba::CameraCalibration::CalibImgInfo> imgs = calibTool.getCalibInfo();
// update the imagemodel
for (size_t i = 0; i < imgs.size(); ++i)
{
int modelIdx = filePathModelIndices[i];
ImageModel::ImgData data = imgModel->getImageData(modelIdx);
data.found = imgs[i].patternFound;
data.error = imgs[i].reprojectionError;
data.boardCornersImg = imgs[i].boardCornersImg;
imgModel->setImageData(modelIdx, data);
}
// signals must be used here because otherwise calibrationDone() and the gui would run in
// different threads
emit calibrationDone();
}
void QOpencvProcessor::rectProcess(const cv::Mat &input)
{
cv::Mat output(input); //Copy constructor
unsigned int rectwidth = m_cvRect.width;
unsigned int rectheight = m_cvRect.height;
unsigned int X = m_cvRect.x;
unsigned int Y = m_cvRect.y;
if( (output.rows < (Y + rectheight)) || (output.cols < (X + rectwidth)) )
{
rectheight = 0;
rectwidth = 0;
}
unsigned long red = 0;
unsigned long green = 0;
unsigned long blue = 0;
unsigned long area = 0;
//-------------------------------------------------------------------------
if((rectheight > 0) && (rectwidth > 0))
{
for(int i = 0; i < 256; i++)
v_temphist[i] = 0;
cv::Mat blurRegion(output, m_cvRect);
cv::blur(blurRegion, blurRegion, cv::Size(m_blurSize, m_blurSize));
unsigned char *p; // a pointer to store the adresses of image rows
if(output.channels() == 3)
{
if(m_seekCalibColors)
{
unsigned char tempRed;
unsigned char tempGreen;
unsigned char tempBlue;
for(unsigned int j = Y; j < Y + rectheight; j++)
{
p = output.ptr(j); //takes pointer to beginning of data on rows
for(unsigned int i = X; i < X + rectwidth; i++)
{
tempBlue = p[3*i];
tempGreen = p[3*i+1];
tempRed = p[3*i+2];
if( isCalibColor(tempGreen) && isSkinColor(tempRed, tempGreen, tempBlue) ) {
area++;
blue += tempBlue;
green += tempGreen;
red += tempRed;
if(f_fill) {
//p[3*i] = 255;
//p[3*i+1] %= LEVEL_SHIFT;
p[3*i+2] %= LEVEL_SHIFT;
}
v_temphist[tempGreen]++;
}
}
}
} else {
if(m_skinFlag)
{
unsigned char tempRed;
unsigned char tempGreen;
unsigned char tempBlue;
for(unsigned int j = Y; j < Y + rectheight; j++)
{
p = output.ptr(j); //takes pointer to beginning of data on rows
for(unsigned int i = X; i < X + rectwidth; i++)
{
tempBlue = p[3*i];
tempGreen = p[3*i+1];
tempRed = p[3*i+2];
if( isSkinColor(tempRed, tempGreen, tempBlue)) {
area++;
blue += tempBlue;
green += tempGreen;
red += tempRed;
if(f_fill) {
//p[3*i] = 255;
//p[3*i+1] %= LEVEL_SHIFT;
p[3*i+2] %= LEVEL_SHIFT;
}
v_temphist[tempGreen]++;
}
}
}
}
else
{
for(unsigned int j = Y; j < Y + rectheight; j++)
{
p = output.ptr(j); //takes pointer to beginning of data on rows
for(unsigned int i = X; i < X + rectwidth; i++)
{
blue += p[3*i];
green += p[3*i+1];
red += p[3*i+2];
if(f_fill) {
//p[3*i] = 255;
//p[3*i+1] %= LEVEL_SHIFT;
p[3*i+2] %= LEVEL_SHIFT;
}
v_temphist[p[3*i+1]]++;
}
}
area = rectwidth*rectheight;
}
}
}
else
{
for(unsigned int j = Y; j < Y + rectheight; j++)
{
p = output.ptr(j);//pointer to beginning of data on rows
for(unsigned int i = X; i < X + rectwidth; i++)
{
green += p[i];
if(f_fill) {
p[i] %= LEVEL_SHIFT;
}
v_temphist[p[i]]++;
}
}
area = rectwidth*rectheight;
}
}
//------end of if((rectheight > 0) && (rectwidth > 0))
m_framePeriod = ((double)cv::getTickCount() - m_timeCounter)*1000.0 / cv::getTickFrequency();
m_timeCounter = cv::getTickCount();
if( area > 0 )
{
cv::rectangle( output , m_cvRect, cv::Scalar(15,250,15), 1, CV_AA);
emit dataCollected(red, green, blue, area, m_framePeriod);
unsigned int mass = 0;
for(int i = 0; i < 256; i++)
mass += v_temphist[i];
if(mass > 0)
for(int i = 0; i < 256; i++)
v_hist[i] = (qreal)v_temphist[i]/mass;
emit histUpdated(v_hist, 256);
if(m_calibFlag)
{
v_calibValues[m_calibSamples] = (qreal)green/area;
m_calibMean += v_calibValues[m_calibSamples];
m_calibSamples++;
if(m_calibSamples == CALIBRATION_VECTOR_LENGTH)
{
m_calibMean /= CALIBRATION_VECTOR_LENGTH;
m_calibError = 0.0;
for(quint16 i = 0; i < CALIBRATION_VECTOR_LENGTH; i++)
{
m_calibError += (v_calibValues[i] - m_calibMean)*(v_calibValues[i] - m_calibMean);
}
m_calibError = 10 * sqrt( m_calibError /(CALIBRATION_VECTOR_LENGTH - 1) );
qWarning("mean: %f; error: %f; samples: %d", m_calibMean,m_calibError, m_calibSamples);
m_calibSamples = 0;
m_calibFlag = false;
m_seekCalibColors = true;
emit calibrationDone(m_calibMean, m_calibError/10, m_calibSamples);
}
}
}
else
{
emit selectRegion( QT_TRANSLATE_NOOP("QImageWidget", "Select region on image" ) );
}
emit frameProcessed(output, m_framePeriod, area);
}
