~paparazzi-uav/paparazzi/v5.0-manual

void detect(InputArray edges, InputArray dx, InputArray dy, OutputArray positions, OutputArray votes) { detectImpl(edges, dx, dy, positions, votes); }

339

340

void setCannyLowThresh(int cannyLowThresh) { cannyLowThresh_ = cannyLowThresh; }

341

int getCannyLowThresh() const { return cannyLowThresh_; }

342

343

void setCannyHighThresh(int cannyHighThresh) { cannyHighThresh_ = cannyHighThresh; }

344

int getCannyHighThresh() const { return cannyHighThresh_; }

345

346

void setMinDist(double minDist) { minDist_ = minDist; }

347

double getMinDist() const { return minDist_; }

348

349

void setDp(double dp) { dp_ = dp; }

350

double getDp() const { return dp_; }

351

352

void setMaxBufferSize(int) { }

353

int getMaxBufferSize() const { return 0; }

354

355

void setLevels(int levels) { levels_ = levels; }

356

int getLevels() const { return levels_; }

357

358

void setVotesThreshold(int votesThreshold) { votesThreshold_ = votesThreshold; }

359

int getVotesThreshold() const { return votesThreshold_; }

360

361

private:

362

void processTempl();

363

void processImage();

364

365

void calcHist();

366

void findPosInHist();

367

368

int levels_;

369

int votesThreshold_;

370

371

std::vector< std::vector<Point> > r_table_;

372

Mat hist_;

373

};

374

375

GeneralizedHoughBallardImpl::GeneralizedHoughBallardImpl()

376

{

377

levels_ = 360;

378

votesThreshold_ = 100;

379

}

380

381

void GeneralizedHoughBallardImpl::processTempl()

382

{

383

CV_Assert( levels_ > 0 );

384

385

const double thetaScale = levels_ / 360.0;

386

387

r_table_.resize(levels_ + 1);

388

std::for_each(r_table_.begin(), r_table_.end(), std::mem_fun_ref(&std::vector<Point>::clear));

389

390

for (int y = 0; y < templSize_.height; ++y)

391

{

392

const uchar* edgesRow = templEdges_.ptr(y);

393

const float* dxRow = templDx_.ptr<float>(y);

394

const float* dyRow = templDy_.ptr<float>(y);

395

396

for (int x = 0; x < templSize_.width; ++x)

397

{

398

const Point p(x, y);

399

400

if (edgesRow[x] && (notNull(dyRow[x]) || notNull(dxRow[x])))

401

{

402

const float theta = fastAtan2(dyRow[x], dxRow[x]);

403

const int n = cvRound(theta * thetaScale);

404

r_table_[n].push_back(p - templCenter_);

405

}

406

}

407

}

408

}

409

410

void GeneralizedHoughBallardImpl::processImage()

411

{

412

calcHist();

413

findPosInHist();

414

}

415

416

void GeneralizedHoughBallardImpl::calcHist()

417

{

418

CV_Assert( imageEdges_.type() == CV_8UC1 );

419

CV_Assert( imageDx_.type() == CV_32FC1 && imageDx_.size() == imageSize_);

420

CV_Assert( imageDy_.type() == imageDx_.type() && imageDy_.size() == imageSize_);

421

CV_Assert( levels_ > 0 && r_table_.size() == static_cast<size_t>(levels_ + 1) );

422

CV_Assert( dp_ > 0.0 );

423

424

const double thetaScale = levels_ / 360.0;

425

const double idp = 1.0 / dp_;

426

427

hist_.create(cvCeil(imageSize_.height * idp) + 2, cvCeil(imageSize_.width * idp) + 2, CV_32SC1);

428

hist_.setTo(0);

429

430

const int rows = hist_.rows - 2;

431

const int cols = hist_.cols - 2;

432

433

for (int y = 0; y < imageSize_.height; ++y)

434

{

435

const uchar* edgesRow = imageEdges_.ptr(y);

436

const float* dxRow = imageDx_.ptr<float>(y);

437

const float* dyRow = imageDy_.ptr<float>(y);

438

439

for (int x = 0; x < imageSize_.width; ++x)

440

{

441

const Point p(x, y);

442

443

if (edgesRow[x] && (notNull(dyRow[x]) || notNull(dxRow[x])))

444

{

445

const float theta = fastAtan2(dyRow[x], dxRow[x]);

446

const int n = cvRound(theta * thetaScale);

447

448

const std::vector<Point>& r_row = r_table_[n];

449

450

for (size_t j = 0; j < r_row.size(); ++j)

451

{

452

Point c = p - r_row[j];

453

454

c.x = cvRound(c.x * idp);

455

c.y = cvRound(c.y * idp);

456

457

if (c.x >= 0 && c.x < cols && c.y >= 0 && c.y < rows)

458

++hist_.at<int>(c.y + 1, c.x + 1);

459

}

460

}

461

}

462

}

463

}

