void CvHoughLines_Processor::onNewImage()
{
if (in_img.empty()) {
LOG(LFATAL) << "Component " << name() << " " << "in_img input stream is empty.";
return;
}
Mat image = in_img.read();
if (image.channels() != 1) {
LOG(LFATAL) << "Component " << name() << " " << "in_img received image must have only one channel.";
return;
}
Types::DrawableContainer c;
vector<Vec4i> lines;
cv::HoughLinesP( image, lines, 1, CV_PI/180, threshold, minLineLength, maxLineGap);
CLOG(LDEBUG) << "Found " << lines.size() << " lines";
for( size_t i = 0; i < lines.size(); i++ )
{
c.add(new Types::Line(cv::Point(lines[i][0], lines[i][1]), cv::Point(lines[i][2], lines[i][3])));
}
out_lines.write(c);
}
void DrawSystem::drawSys() {
cv::Mat image = in_img.read().clone();
std::vector<cv::Point2f> impoints = in_impoints.read();
Types::DrawableContainer csystem = in_csystem.read();
//csystem.draw(image, cv::Scalar(0,255,0));
for(int i = 0; i < csystem.size(); ++i) {
Types::Line * line = dynamic_cast <Types::Line *> (csystem.get(i));
//line->draw(image, cv::Scalar(0,255,0));
cv::line(image, line->getP1(), line->getP2(), line->getCol(), 13);
if(i == 0)
cv::putText(image, "X", line->getP2(), cv::FONT_HERSHEY_SIMPLEX, 3.5, line->getCol(), 7, 8, false);
if(i == 1)
cv::putText(image, "Y", line->getP2(), cv::FONT_HERSHEY_SIMPLEX, 3.5, line->getCol(), 7, 8, false);
if(i == 2)
cv::putText(image, "Z", line->getP2(), cv::FONT_HERSHEY_SIMPLEX, 3.5, line->getCol(), 7, 8, false);
}
for(int i = 0; i < impoints.size(); ++i) {
circle(image, impoints[i], 15, cv::Scalar(0, 255, 0), -1, 8, 0);
}
out_img.write(image);
}
void CvWindow_Sink::onNewImageN(int n) {
CLOG(LTRACE) << name() << "::onNewImage(" << n << ")";
try {
if (!in_img[n]->empty()) {
img[n] = in_img[n]->read().clone();
}
Types::DrawableContainer ctr;
while (!in_draw[n]->empty()) {
ctr.add(in_draw[n]->read()->clone());
to_draw[n] = boost::shared_ptr<Types::Drawable>(ctr.clone());
}
if (to_draw[n]) {
to_draw[n]->draw(img[n], CV_RGB(255,0,255));
// TODO: dodać wygaszanie starszych drawable, np. przez 10 odświeżeń
//to_draw[n] = boost::shared_ptr<Types::Drawable>();
}
// Display image.
//onStep();
} catch (std::exception &ex) {
CLOG(LERROR) << "CvWindow::onNewImage failed: " << ex.what() << "\n";
}
}
void CvWindow_Sink::onNewImageN(int n) {
CLOG(LTRACE) << name() << "::onNewImage(" << n << ")";
try {
if (!in_img[n]->empty()) {
img[n] = in_img[n]->read().clone();
}
if (to_draw_timeout[n])
--to_draw_timeout[n];
Types::DrawableContainer ctr;
while (!in_draw[n]->empty()) {
ctr.add(in_draw[n]->read()->clone());
to_draw[n] = boost::shared_ptr<Types::Drawable>(ctr.clone());
to_draw_timeout[n] = 10;
}
if (to_draw[n]) {
float opacity = 0.1 * to_draw_timeout[n];
if (opacity > 0.01) {
cv::Mat overlay;
img[n].copyTo(overlay);
to_draw[n]->draw(overlay, CV_RGB(255,0,255));
cv::addWeighted(overlay, opacity, img[n], 1-opacity, 0, img[n]);
}
}
// Display image.
//onStep();
} catch (std::exception &ex) {
CLOG(LERROR) << "CvWindow::onNewImage failed: " << ex.what() << "";
}
}
bool MS_Blueball_Decide::onStep()
{
LOG(LTRACE) << "MS_Blueball_Decide::step\n";
blobs_ready = hue_ready = false;
try {
int id = 0;
int i;
Types::Blobs::Blob *currentBlob;
Types::DrawableContainer Blueballs;
// iterate through all found blobs
for (i = 0; i < blobs.GetNumBlobs(); i++ )
{
currentBlob = blobs.GetBlob(i);
// // get mean color from area coverd by blob (from hue component)
// double me = currentBlob->Mean(&h);
// double st = currentBlob->StdDev(&h);
// get blob bounding rectangle and ellipse
CvBox2D r2 = currentBlob->GetEllipse();
// // blob moments
// double m00, m10, m01, m11, m02, m20;
// double M11, M02, M20, M7;
//
// // calculate moments
// m00 = currentBlob->Moment(0,0);
// m01 = currentBlob->Moment(0,1);
// m10 = currentBlob->Moment(1,0);
// m11 = currentBlob->Moment(1,1);
// m02 = currentBlob->Moment(0,2);
// m20 = currentBlob->Moment(2,0);
//
// M11 = m11 - (m10*m01)/m00;
// M02 = m02 - (m01*m01)/m00;
// M20 = m20 - (m10*m10)/m00;
//
// // for circle it should be ~0.0063
// M7 = (M20*M02-M11*M11) / (m00*m00*m00*m00);
std::cout << "Center: " << r2.center.x << "," << r2.center.y << "\n";
++id;
Blueballs.add(new Types::Ellipse(Point(r2.center.x, r2.center.y), Size(r2.size.width, r2.size.height), r2.angle));
}
out_balls.write(Blueballs);
newImage->raise();
return true;
} catch (...) {
LOG(LERROR) << "MS_Blueball_Decide::onNewImage failed\n";
return false;
}
}
bool MS_Barcode_Decide::onStep()
{
blobs_ready = hue_ready = false;
try {
int i;
IplImage h = IplImage(hue_img);
Types::Blobs::Blob *currentBlob;
Types::DrawableContainer signs;
// iterate through all found blobs
for (i = 0; i < blobs.GetNumBlobs(); i++ )
{
currentBlob = blobs.GetBlob(i);
// get mean color from area coverd by blob (from hue component)
double me = currentBlob->Mean(&h);
double st = currentBlob->StdDev(&h);
// get blob bounding rectangle and ellipse
//CvBox2D be = currentBlob->GetEllipse();
cv::Rect bb = currentBlob->GetBoundingBox();
signs.add(new Types::Rectangle(bb.x, bb.y, bb.width, bb.height));
}
out_signs.write(signs);
newImage->raise();
return true;
} catch (...) {
LOG(LERROR) << "MS_Sign_Decide::onNewImage failed\n";
return false;
}
}
bool ExtractBlocks_Processor::onStep()
{
LOG(LTRACE) << "ExtractBlocks_Processor::step\n";
blobs_ready = hue_ready = false;
try {
int id = 0;
int i;
IplImage h = IplImage(hue_img);
Types::Blobs::Blob *currentBlob;
Types::DrawableContainer blocks;
// iterate through all found blobs
for (i = 0; i < blobs.GetNumBlobs(); i++ )
{
currentBlob = blobs.GetBlob(i);
// get blob bounding rectangle
CvRect r2 = currentBlob->GetBoundingBox();
++id;
blocks.add(new Types::Rectangle(r2.x, r2.y, r2.width, r2.height));
out_blocks.write(blocks);
newImage->raise();
}
return true;
} catch (...) {
LOG(LERROR) << "ExtractBlocks_Processor::onNewImage failed\n";
return false;
}
}
void DrawCoordinateSystem::projectPoints(){
cv::Mat_<double> rvec(3,1);
cv::Mat_<double> tvec(3,1);
Types::HomogMatrix homogMatrix;
cv::Mat_<double> rotMatrix;
rotMatrix.create(3,3);
// Try to read rotation and translation from rvec and tvec or homogenous matrix.
if(!in_rvec.empty()&&!in_tvec.empty()){
rvec = in_rvec.read();
tvec = in_tvec.read();
}
else if(!in_homogMatrix.empty()){
homogMatrix = in_homogMatrix.read();
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
rotMatrix(i,j)=homogMatrix(i, j);
}
tvec(i, 0) = homogMatrix(i, 3);
}
Rodrigues(rotMatrix, rvec);
}
else
return;
// Get camera info properties.
Types::CameraInfo camera_matrix = in_camera_matrix.read();
vector<cv::Point3f> object_points;
vector<cv::Point2f> image_points;
// Create four points constituting ends of three lines.
object_points.push_back(cv::Point3f(0,0,0));
object_points.push_back(cv::Point3f(0.1,0,0));
object_points.push_back(cv::Point3f(0,0.1,0));
object_points.push_back(cv::Point3f(0,0,0.1));
// Project points taking into account the camera properties.
if (rectified) {
cv::Mat K, _r, _t;
cv::decomposeProjectionMatrix(camera_matrix.projectionMatrix(), K, _r, _t);
cv::projectPoints(object_points, rvec, tvec, K, cv::Mat(), image_points);
} else {
cv::projectPoints(object_points, rvec, tvec, camera_matrix.cameraMatrix(), camera_matrix.distCoeffs(), image_points);
}
// Draw lines between projected points.
Types::Line ax(image_points[0], image_points[1]);
ax.setCol(cv::Scalar(255, 0, 0));
Types::Line ay(image_points[0], image_points[2]);
ay.setCol(cv::Scalar(0, 255, 0));
Types::Line az(image_points[0], image_points[3]);
az.setCol(cv::Scalar(0, 0, 255));
// Add lines to container.
Types::DrawableContainer ctr;
ctr.add(ax.clone());
ctr.add(ay.clone());
ctr.add(az.clone());
CLOG(LINFO)<<"Image Points: "<<image_points;
// Write output to dataports.
out_csystem.write(ctr);
out_impoints.write(image_points);
}
