RTC::ReturnCode_t creekCameraViewer::onExecute(RTC::UniqueId ec_id)
{
// get camera pose
for(int i=0; i<m_cameraPoseIn.size(); i++) {
if( m_cameraPoseIn[i]->isNew() ) {
m_cameraPoseIn[i]->read();
TimedCoordinateSystem tmp;
tmp.tm = toSec(m_cameraPose[i].tm);
tmp.p << m_cameraPose[i].data.position.x, m_cameraPose[i].data.position.y, m_cameraPose[i].data.position.z;
tmp.R = cnoid::rotFromRpy(m_cameraPose[i].data.orientation.r, m_cameraPose[i].data.orientation.p, m_cameraPose[i].data.orientation.y);
// logging
m_tcsSeq[i].push_back(tmp);
if( m_tcsSeq[i].size() > m_maxSeqNum ) {
m_tcsSeq[i].pop_front();
}
}
}
// get image data
for(int i=0; i<m_ports.size(); i++) {
if( m_ports[i]->isNew() ) {
m_ports[i]->read();
if( m_scanner.scan(m_ports[i]->m_zbar) != 0 ) {
bool closeFlag(false);
for(zbar::Image::SymbolIterator symbol = m_ports[i]->m_zbar.symbol_begin(); symbol != m_ports[i]->m_zbar.symbol_end(); ++symbol) {
if( symbol->get_type() == zbar::ZBAR_QRCODE ) {
// get center of QrCode
cv::Point2f center; center.x = 0.0; center.y = 0.0;
int n = symbol->get_location_size();
for(int j=0; j<n; j++) {
center.x += symbol->get_location_x(j);
center.y += symbol->get_location_y(j);
}
center.x = center.x / (float)n;
center.y = center.y / (float)n;
// draw QrCode
if( m_service0.draw() ) {
std::vector<cv::Point2f> points(n);
for(int j=0; j<n; j++) {
points[j].x = symbol->get_location_x(j);
points[j].y = symbol->get_location_y(j);
}
drawFrame(m_ports[i]->m_frame, points, center);
}
if( m_searchFlag[i] ) {
// pixel to vector
cnoid::Vector3 e = pixelToVector(center.x, center.y);
cnoid::Vector3 cp, ce; cnoid::Matrix3 cR;
getCameraPose( toSec(m_ports[i]->m_image.tm), i, cp, cR);
ce = cR * e;
// calc QrCode position
std::map< std::string, QrCodeData >::iterator qrDataIt = m_qrDataSet.find( symbol->get_data() );
if( qrDataIt == m_qrDataSet.end() ) {
QrCodeData qrdata;
qrdata.pos << 0,0,0;
qrdata.data = symbol->get_data();
qrdata.calc = false;
qrdata.p1 = cp;
qrdata.e1 = ce;
qrdata.p2 << 0,0,0;
qrdata.e2 << 0,0,0;
qrdata.name1 = m_ports[i]->name();
qrdata.name2 = "";
m_qrDataSet[ symbol->get_data() ] = qrdata;
std::cout << m_ports[i]->name() << " find QrCode" << std::endl;
closeFlag = true;
}
else if( !qrDataIt->second.calc ) {
cnoid::Vector3 qrposition(0, 0, 0);
if( intersectLines( qrDataIt->second.p1, qrDataIt->second.e1, cp, ce, qrposition ) ) {
qrDataIt->second.pos = qrposition;
qrDataIt->second.calc = true;
qrDataIt->second.p2 = cp;
qrDataIt->second.e2 = ce;
qrDataIt->second.name2 = m_ports[i]->name();
closeFlag = true;
std::cout << "success (" << qrDataIt->second.name1 << ", " << m_ports[i]->name() << " )" << std::endl;
std::cout << " data = " << qrDataIt->first << std::endl;
std::cout << " pos = " << qrposition.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << std::endl;
}
// std::cout << m_ports[i]->name() << " find QrCode" << std::endl;
// Eigen::IOFormat IO(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]");
// std::cout << qrDataIt->second.p1.format(IO) << std::endl;
// std::cout << qrDataIt->second.e1.format(IO) << std::endl;
// std::cout << cp.format(IO) << std::endl;
// std::cout << ce.format(IO) << std::endl << std::endl;
}
}
}
} // end of for loop (zbar symbol iterator)
if( closeFlag )
m_searchFlag[i] = false;
}
}
}
combineImage();
cv::imshow("all", m_allImage);
//cv::waitKey(1);
return RTC::RTC_OK;
}
cLine cGraphicsMFC::pixelToSightLine(int xpix, int ypix)
{ /* This is a line that runs from the viewer's eye to the direction matching the pixel point. */
cVector planepoint = pixelToVector(xpix, ypix, 0.0);
return cLine(planepoint, -cVector::ZAXIS); //Second arg should be a unit vector.
}