464

465

void GeneralizedHoughBallardImpl::findPosInHist()

466

{

467

CV_Assert( votesThreshold_ > 0 );

468

469

const int histRows = hist_.rows - 2;

470

const int histCols = hist_.cols - 2;

471

472

for(int y = 0; y < histRows; ++y)

473

{

474

const int* prevRow = hist_.ptr<int>(y);

475

const int* curRow = hist_.ptr<int>(y + 1);

476

const int* nextRow = hist_.ptr<int>(y + 2);

477

478

for(int x = 0; x < histCols; ++x)

479

{

480

const int votes = curRow[x + 1];

481

482

if (votes > votesThreshold_ && votes > curRow[x] && votes >= curRow[x + 2] && votes > prevRow[x + 1] && votes >= nextRow[x + 1])

483

{

484

posOutBuf_.push_back(Vec4f(static_cast<float>(x * dp_), static_cast<float>(y * dp_), 1.0f, 0.0f));

485

voteOutBuf_.push_back(Vec3i(votes, 0, 0));

486

}

487

}

488

}

489

}

490

}

491

492

Ptr<GeneralizedHoughBallard> cv::createGeneralizedHoughBallard()

493

{

494

return makePtr<GeneralizedHoughBallardImpl>();

495

}

496

497

// GeneralizedHoughGuil

498

499

namespace

500

