void CStereoMatching::LowestLevelInitialMatch(cv::Mat image[], cv::Mat mask[], cv::Mat &disparity, bool IsZeroOne) { if (Verbose>=2) printf("\t\tlowest level initial match starts...\n"); disparity = cv::Mat(m_data->m_LowestLevelSize, CV_16SC1, cv::Scalar(NOMATCH)); int YL, YR, XL, XR, XL1, XR1; YL = margin[!IsZeroOne].YL; YR = margin[!IsZeroOne].YR; XL = margin[!IsZeroOne].XL; XR = margin[!IsZeroOne].XR; XL1 = margin[IsZeroOne].XL; XR1 = margin[IsZeroOne].XR; int window_size = 2*MatchBlockRadius+1; int vec_size = square_(window_size)*3; #pragma omp parallel for for (int y=YL; y<=YR; y++) { uchar *p = mask[0].ptr<uchar>(y); uchar *q = mask[1].ptr<uchar>(y); short *s = disparity.ptr<short>(y); uchar ** window_ptrL = new uchar *[window_size]; uchar ** window_ptrR = new uchar *[window_size]; for (int i=-MatchBlockRadius; i<=MatchBlockRadius; i++) { window_ptrL[i+MatchBlockRadius] = image[0].ptr<uchar>(y+i); window_ptrR[i+MatchBlockRadius] = image[1].ptr<uchar>(y+i); } for (int x=XL; x<=XR; x++) { // mask if (p[x] != 255)// || window_ptrL[MatchBlockRadius][XL*3] > 200 || window_ptrL[MatchBlockRadius][XL*3+1] > 200 || window_ptrL[MatchBlockRadius][XL*3] > 200) continue; arma::vec vecL(vec_size), vecR(vec_size); double normL = m_data->WindowToVec(window_ptrL, x-MatchBlockRadius, window_size, vecL); vecL /= normL; short temp_i = -1; double CurrentMaxValue = -1; for (int iMatch=XL1; iMatch<=XR1; iMatch++) { if (q[iMatch] != 255) continue; double normR = m_data->WindowToVec(window_ptrR, iMatch-MatchBlockRadius, window_size, vecR); double CurrentValue = arma::dot(vecL, vecR)/normR; if (CurrentValue > CurrentMaxValue) { temp_i = iMatch; CurrentMaxValue = CurrentValue; } } if (temp_i != -1) { s[x] = short(temp_i - x); } } delete [] window_ptrL; delete [] window_ptrR; } }
void ShapesDialog::onSubscribeButtonClicked() { dds::topic::qos::TopicQos topicQos = dp_.default_topic_qos() << dds::core::policy::Durability::Persistent() << dds::core::policy::DurabilityService( dds::core::Duration(3600,0), dds::core::policy::HistoryKind::KEEP_LAST, 100, 8192, 4196, 8192); dds::sub::qos::SubscriberQos SQos = dp_.default_subscriber_qos() << gQos_; dds::sub::Subscriber sub(dp_, SQos); int d = mainWidget.sizeSlider->value(); QRect rect(0, 0, d, d); QRect constr(0, 0, IS_WIDTH, IS_HEIGHT); int x = static_cast<int>(constr.width() * ((float)rand() / RAND_MAX)*0.9F); int y = static_cast<int>(constr.height() * ((float)rand() / RAND_MAX)*0.9F); int sIdx = mainWidget.rShapeList->currentIndex(); QColor gray = QColor(0x99, 0x99, 0x99); QBrush brush(gray, Qt::SolidPattern); QPen pen(QColor(0xff,0xff,0xff), 1, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin); std::vector<std::string> empty; filterParams_ = empty; std::string filterS; if (filterDialog_->isEnabled()) { QRect rect = filterDialog_->getFilterBounds(); std::string x0 = lexicalCast(rect.x()); std::string x1 = lexicalCast(rect.x() + rect.width() -d); std::string y0 = lexicalCast(rect.y()); std::string y1 = lexicalCast(rect.y() + rect.height() -d); filterParams_.push_back(x0); filterParams_.push_back(x1); filterParams_.push_back(y0); filterParams_.push_back(y1); filterS = "(x BETWEEN " + filterParams_[0] + " AND " + filterParams_[1] + ") AND (y BETWEEN " + filterParams_[2] + " AND " + filterParams_[3] + ")"; if (filterDialog_->filterOutside() == false) { filterS = "(x < " + filterParams_[0] + " ) OR ( x > " + filterParams_[1] + " ) OR (y < " + filterParams_[2] + ") OR ( y > " + filterParams_[3] + ")"; } } switch (sIdx) { case CIRCLE: { dds::topic::Topic<ShapeType> circle_(dp_, circleTopicName, topicQos); dds::topic::ContentFilteredTopic<ShapeType> cfcircle_(dds::core::null); dds::sub::DataReader<ShapeType> dr(sub, circle_, readerQos_.get_qos()); if (filterDialog_->isEnabled()) { std::string tname = "CFCircle"; const dds::topic::Filter filter(filterS); std::cout << filterS << std::endl; dds::topic::ContentFilteredTopic<ShapeType> cfcircle_(circle_, "CFCircle", filter); dds::sub::DataReader<ShapeType> dr2(sub, cfcircle_, readerQos_.get_qos()); dr = dr2; } for (int i = 0; i < CN; ++i) { std::string colorStr(colorString_[i]); DDSShapeDynamics::ref_type dynamics(new DDSShapeDynamics(x, y, dr, colorStr, i)); Shape::ref_type circle(new Circle(rect, dynamics, pen, brush, true)); dynamics->setShape(circle); shapesWidget->addShape(circle); } break; } case SQUARE: { dds::topic::Topic<ShapeType> square_(dp_, squareTopicName, topicQos); dds::sub::LoanedSamples<ShapeType>::iterator si; dds::topic::ContentFilteredTopic<ShapeType> cfsquare_(dds::core::null); dds::sub::DataReader<ShapeType> dr(sub, square_, readerQos_.get_qos()); if (filterDialog_->isEnabled()) { std::string tname = "CFSquare"; const dds::topic::Filter filter(filterS); std::cout << filterS << std::endl; dds::topic::ContentFilteredTopic<ShapeType> cfsquare_(square_, "CFSquare", filter); dds::sub::DataReader<ShapeType> dr2(sub, cfsquare_, readerQos_.get_qos()); dr = dr2; } for (int i = 0; i < CN; ++i) { std::string colorStr(colorString_[i]); DDSShapeDynamics::ref_type dynamics(new DDSShapeDynamics(x, y, dr, colorStr, i)); Shape::ref_type square(new Square(rect, dynamics, pen, brush, true)); dynamics->setShape(square); shapesWidget->addShape(square); } break; } case TRIANGLE: { dds::topic::Topic<ShapeType> triangle_(dp_, triangleTopicName, topicQos); dds::sub::LoanedSamples<ShapeType>::iterator si; dds::topic::ContentFilteredTopic<ShapeType> cftriangle_(dds::core::null); dds::sub::DataReader<ShapeType> dr(sub, triangle_, readerQos_.get_qos()); if (filterDialog_->isEnabled()) { std::string tname = "CFTriangle"; const dds::topic::Filter filter(filterS); std::cout << filterS << std::endl; dds::topic::ContentFilteredTopic<ShapeType> cftriangle_(triangle_, "CFTriangle", filter); dds::sub::DataReader<ShapeType> dr2(sub, cftriangle_, readerQos_.get_qos()); dr = dr2; } for (int i = 0; i < CN; ++i) { std::string colorStr(colorString_[i]); DDSShapeDynamics::ref_type dynamics(new DDSShapeDynamics(x, y, dr, colorStr, i)); Shape::ref_type triangle(new Triangle(rect, dynamics, pen, brush, true)); dynamics->setShape(triangle); shapesWidget->addShape(triangle); } break; } default: break; } }
void ShapesDialog::onPublishButtonClicked() { dds::topic::qos::TopicQos topicQos = dp_.default_topic_qos() << dds::core::policy::Durability::Persistent() << dds::core::policy::DurabilityService(dds::core::Duration(3600,0), dds::core::policy::HistoryKind::KEEP_LAST, 100, 8192, 4196, 8192); dds::pub::qos::PublisherQos PQos = dp_.default_publisher_qos() << gQos_; dds::pub::Publisher pub(dp_, PQos); int d = mainWidget.sizeSlider->value(); float speed = ((float)mainWidget.speedSlider->value()) / 9; QRect rect(0, 0, d, d); // TODO: This should be retrieved from the canvas... QRect constr(0, 0, IS_WIDTH, IS_HEIGHT); // QRect constr = this->geometry(); int x = constr.width() * ((float)rand() / RAND_MAX); int y = constr.height() * ((float)rand() / RAND_MAX); int cIdx = mainWidget.colorList->currentIndex(); int sIdx = mainWidget.wShapeList->currentIndex(); QBrush brush(color_[cIdx], Qt::SolidPattern); QPen pen(QColor(0xff, 0xff, 0xff), 1, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin); ShapeType shape; shape.color = DDS::string_dup(colorString_[cIdx]); shape.shapesize = rect.width(); shape.x = x; shape.y = y; switch (sIdx) { case CIRCLE: { dds::topic::Topic<ShapeType> circle_(dp_, circleTopicName, topicQos); dds::pub::qos::DataWriterQos dwqos = circle_.qos(); dds::pub::DataWriter<ShapeType> dw(pub, circle_, writerQos_.get_qos()); BouncingShapeDynamics::ref_type dynamics(new BouncingShapeDynamics(x, y, rect, constr, PI/6, speed, shape, dw)); Shape::ref_type circle(new Circle(rect, dynamics, pen, brush)); shapesWidget->addShape(circle); break; } case SQUARE: { dds::topic::Topic<ShapeType> square_(dp_, squareTopicName, topicQos); dds::pub::qos::DataWriterQos dwqos = square_.qos(); dds::pub::DataWriter<ShapeType> dw(pub, square_, writerQos_.get_qos()); BouncingShapeDynamics::ref_type dynamics(new BouncingShapeDynamics(x, y, rect, constr, PI/6, speed, shape, dw)); Shape::ref_type square(new Square(rect, dynamics, pen, brush)); shapesWidget->addShape(square); break; } case TRIANGLE: { dds::topic::Topic<ShapeType> triangle_(dp_, triangleTopicName, topicQos); dds::pub::qos::DataWriterQos dwqos = triangle_.qos(); dds::pub::DataWriter<ShapeType> dw(pub, triangle_, writerQos_.get_qos()); BouncingShapeDynamics::ref_type dynamics(new BouncingShapeDynamics(x, y, rect, constr, PI/6, speed, shape, dw)); Shape::ref_type triangle(new Triangle(rect, dynamics, pen, brush)); shapesWidget->addShape(triangle); break; } default: break; }; }
// input: src_disparity<double> // output: dst_disparity<short> void CStereoMatching::HighLevelInitialMatch(cv::Mat image[], cv::Mat mask[], cv::Mat &disparity, int Pyrm_depth, bool IsZeroOne) { if (Verbose>=2) printf("\t\thigh level initial match starts...\n"); cv::Mat dst_disparity(image[0].size(), CV_16S, cv::Scalar(NOMATCH)); int YL, YR, XL, XR, XL1, XR1, YR1; YL = margin[!IsZeroOne].YL; YR = margin[!IsZeroOne].YR; XL = margin[!IsZeroOne].XL; XR = margin[!IsZeroOne].XR; XL1 = margin[IsZeroOne].XL; XR1 = margin[IsZeroOne].XR; YR1 = margin[IsZeroOne].YR; int window_size = 2*MatchBlockRadius+1; int vec_size = square_(window_size)*3; #pragma omp parallel for for (int y=YL; y<=YR; y++) { uchar *p = mask[0].ptr<uchar>(y); uchar *q = mask[1].ptr<uchar>(y); uchar ** window_ptrL = new uchar *[window_size]; uchar ** window_ptrR = new uchar *[window_size]; for (int i=-MatchBlockRadius; i<=MatchBlockRadius; i++) { window_ptrL[i+MatchBlockRadius] = image[0].ptr<uchar>(y+i); window_ptrR[i+MatchBlockRadius] = image[1].ptr<uchar>(y+i); } short *d = dst_disparity.ptr<short>(y); double *s = disparity.ptr<double>(int((y+1)/2.0));//@@û¿´¶®MIN(int((y+1)/2.0), YR1) int boundary_L = XL1; int boundary_R = XR1; for (int x=XL; x<=XR; x++) { // mask if (p[x] != 255)// || window_ptrL[MatchBlockRadius][XL*3] > 200 || window_ptrL[MatchBlockRadius][XL*3+1] > 200 || window_ptrL[MatchBlockRadius][XL*3] > 200) continue; int temp2 = int((x+1)/2.0);//@@û¿´¶®MIN(int((x+1)/2.0), XR1>>1) arma::vec vecL(vec_size), vecR(vec_size); double normL = m_data->WindowToVec(window_ptrL, x-MatchBlockRadius, window_size, vecL); vecL /= normL; ushort temp_i; double CurrentMaxValue = -1; if (s[temp2] == NOMATCH) { for (int i=temp2+1; i<=XR>>1 ; i++) { if (s[i] != NOMATCH) { boundary_R = MIN(i + int(s[i]*2) + m_offset + 1, XR1); break; } } } else { boundary_L = MAX(x + int(s[temp2]*2+0.5) - m_offset, XL1); boundary_R = MIN(x + int(s[temp2]*2+0.5) + m_offset, XR1); } for (int iMatch=boundary_L; iMatch<=boundary_R; iMatch++) { if (q[iMatch] != 255) continue; double normR = m_data->WindowToVec(window_ptrR, iMatch-MatchBlockRadius, window_size, vecR); double CurrentValue = arma::dot(vecL, vecR)/normR; if (CurrentValue > CurrentMaxValue) { CurrentMaxValue = CurrentValue; temp_i = iMatch; } } if (CurrentMaxValue > -1) d[x] = short(temp_i - x); } delete [] window_ptrL; delete [] window_ptrR; }