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