{

501

class GeneralizedHoughGuilImpl : public GeneralizedHoughGuil, private GeneralizedHoughBase

502

{

503

public:

504

GeneralizedHoughGuilImpl();

505

506

void setTemplate(InputArray templ, Point templCenter) { setTemplateImpl(templ, templCenter); }

507

void setTemplate(InputArray edges, InputArray dx, InputArray dy, Point templCenter) { setTemplateImpl(edges, dx, dy, templCenter); }

508

509

void detect(InputArray image, OutputArray positions, OutputArray votes) { detectImpl(image, positions, votes); }

510

void detect(InputArray edges, InputArray dx, InputArray dy, OutputArray positions, OutputArray votes) { detectImpl(edges, dx, dy, positions, votes); }

511

512

void setCannyLowThresh(int cannyLowThresh) { cannyLowThresh_ = cannyLowThresh; }

513

int getCannyLowThresh() const { return cannyLowThresh_; }

514

515

void setCannyHighThresh(int cannyHighThresh) { cannyHighThresh_ = cannyHighThresh; }

516

int getCannyHighThresh() const { return cannyHighThresh_; }

517

518

void setMinDist(double minDist) { minDist_ = minDist; }

519

double getMinDist() const { return minDist_; }

520

521

void setDp(double dp) { dp_ = dp; }

522

double getDp() const { return dp_; }

523

524

void setMaxBufferSize(int maxBufferSize) { maxBufferSize_ = maxBufferSize; }

525

int getMaxBufferSize() const { return maxBufferSize_; }

526

527

void setXi(double xi) { xi_ = xi; }

528

double getXi() const { return xi_; }

529

530

void setLevels(int levels) { levels_ = levels; }

531

int getLevels() const { return levels_; }

532

533

void setAngleEpsilon(double angleEpsilon) { angleEpsilon_ = angleEpsilon; }

534

double getAngleEpsilon() const { return angleEpsilon_; }

535

536

void setMinAngle(double minAngle) { minAngle_ = minAngle; }

537

double getMinAngle() const { return minAngle_; }

538

539

void setMaxAngle(double maxAngle) { maxAngle_ = maxAngle; }

540

double getMaxAngle() const { return maxAngle_; }

541

542

void setAngleStep(double angleStep) { angleStep_ = angleStep; }

543

double getAngleStep() const { return angleStep_; }

544

545

void setAngleThresh(int angleThresh) { angleThresh_ = angleThresh; }

546

int getAngleThresh() const { return angleThresh_; }

547

548

void setMinScale(double minScale) { minScale_ = minScale; }

549

double getMinScale() const { return minScale_; }

550

551

void setMaxScale(double maxScale) { maxScale_ = maxScale; }

552

double getMaxScale() const { return maxScale_; }

553

554

void setScaleStep(double scaleStep) { scaleStep_ = scaleStep; }

555

double getScaleStep() const { return scaleStep_; }

556

557

void setScaleThresh(int scaleThresh) { scaleThresh_ = scaleThresh; }

558

int getScaleThresh() const { return scaleThresh_; }

559

560

void setPosThresh(int posThresh) { posThresh_ = posThresh; }

561

int getPosThresh() const { return posThresh_; }

562

563

private:

564

void processTempl();

565

void processImage();

566

567

int maxBufferSize_;

568

double xi_;

569

int levels_;

570

double angleEpsilon_;

571

572

double minAngle_;

573

double maxAngle_;

574

double angleStep_;

575

int angleThresh_;

576

577

double minScale_;

578

double maxScale_;

579

double scaleStep_;

580

int scaleThresh_;

581

582

int posThresh_;

583

584

struct ContourPoint

585

{

586

Point2d pos;

587

double theta;

588

};

589

590

struct Feature

591

{

592

ContourPoint p1;

593

ContourPoint p2;

594

595

double alpha12;

596

double d12;

597

598

Point2d r1;

599

Point2d r2;

600

};

601

602

void buildFeatureList(const Mat& edges, const Mat& dx, const Mat& dy, std::vector< std::vector<Feature> >& features, Point2d center = Point2d());

603

void getContourPoints(const Mat& edges, const Mat& dx, const Mat& dy, std::vector<ContourPoint>& points);

604

605

void calcOrientation();

606

void calcScale(double angle);

607

void calcPosition(double angle, int angleVotes, double scale, int scaleVotes);

608

609

std::vector< std::vector<Feature> > templFeatures_;

610

std::vector< std::vector<Feature> > imageFeatures_;

611

612

std::vector< std::pair<double, int> > angles_;

613

std::vector< std::pair<double, int> > scales_;

614

};

615

616

double clampAngle(double a)

617

{

618

double res = a;

619

620

while (res > 360.0)

621

res -= 360.0;

622

while (res < 0)

623

res += 360.0;

624

625

return res;

626

}

627

628

bool angleEq(double a, double b, double eps = 1.0)

629

{

630

return (fabs(clampAngle(a - b)) <= eps);

631

}

632

633

GeneralizedHoughGuilImpl::GeneralizedHoughGuilImpl()

634

{

635

maxBufferSize_ = 1000;

636

xi_ = 90.0;

637

levels_ = 360;

638

angleEpsilon_ = 1.0;

639

640

minAngle_ = 0.0;

641

maxAngle_ = 360.0;

642

angleStep_ = 1.0;

643

angleThresh_ = 15000;

644

645

minScale_ = 0.5;

646

maxScale_ = 2.0;

647

scaleStep_ = 0.05;

648

scaleThresh_ = 1000;

649

650

posThresh_ = 100;

651

}

652

653

void GeneralizedHoughGuilImpl::processTempl()

654

{

655

buildFeatureList(templEdges_, templDx_, templDy_, templFeatures_, templCenter_);

656

}

657

658

void GeneralizedHoughGuilImpl::processImage()

659

{

660

buildFeatureList(imageEdges_, imageDx_, imageDy_, imageFeatures_);

661

662

calcOrientation();

663

664

for (size_t i = 0; i < angles_.size(); ++i)

665

{

666

const double angle = angles_[i].first;

667

const int angleVotes = angles_[i].second;

668

669

calcScale(angle);

670

671

for (size_t j = 0; j < scales_.size(); ++j)

672

{

673

const double scale = scales_[j].first;

674

const int scaleVotes = scales_[j].second;

675

676

calcPosition(angle, angleVotes, scale, scaleVotes);

677

}

678

}

679

}

680

681

void GeneralizedHoughGuilImpl::buildFeatureList(const Mat& edges, const Mat& dx, const Mat& dy, std::vector< std::vector<Feature> >& features, Point2d center)

682

{

683

CV_Assert( levels_ > 0 );

684

685

const double maxDist = sqrt((double) templSize_.width * templSize_.width + templSize_.height * templSize_.height) * maxScale_;

686

687

const double alphaScale = levels_ / 360.0;

688

689

std::vector<ContourPoint> points;

690

getContourPoints(edges, dx, dy, points);

691

692

features.resize(levels_ + 1);

693

std::for_each(features.begin(), features.end(), std::mem_fun_ref(&std::vector<Feature>::clear));

694

std::for_each(features.begin(), features.end(), std::bind2nd(std::mem_fun_ref(&std::vector<Feature>::reserve), maxBufferSize_));

695

696

for (size_t i = 0; i < points.size(); ++i)

697

{

698

ContourPoint p1 = points[i];

699

700

for (size_t j = 0; j < points.size(); ++j)

701

{

702

ContourPoint p2 = points[j];

703

704

if (angleEq(p1.theta - p2.theta, xi_, angleEpsilon_))

705

{

706

const Point2d d = p1.pos - p2.pos;

707

708

Feature f;

709

710

f.p1 = p1;

711

f.p2 = p2;

712

713

f.alpha12 = clampAngle(fastAtan2((float)d.y, (float)d.x) - p1.theta);

714

f.d12 = norm(d);

715

716

if (f.d12 > maxDist)

717

continue;

718

719

f.r1 = p1.pos - center;

720

f.r2 = p2.pos - center;

721

722

const int n = cvRound(f.alpha12 * alphaScale);

723

724

if (features[n].size() < static_cast<size_t>(maxBufferSize_))

725

features[n].push_back(f);

726

}

727

}

728

}

729

}

730

731

void GeneralizedHoughGuilImpl::getContourPoints(const Mat& edges, const Mat& dx, const Mat& dy, std::vector<ContourPoint>& points)

732

{

733

CV_Assert( edges.type() == CV_8UC1 );

734

CV_Assert( dx.type() == CV_32FC1 && dx.size == edges.size );

735

CV_Assert( dy.type() == dx.type() && dy.size == edges.size );

736

737

points.clear();

738

points.reserve(edges.size().area());

739

740

for (int y = 0; y < edges.rows; ++y)

741

{

742

const uchar* edgesRow = edges.ptr(y);

743

const float* dxRow = dx.ptr<float>(y);

744

const float* dyRow = dy.ptr<float>(y);

745

746

for (int x = 0; x < edges.cols; ++x)

747

{

748

if (edgesRow[x] && (notNull(dyRow[x]) || notNull(dxRow[x])))

749

{

750

ContourPoint p;

751

752

p.pos = Point2d(x, y);

753

p.theta = fastAtan2(dyRow[x], dxRow[x]);

754

755

points.push_back(p);

756

}

757

}

758

}

759

}

760

761

void GeneralizedHoughGuilImpl::calcOrientation()

762

{

763

CV_Assert( levels_ > 0 );

764

CV_Assert( templFeatures_.size() == static_cast<size_t>(levels_ + 1) );

765

CV_Assert( imageFeatures_.size() == templFeatures_.size() );

766

CV_Assert( minAngle_ >= 0.0 && minAngle_ < maxAngle_ && maxAngle_ <= 360.0 );

767

CV_Assert( angleStep_ > 0.0 && angleStep_ < 360.0 );

768

CV_Assert( angleThresh_ > 0 );

769

770

const double iAngleStep = 1.0 / angleStep_;

771

const int angleRange = cvCeil((maxAngle_ - minAngle_) * iAngleStep);

772

773

std::vector<int> OHist(angleRange + 1, 0);

774

for (int i = 0; i <= levels_; ++i)

775

{

776

const std::vector<Feature>& templRow = templFeatures_[i];

777

const std::vector<Feature>& imageRow = imageFeatures_[i];

778

779

for (size_t j = 0; j < templRow.size(); ++j)

780

{

781

Feature templF = templRow[j];

782

783

for (size_t k = 0; k < imageRow.size(); ++k)

784

{

785

Feature imF = imageRow[k];

786

787

const double angle = clampAngle(imF.p1.theta - templF.p1.theta);

788

if (angle >= minAngle_ && angle <= maxAngle_)

789

{

790

const int n = cvRound((angle - minAngle_) * iAngleStep);

791

++OHist[n];

792

}

793

}

794

}

795

}

796

797

angles_.clear();

798

799

for (int n = 0; n < angleRange; ++n)

800

{

801

if (OHist[n] >= angleThresh_)

802

{

803

const double angle = minAngle_ + n * angleStep_;

804

angles_.push_back(std::make_pair(angle, OHist[n]));

805

}

806

}

807

}

808

809

void GeneralizedHoughGuilImpl::calcScale(double angle)

810

{

811

CV_Assert( levels_ > 0 );

812

CV_Assert( templFeatures_.size() == static_cast<size_t>(levels_ + 1) );

813

CV_Assert( imageFeatures_.size() == templFeatures_.size() );

814

CV_Assert( minScale_ > 0.0 && minScale_ < maxScale_ );

815

CV_Assert( scaleStep_ > 0.0 );

816

CV_Assert( scaleThresh_ > 0 );

817

818

const double iScaleStep = 1.0 / scaleStep_;

819

const int scaleRange = cvCeil((maxScale_ - minScale_) * iScaleStep);

820

821

std::vector<int> SHist(scaleRange + 1, 0);

822

823

for (int i = 0; i <= levels_; ++i)

824

{

825

const std::vector<Feature>& templRow = templFeatures_[i];

826

const std::vector<Feature>& imageRow = imageFeatures_[i];

827

828

for (size_t j = 0; j < templRow.size(); ++j)

829

{

830

Feature templF = templRow[j];

831

832

templF.p1.theta += angle;

833

834

for (size_t k = 0; k < imageRow.size(); ++k)

835

{

836

Feature imF = imageRow[k];

837

838

if (angleEq(imF.p1.theta, templF.p1.theta, angleEpsilon_))

839

{

840

const double scale = imF.d12 / templF.d12;

841

if (scale >= minScale_ && scale <= maxScale_)

842

{

843

const int s = cvRound((scale - minScale_) * iScaleStep);

844

++SHist[s];

845

}

846

}

847

}

848

}

849

}

850

851

scales_.clear();

852

853

for (int s = 0; s < scaleRange; ++s)

854

{

855

if (SHist[s] >= scaleThresh_)

856

{

857

const double scale = minScale_ + s * scaleStep_;

858

scales_.push_back(std::make_pair(scale, SHist[s]));

859

}

860

}

861

}

862

863

void GeneralizedHoughGuilImpl::calcPosition(double angle, int angleVotes, double scale, int scaleVotes)

864

{

865

CV_Assert( levels_ > 0 );

866

CV_Assert( templFeatures_.size() == static_cast<size_t>(levels_ + 1) );

867

CV_Assert( imageFeatures_.size() == templFeatures_.size() );

868

CV_Assert( dp_ > 0.0 );

869

CV_Assert( posThresh_ > 0 );

870

871

const double sinVal = sin(toRad(angle));

872

const double cosVal = cos(toRad(angle));

873

const double idp = 1.0 / dp_;

874

875

const int histRows = cvCeil(imageSize_.height * idp);

876

const int histCols = cvCeil(imageSize_.width * idp);

877

878

Mat DHist(histRows + 2, histCols + 2, CV_32SC1, Scalar::all(0));

879

880

for (int i = 0; i <= levels_; ++i)

881

{

882

const std::vector<Feature>& templRow = templFeatures_[i];

883

const std::vector<Feature>& imageRow = imageFeatures_[i];

884

885

for (size_t j = 0; j < templRow.size(); ++j)

886

{

887

Feature templF = templRow[j];

888

889

templF.p1.theta += angle;

890

891

templF.r1 *= scale;

892

templF.r2 *= scale;

893

894

templF.r1 = Point2d(cosVal * templF.r1.x - sinVal * templF.r1.y, sinVal * templF.r1.x + cosVal * templF.r1.y);

895

templF.r2 = Point2d(cosVal * templF.r2.x - sinVal * templF.r2.y, sinVal * templF.r2.x + cosVal * templF.r2.y);

896

897

for (size_t k = 0; k < imageRow.size(); ++k)

898

{

899

Feature imF = imageRow[k];

900

901

if (angleEq(imF.p1.theta, templF.p1.theta, angleEpsilon_))

902

{

903

Point2d c1, c2;

904

905

c1 = imF.p1.pos - templF.r1;

906

c1 *= idp;

907

908

c2 = imF.p2.pos - templF.r2;

909

c2 *= idp;

910

911

if (fabs(c1.x - c2.x) > 1 || fabs(c1.y - c2.y) > 1)

912

continue;

913

914

if (c1.y >= 0 && c1.y < histRows && c1.x >= 0 && c1.x < histCols)

915

++DHist.at<int>(cvRound(c1.y) + 1, cvRound(c1.x) + 1);

916

}

917

}

918

}

919

}

920

921

for(int y = 0; y < histRows; ++y)

922

{

923

const int* prevRow = DHist.ptr<int>(y);

924

const int* curRow = DHist.ptr<int>(y + 1);

925

const int* nextRow = DHist.ptr<int>(y + 2);

926

927

for(int x = 0; x < histCols; ++x)

928

{

929

const int votes = curRow[x + 1];

930

931

if (votes > posThresh_ && votes > curRow[x] && votes >= curRow[x + 2] && votes > prevRow[x + 1] && votes >= nextRow[x + 1])

932

{

933

posOutBuf_.push_back(Vec4f(static_cast<float>(x * dp_), static_cast<float>(y * dp_), static_cast<float>(scale), static_cast<float>(angle)));

934

voteOutBuf_.push_back(Vec3i(votes, scaleVotes, angleVotes));

935

}

936

}

937

}

938

}

939

}

940

941

Ptr<GeneralizedHoughGuil> cv::createGeneralizedHoughGuil()

942

{

943

return makePtr<GeneralizedHoughGuilImpl>();

944

}

Older »