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. }