static bool findCircle( Point2f pt0, Point2f pt1, Point2f pt2,
Point2f* center, float* radius )
{
double x1 = (pt0.x + pt1.x) * 0.5;
double dy1 = pt0.x - pt1.x;
double x2 = (pt1.x + pt2.x) * 0.5;
double dy2 = pt1.x - pt2.x;
double y1 = (pt0.y + pt1.y) * 0.5;
double dx1 = pt1.y - pt0.y;
double y2 = (pt1.y + pt2.y) * 0.5;
double dx2 = pt2.y - pt1.y;
double t = 0;
if( intersectLines( x1, dx1, y1, dy1, x2, dx2, y2, dy2, &t ) >= 0 )
{
center->x = (float) (x2 + dx2 * t);
center->y = (float) (y2 + dy2 * t);
*radius = (float)norm(*center - pt0);
return true;
}
center->x = center->y = 0.f;
radius = 0;
return false;
}
void ShapeMaker::cubicToRec( const Point& a, const Point& b, const Point& c, const Point& d, double k, int iteration ) {
Point s = intersectLines( a, b, c, d );
double dx = (a.x+d.x+s.x*4-(b.x+c.x)*3)*.125;
double dy = (a.y+d.y+s.y*4-(b.y+c.y)*3)*.125;
Bezier bz( a, b, c, d );
Bezier b0, b1;
if( dx*dx + dy*dy > k && iteration<10 ) {
// fprintf(stderr,"[%03i] split: %f\n", iteration, dx*dx + dy*dy);
bezierSplit( bz, &b0, &b1 );
// recurse
iteration++;
cubicToRec( a, b0.p1, b0.p2, b0.p3, k, iteration );
//lineTo( b0.p3.x, b0.p3.y );
cubicToRec( b1.p0, b1.p1, b1.p2, d, k, iteration );
//lineTo( b1.p1.x, b1.p1.y );
//lineTo( b1.p2.x, b1.p2.y );
//lineTo( d.x, d.y );
} else {
// fprintf(stderr,"#### %i %i %i %i\n", (int)s.x, (int)s.y, (int)d.x, (int)d.y );
//lineTo( (int)s.x, (int)s.y );
//lineTo( (int)d.x, (int)d.y );
curveTo( s.x, s.y, d.x, d.y );
}
}
int TriangulationSidebar::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
{
_id = QWidget::qt_metacall(_c, _id, _a);
if (_id < 0)
return _id;
if (_c == QMetaObject::InvokeMetaMethod) {
switch (_id) {
case 0: drawLine((*reinterpret_cast< QPointF(*)>(_a[1])),(*reinterpret_cast< QPointF(*)>(_a[2]))); break;
case 1: intersectLines(); break;
case 2: selectPoint((*reinterpret_cast< QPointF(*)>(_a[1])),(*reinterpret_cast< bool(*)>(_a[2]))); break;
case 3: clearSelectedPoints(); break;
case 4: selectLines((*reinterpret_cast< QList<QPair<QPointF,QPointF> >(*)>(_a[1]))); break;
case 5: clearSelectedLines(); break;
case 6: deletePoints(); break;
case 7: deleteLines(); break;
case 8: addPoint((*reinterpret_cast< QPointF(*)>(_a[1]))); break;
case 9: addLine((*reinterpret_cast< QPointF(*)>(_a[1])),(*reinterpret_cast< QPointF(*)>(_a[2]))); break;
case 10: addLandmark((*reinterpret_cast< QPointF(*)>(_a[1]))); break;
case 11: on_landmarksListWidget_itemSelectionChanged(); break;
case 12: on_deleteLines_clicked(); break;
case 13: on_deletePoints_clicked(); break;
case 14: on_intersectLinesButton_clicked(); break;
case 15: on_linesListWidget_itemSelectionChanged(); break;
case 16: on_drawLinesButton_clicked(); break;
case 17: on_pointsListWidget_itemSelectionChanged(); break;
case 18: recalculateDistanceAltitiude(); break;
case 19: newBarometerData((*reinterpret_cast< ushort(*)>(_a[1])),(*reinterpret_cast< ulong(*)>(_a[2]))); break;
case 20: newAccelerometerData((*reinterpret_cast< short(*)>(_a[1])),(*reinterpret_cast< short(*)>(_a[2])),(*reinterpret_cast< short(*)>(_a[3]))); break;
case 21: newRobotElevation((*reinterpret_cast< double(*)>(_a[1])),(*reinterpret_cast< double(*)>(_a[2]))); break;
default: ;
}
_id -= 22;
}
return _id;
}
NavigationWidget::NavigationWidget(QWidget *parent) :
QWidget(parent)
{
setupUi(this);
//navView = new NavigationView(this);
//verticalLayout->insertWidget(0, navView);
connect(navView, SIGNAL(mouseAtGeoPos(QPointF)), this, SLOT(displayCoordinates(QPointF)));
triangulationWidget = new TriangulationWidget();
connect(triangulationButton, SIGNAL(clicked()), triangulationWidget, SLOT(show()));
connect(connectLineButton, SIGNAL(clicked()), navView, SLOT(plotLineForSelected()));
connect(markIsectButton, SIGNAL(clicked()), navView, SLOT(plotLineIntersection()));
connect(requestGPSButton, SIGNAL(clicked()), this, SLOT(sendGPSRequest()));
connect(RobotController::gpsController(), SIGNAL(gotPositionUpdate(GPSPositionData)), this, SLOT(gotGPSUpdate(GPSPositionData)));
connect(navView, SIGNAL(newPointAdded(QPointF)), triangulationSidebar, SLOT(addPoint(QPointF)));
connect(navView, SIGNAL(newLineAdded(QPointF,QPointF)), triangulationSidebar, SLOT(addLine(QPointF,QPointF)));
connect(navView, SIGNAL(newLandmarkAdded(QPointF)), triangulationSidebar, SLOT(addLandmark(QPointF)));
connect(triangulationSidebar, SIGNAL(selectPoint(QPointF, bool)), navView, SLOT(selectPoint(QPointF, bool)));
connect(triangulationSidebar, SIGNAL(clearSelectedPoints()), navView, SLOT(clearSelectedPoints()));
connect(triangulationSidebar, SIGNAL(drawLine(QPointF,QPointF)), navView, SLOT(plotLineForPoints(QPointF,QPointF)));
connect(triangulationSidebar, SIGNAL(selectLines(QList<QPair<QPointF,QPointF> >)), navView, SLOT(selectLines(QList<QPair<QPointF,QPointF> >)));
connect(triangulationSidebar, SIGNAL(clearSelectedLines()), navView, SLOT(clearSelectedLines()));
connect(triangulationSidebar, SIGNAL(intersectLines()), navView, SLOT(plotLineIntersection()));
connect(triangulationSidebar, SIGNAL(deleteLines()), navView, SLOT(deleteSelectedLines()));
connect(triangulationSidebar, SIGNAL(deletePoints()), navView, SLOT(deleteSelectedPoints()));
// Test rover slots
/*navView->setRoverPosition(QPointF(-110.791497, 38.406392));
navView->setRoverPosition(QPointF(-110.790889, 38.406158));
navView->setRoverPosition(QPointF(-110.790469, 38.405850));
navView->setRoverPosition(QPointF(-110.790257, 38.405575));
navView->setRoverPosition(QPointF(-110.790161, 38.405359));
navView->setRoverPosition(QPointF(-110.790177, 38.405195));
navView->setRoverPosition(QPointF(-110.790037, 38.404941));
navView->setRoverPosition(QPointF(-110.789840, 38.404890));
navView->setRoverPosition(QPointF(-110.789420, 38.404986));
navView->setRoverPosition(QPointF(-110.789219, 38.404973));
navView->setRoverHeading(92.334915);*/
//navView->setRoverPosition(QPointF(-123.212968,44.674917)); // adair
//navView->setRoverPosition(QPointF(-123.274951,44.567328)); // covell parking lot
}
void QgsGeometryValidator::checkRingIntersections(
int p0, int i0, const QgsPolyline &ring0,
int p1, int i1, const QgsPolyline &ring1 )
{
for ( int i = 0; !mStop && i < ring0.size() - 1; i++ )
{
QgsVector v = ring0[i+1] - ring0[i];
for ( int j = 0; !mStop && j < ring1.size() - 1; j++ )
{
QgsVector w = ring1[j+1] - ring1[j];
QgsPoint s;
if ( intersectLines( ring0[i], v, ring1[j], w, s ) )
{
double d = -distLine2Point( ring0[i], v.perpVector(), s );
if ( d >= 0 && d <= v.length() )
{
d = -distLine2Point( ring1[j], w.perpVector(), s );
if ( d > 0 && d < w.length() &&
ring0[i+1] != ring1[j+1] && ring0[i+1] != ring1[j] &&
ring0[i+0] != ring1[j+1] && ring0[i+0] != ring1[j] )
{
QString msg = QObject::tr( "segment %1 of ring %2 of polygon %3 intersects segment %4 of ring %5 of polygon %6 at %7" )
.arg( i0 ).arg( i ).arg( p0 )
.arg( i1 ).arg( j ).arg( p1 )
.arg( s.toString() );
QgsDebugMsg( msg );
emit errorFound( QgsGeometry::Error( msg, s ) );
mErrorCount++;
}
}
}
}
}
}
void QgsGeometryValidator::validatePolyline( int i, QgsPolyline line, bool ring )
{
if ( ring )
{
if ( line.size() < 4 )
{
QString msg = QObject::tr( "ring %1 with less than four points" ).arg( i );
QgsDebugMsg( msg );
emit errorFound( QgsGeometry::Error( msg ) );
mErrorCount++;
return;
}
if ( line[0] != line[ line.size()-1 ] )
{
QString msg = QObject::tr( "ring %1 not closed" ).arg( i );
QgsDebugMsg( msg );
emit errorFound( QgsGeometry::Error( msg ) );
mErrorCount++;
return;
}
}
else if ( line.size() < 2 )
{
QString msg = QObject::tr( "line %1 with less than two points" ).arg( i );
QgsDebugMsg( msg );
emit errorFound( QgsGeometry::Error( msg ) );
mErrorCount++;
return;
}
int j = 0;
while ( j < line.size() - 1 )
{
int n = 0;
while ( j < line.size() - 1 && line[j] == line[j+1] )
{
line.remove( j );
n++;
}
if ( n > 0 )
{
QString msg = QObject::tr( "line %1 contains %n duplicate node(s) at %2", "number of duplicate nodes", n ).arg( i ).arg( j );
QgsDebugMsg( msg );
emit errorFound( QgsGeometry::Error( msg, line[j] ) );
mErrorCount++;
}
j++;
}
for ( j = 0; !mStop && j < line.size() - 3; j++ )
{
QgsVector v = line[j+1] - line[j];
double vl = v.length();
int n = ( j == 0 && ring ) ? line.size() - 2 : line.size() - 1;
for ( int k = j + 2; !mStop && k < n; k++ )
{
QgsVector w = line[k+1] - line[k];
QgsPoint s;
if ( !intersectLines( line[j], v, line[k], w, s ) )
continue;
double d = 0.0;
try
{
d = -distLine2Point( line[j], v.perpVector(), s );
}
catch ( QgsException & e )
{
Q_UNUSED( e );
QgsDebugMsg( "Error validating: " + e.what() );
continue;
}
if ( d < 0 || d > vl )
continue;
try
{
d = -distLine2Point( line[k], w.perpVector(), s );
}
catch ( QgsException & e )
{
Q_UNUSED( e );
QgsDebugMsg( "Error validating: " + e.what() );
continue;
}
if ( d <= 0 || d >= w.length() )
continue;
QString msg = QObject::tr( "segments %1 and %2 of line %3 intersect at %4" ).arg( j ).arg( k ).arg( i ).arg( s.toString() );
QgsDebugMsg( msg );
emit errorFound( QgsGeometry::Error( msg, s ) );
mErrorCount++;
}
}
}
// Detect if lines l1 and l2 will intersect between now and the next time step.
inline IntersectionType intersect(Line *l1, Line *l2, double time) {
assert(compareLines(l1, l2) < 0);
if (!rectanglesOverlap(l1, l2)) {
return NO_INTERSECTION;
}
Vec p1 = {.x = l2->p3.x - l1->delta.x, .y = l2->p3.y - l1->delta.y};
Vec p2 = {.x = l2->p4.x - l1->delta.x, .y = l2->p4.y - l1->delta.y};
if (intersectLines(l1->p1, l1->p2, l2->p1, l2->p2)) {
return ALREADY_INTERSECTED;
}
int num_line_intersections = 0;
bool top_intersected = false;
bool bot_intersected = false;
if (intersectLines(l1->p1, l1->p2, p1, p2)) {
num_line_intersections++;
}
if (intersectLines(l1->p1, l1->p2, p1, l2->p1)) {
num_line_intersections++;
top_intersected = true;
}
if (intersectLines(l1->p1, l1->p2, p2, l2->p2)) {
num_line_intersections++;
bot_intersected = true;
}
if (num_line_intersections == 2) {
return L2_WITH_L1;
}
if (pointInParallelogram(l1->p1, l2->p1, l2->p2, p1, p2) &&
pointInParallelogram(l1->p2, l2->p1, l2->p2, p1, p2)) {
return L1_WITH_L2;
}
if (num_line_intersections == 0) {
return NO_INTERSECTION;
}
Vec v1 = Vec_makeFromLine(*l1);
Vec v2 = Vec_makeFromLine(*l2);
double angle = Vec_angle(v1, v2);
if ((top_intersected && angle < 0) ||
(bot_intersected && angle > 0)) {
return L2_WITH_L1;
}
return L1_WITH_L2;
}
// Check if a point is in the parallelogram.
inline bool pointInParallelogram(Vec point, Vec p1, Vec p2, Vec p3, Vec p4) {
double d1 = direction(p1, p2, point);
double d2 = direction(p3, p4, point);
double d3 = direction(p1, p3, point);
double d4 = direction(p2, p4, point);
return d1 * d2 < 0 && d3 * d4 < 0;
}
// Check if two lines intersect.
inline bool intersectLines(Vec p1, Vec p2, Vec p3, Vec p4) {
return side(p1, p2, p3) != side(p1, p2, p4) &&
side(p3, p4, p1) != side(p3, p4, p2);
}
// Obtain the intersection point for two intersecting line segments.
inline Vec getIntersectionPoint(Vec p1, Vec p2, Vec p3, Vec p4) {
double u;
u = ((p4.x - p3.x) * (p1.y - p3.y) -
(p4.y - p3.y) * (p1.x - p3.x)) /
((p4.y - p3.y) * (p2.x - p1.x) -
(p4.x - p3.x) * (p2.y - p1.y));
Vec p;
p.x = p1.x + (p2.x - p1.x) * u;
p.y = p1.y + (p2.y - p1.y) * u;
return p;
}
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;
}
void
Cas1DVanishingPoint::findOrthogonalVanishingPts()
{
size_t min_support = 4;
float min_rho = 10;
std::vector<cv::Vec4i> remaining_lines(mLines);
bool interiorUsed = false;
std::vector<cv::Point2f> vanishingPts;
std::vector<float> vanishingThetas, vanishingRhos;
std::vector<size_t> vanishingSupports;
for (size_t i = 0; i < 2; i++)
{
// showLines(remaining_lines);
std::vector<cv::Point2f> pts = intersectLines(remaining_lines);
std::vector<cv::Point2f> intPts, extPts;
std::vector<float> intThetas, intRhos, extThetas, extRhos;
for (size_t j = 0; j < pts.size(); j++)
{
float rho = hypot(pts[j].x, pts[j].y);
if (rho < 2.0f * mInteriorRadius) intPts.push_back(pts[j]);
if (rho > mInteriorRadius) extPts.push_back(pts[j]);
}
cv::Point2f intVP;
float intVPTheta, intVPRho;
size_t intSupport = 0;
if (!interiorUsed && intPts.size() > 0)
{
findInteriorVanishingPt(remaining_lines, intVPTheta, intVPRho);
intVP.x = intVPRho * cos(intVPTheta);
intVP.y = intVPRho * sin(intVPTheta);
intSupport = linesSupport(intVPTheta, intVPRho, remaining_lines).size();
}
if (extPts.size() == 0) continue;
float extVPTheta, extVPRho;
findExteriorVanishingPt(extPts, extVPTheta, extVPRho);
cv::Point2f extVP;
extVP.x = extVPRho * cos(extVPTheta);
extVP.y = extVPRho * sin(extVPTheta);
size_t extSupport = linesSupport(extVPTheta, extVPRho, remaining_lines).size();
if (extSupport < intSupport)
{
vanishingPts.push_back(intVP);
vanishingThetas.push_back(intVPTheta);
vanishingRhos.push_back(intVPRho);
vanishingSupports.push_back(intSupport);
interiorUsed = true;
}
else
{
vanishingPts.push_back(extVP);
vanishingThetas.push_back(extVPTheta);
vanishingRhos.push_back(extVPRho);
vanishingSupports.push_back(extSupport);
}
removeVanishingLines(vanishingThetas.back(), vanishingRhos.back(), remaining_lines);
}
// showLines(remaining_lines);
if (vanishingPts.size() > 1 && vanishingSupports[0] < vanishingSupports[1])
{
std::swap(vanishingPts[0], vanishingPts[1]);
std::swap(vanishingThetas[0], vanishingThetas[1]);
std::swap(vanishingRhos[0], vanishingRhos[1]);
std::swap(vanishingSupports[0], vanishingSupports[1]);
}
if (vanishingPts.size() == 0 || vanishingSupports[0] < min_support)
{
mMessage = NOTHING_DETECTED;
return;
}
else if (vanishingPts.size() == 1 || vanishingSupports[1] < min_support)
{
mVanishingPts.push_back(vanishingPts[0]);
mMessage = ONE_DETECTED;
return;
}
float angle = mod(vanishingThetas[0] - vanishingThetas[1], 2.0f * CV_PI);
if (angle > CV_PI) angle = 2.0f * CV_PI - angle;
float threshold = CV_PI / 180.0f;
//
// non-orthogonal
if (angle < CV_PI / 2.0f - 10.0f * threshold) mMessage = TWO_NON_ORTHOGONAL_DETECTED;
//
// degen
else if (angle >= CV_PI / 2.0f - 10.0f * threshold && angle < CV_PI / 2.0f + 1.0 * threshold)
{
if (mod(vanishingThetas[1] - vanishingThetas[0], 2.0f * CV_PI) < CV_PI)
vanishingThetas[1] = vanishingThetas[0] + CV_PI / 2.0f;
else vanishingThetas[1] = vanishingThetas[0] - CV_PI / 2.0f;
float theta3rd, rho3rd;
find3rdDegenVanishingPt(intersectLines(remaining_lines), vanishingThetas, vanishingRhos, theta3rd, rho3rd);
if (linesSupport(theta3rd, rho3rd, remaining_lines).size() > min_support)
{
vanishingThetas.push_back(theta3rd);
vanishingRhos.push_back(rho3rd);
vanishingPts.push_back(cv::Point2f(rho3rd * cos(theta3rd), rho3rd * sin(theta3rd)));
if (*std::min_element(vanishingRhos.begin(), vanishingRhos.end()) < min_rho)
{
mMessage = THREE_DETECTED_WITH_TWO_INFINITE;
}
else
{
std::sort(vanishingRhos.begin(), vanishingRhos.end());
mFocal = sqrt(vanishingRhos[0] * vanishingRhos[1]);
mMessage = THREE_DETECTED_WITH_ONE_INFINITE;
}
}
else
{
mMessage = TWO_DETECTED_WITH_ONE_INFINITE;
}
}
// Normal
else
{
float focal = sqrt(-vanishingRhos[0] * vanishingRhos[1] * cos(vanishingThetas[0] - vanishingThetas[1]));
cv::Mat vp(2, 2, CV_64F);
vp.at<double>(0, 0) = vanishingPts[0].x;
vp.at<double>(0, 1) = vanishingPts[0].y;
vp.at<double>(1, 0) = vanishingPts[1].x;
vp.at<double>(1, 1) = vanishingPts[1].y;
cv::Mat guess = vp.inv() * cv::Mat(2, 1, CV_64F, -focal * focal);
float intTheta, intRho, extTheta, extRho;
size_t intSupport = 0, extSupport = 0;
if (cv::norm(guess) < mInteriorRadius * 2 && !interiorUsed)
{
findInteriorVanishingPt(remaining_lines, intTheta, intRho);
intSupport = linesSupport(intTheta, intRho, remaining_lines).size();
}
else
{
findExteriorVanishingPt(intersectLines(remaining_lines), extTheta, extRho);
extSupport = linesSupport(extTheta, extRho, remaining_lines).size();
}
if (std::max(intSupport, extSupport) > min_support)
{
float theta = intSupport > extSupport ? intTheta : extTheta;
float rho = intSupport > extSupport ? intRho : extRho;
vanishingThetas.push_back(theta);
vanishingRhos.push_back(rho);
bool success = refineVanishingPts(intersectLines(mLines), vanishingThetas, vanishingRhos, mFocal);
if (success)
{
vanishingPts.clear();
for (size_t i = 0; i < 3; i++)
vanishingPts.push_back(cv::Point2f(vanishingRhos[i] * cos(vanishingThetas[i]), vanishingRhos[i] * sin(vanishingThetas[i])));
mMessage = THREE_DETECTED_WITH_REFINERY;
}
else
{
vanishingPts.push_back(cv::Point2f(guess.at<double>(0), guess.at<double>(1)));
mFocal = focal;
mMessage = THREE_DETECTED_WITHOUT_REFINERY;
}
}
else
{
vanishingPts.push_back(cv::Point2f(guess.at<double>(0), guess.at<double>(1)));
mFocal = focal;
mMessage = THREE_DETECTED_WITHOUT_REFINERY;
}
}
mVanishingPts = vanishingPts;
/* std::cout << cv::Mat(vanishingPts) << std::endl;
showVanishing(vanishingPts); */
// std::cout << mMessage << std::endl;
}
// Detect if lines l1 and l2 will be intersected between now and the
// next time step.
IntersectionType intersect(Line *l1, Line *l2, double time)
{
Vec vel;
Vec p1, p2;
Vec v1(*l1), v2(*l2);
// Get relative velocity
vel = l2->vel - l1->vel;
// Get the parallelogram
p1 = l2->p1 + (vel * time);
p2 = l2->p2 + (vel * time);
int num_line_intersections = 0;
bool top_intersected = false;
bool bottom_intersected = false;
if (intersectLines(l1->p1, l1->p2, l2->p1, l2->p2)) {
return ALREADY_INTERSECTED;
}
if (intersectLines(l1->p1, l1->p2, p1, p2)) {
num_line_intersections++;
}
if (intersectLines(l1->p1, l1->p2, p1, l2->p1)) {
num_line_intersections++;
top_intersected = true;
}
if (intersectLines(l1->p1, l1->p2, p2, l2->p2)) {
num_line_intersections++;
bottom_intersected = true;
}
if (num_line_intersections == 2) {
return L2_WITH_L1;
}
if (pointInParallelogram(l1->p1, l2->p1, l2->p2, p1, p2) &&
pointInParallelogram(l1->p2, l2->p1, l2->p2, p1, p2)) {
return L1_WITH_L2;
}
if (num_line_intersections == 0) {
return NO_INTERSECTION;
}
double angle = v1.angle(v2);
if (top_intersected) {
if (angle < 0) {
return L2_WITH_L1;
} else {
return L1_WITH_L2;
}
}
if (bottom_intersected) {
if (angle > 0) {
return L2_WITH_L1;
} else {
return L1_WITH_L2;
}
}
return L1_WITH_L2;
}
