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

*P.ptr<double>(0,1) = -2 * m[2][4] * m[1][1] + 2 * t11 + 2 * t12 - 2 * m[1][4] * m[2][1] - 2 * m[2][5] * m[1][2] + 2 * t15 + 2 * t16 - 2 * m[1][5] * m[2][2];

489

*P.ptr<double>(0,2) = t19 - 2 * m[2][4] * m[2][1] + t22 + t23 + t24 - 2 * m[2][5] * m[2][2];

490

*P.ptr<double>(0,3) = t28 + t29 - 2 * m[1][6] * m[1][3];

491

*P.ptr<double>(0,4) = -2 * m[2][6] * m[1][3] + 2 * t34 - 2 * m[1][6] * m[2][3] + 2 * t36;

492

*P.ptr<double>(0,5) = -2 * m[2][6] * m[2][3] + t40 + t41;

493

494

*P.ptr<double>(1,0) = t8 - 2 * m[1][8] * m[1][2] - 2 * m[1][7] * m[1][1] + t47 + t48 + t2;

495

*P.ptr<double>(1,1) = 2 * t15 - 2 * m[1][8] * m[2][2] - 2 * m[2][8] * m[1][2] + 2 * t52 - 2 * m[1][7] * m[2][1] - 2 * m[2][7] * m[1][1] + 2 * t55 + 2 * t11;

496

*P.ptr<double>(1,2) = -2 * m[2][8] * m[2][2] + t22 + t23 + t59 - 2 * m[2][7] * m[2][1] + t62;

497

*P.ptr<double>(1,3) = t29 + t64 - 2 * m[1][9] * m[1][3];

498

*P.ptr<double>(1,4) = 2 * t34 + 2 * t68 - 2 * m[2][9] * m[1][3] - 2 * m[1][9] * m[2][3];

499

*P.ptr<double>(1,5) = -2 * m[2][9] * m[2][3] + t74 + t41;

500

501

*P.ptr<double>(2,0) = -2 * m[1][11] * m[1][2] + t2 + t8 + t78 + t79 - 2 * m[1][10] * m[1][1];

502

*P.ptr<double>(2,1) = 2 * t15 - 2 * m[1][11] * m[2][2] + 2 * t84 - 2 * m[1][10] * m[2][1] - 2 * m[2][10] * m[1][1] + 2 * t87 - 2 * m[2][11] * m[1][2]+ 2 * t11;

503

*P.ptr<double>(2,2) = t90 + t22 - 2 * m[2][10] * m[2][1] + t23 - 2 * m[2][11] * m[2][2] + t95;

504

*P.ptr<double>(2,3) = -2 * m[1][12] * m[1][3] + t99 + t29;

505

*P.ptr<double>(2,4) = 2 * t34 + 2 * t101 - 2 * m[2][12] * m[1][3] - 2 * m[1][12] * m[2][3];

506

*P.ptr<double>(2,5) = t41 + t105 - 2 * m[2][12] * m[2][3];

507

508

*P.ptr<double>(3,0) = t48 + t1 - 2 * m[1][8] * m[1][5] + t7 - 2 * m[1][7] * m[1][4] + t47;

509

*P.ptr<double>(3,1) = 2 * t16 - 2 * m[1][7] * m[2][4] + 2 * t55 + 2 * t52 - 2 * m[1][8] * m[2][5] - 2 * m[2][8] * m[1][5] - 2 * m[2][7] * m[1][4] + 2 * t12;

510

*P.ptr<double>(3,2) = t24 - 2 * m[2][8] * m[2][5] + t19 - 2 * m[2][7] * m[2][4] + t62 + t59;

511

*P.ptr<double>(3,3) = -2 * m[1][9] * m[1][6] + t64 + t28;

512

*P.ptr<double>(3,4) = 2 * t68 + 2 * t36 - 2 * m[2][9] * m[1][6] - 2 * m[1][9] * m[2][6];

513

*P.ptr<double>(3,5) = t40 + t74 - 2 * m[2][9] * m[2][6];

514

515

*P.ptr<double>(4,0) = t1 - 2 * m[1][10] * m[1][4] + t7 + t78 + t79 - 2 * m[1][11] * m[1][5];

516

*P.ptr<double>(4,1) = 2 * t84 - 2 * m[1][11] * m[2][5] - 2 * m[1][10] * m[2][4] + 2 * t16 - 2 * m[2][11] * m[1][5] + 2 * t87 - 2 * m[2][10] * m[1][4] + 2 * t12;

517

*P.ptr<double>(4,2) = t19 + t24 - 2 * m[2][10] * m[2][4] - 2 * m[2][11] * m[2][5] + t95 + t90;

518

*P.ptr<double>(4,3) = t28 - 2 * m[1][12] * m[1][6] + t99;

519

*P.ptr<double>(4,4) = 2 * t101 + 2 * t36 - 2 * m[2][12] * m[1][6] - 2 * m[1][12] * m[2][6];

520

*P.ptr<double>(4,5) = t105 - 2 * m[2][12] * m[2][6] + t40;

521

522

*P.ptr<double>(5,0) = -2 * m[1][10] * m[1][7] + t47 + t48 + t78 + t79 - 2 * m[1][11] * m[1][8];

523

*P.ptr<double>(5,1) = 2 * t84 + 2 * t87 - 2 * m[2][11] * m[1][8] - 2 * m[1][10] * m[2][7] - 2 * m[2][10] * m[1][7] + 2 * t55 + 2 * t52 - 2 * m[1][11] * m[2][8];

524

*P.ptr<double>(5,2) = -2 * m[2][10] * m[2][7] - 2 * m[2][11] * m[2][8] + t62 + t59 + t90 + t95;

525

*P.ptr<double>(5,3) = t64 - 2 * m[1][12] * m[1][9] + t99;

526

*P.ptr<double>(5,4) = 2 * t68 - 2 * m[2][12] * m[1][9] - 2 * m[1][12] * m[2][9] + 2 * t101;

527

*P.ptr<double>(5,5) = t105 - 2 * m[2][12] * m[2][9] + t74;

528

529

return P;

530

}

531

532

void upnp::generate_all_possible_solutions_for_f_unk(const double betas[5], double solutions[18][3])

533

{

534

int matrix_to_resolve[18][9] = {

535

{ 2, 0, 0, 1, 1, 0, 2, 0, 2 }, { 2, 0, 0, 1, 1, 0, 1, 1, 2 },

536

{ 2, 0, 0, 1, 1, 0, 0, 2, 2 }, { 2, 0, 0, 0, 2, 0, 2, 0, 2 },

537

{ 2, 0, 0, 0, 2, 0, 1, 1, 2 }, { 2, 0, 0, 0, 2, 0, 0, 2, 2 },

538

{ 2, 0, 0, 2, 0, 2, 1, 1, 2 }, { 2, 0, 0, 2, 0, 2, 0, 2, 2 },

539

{ 2, 0, 0, 1, 1, 2, 0, 2, 2 }, { 1, 1, 0, 0, 2, 0, 2, 0, 2 },

540

{ 1, 1, 0, 0, 2, 0, 1, 1, 2 }, { 1, 1, 0, 2, 0, 2, 0, 2, 2 },

541

{ 1, 1, 0, 2, 0, 2, 1, 1, 2 }, { 1, 1, 0, 2, 0, 2, 0, 2, 2 },

542

{ 1, 1, 0, 1, 1, 2, 0, 2, 2 }, { 0, 2, 0, 2, 0, 2, 1, 1, 2 },

543

{ 0, 2, 0, 2, 0, 2, 0, 2, 2 }, { 0, 2, 0, 1, 1, 2, 0, 2, 2 }

544

};

545

546

int combination[18][3] = {

547

{ 1, 2, 4 }, { 1, 2, 5 }, { 1, 2, 6 }, { 1, 3, 4 },

548

{ 1, 3, 5 }, { 1, 3, 6 }, { 1, 4, 5 }, { 1, 4, 6 },

549

{ 1, 5, 6 }, { 2, 3, 4 }, { 2, 3, 5 }, { 2, 3, 6 },

550

{ 2, 4, 5 }, { 2, 4, 6 }, { 2, 5, 6 }, { 3, 4, 5 },

551

{ 3, 4, 6 }, { 3, 5, 6 }

552

};

553

554

for (int i = 0; i < 18; ++i) {

555

double matrix[9], independent_term[3];

556

Mat M = Mat(3, 3, CV_64F, matrix);

557

Mat I = Mat(3, 1, CV_64F, independent_term);

558

Mat S = Mat(1, 3, CV_64F);

559

560

for (int j = 0; j < 9; ++j) matrix[j] = (double)matrix_to_resolve[i][j];

561

562

independent_term[0] = log( abs( betas[ combination[i][0]-1 ] ) );

563

independent_term[1] = log( abs( betas[ combination[i][1]-1 ] ) );

564

independent_term[2] = log( abs( betas[ combination[i][2]-1 ] ) );

565

566

exp( Mat(M.inv() * I), S);

567

568

solutions[i][0] = S.at<double>(0);

569

solutions[i][1] = S.at<double>(1) * sign( betas[1] );

570

solutions[i][2] = abs( S.at<double>(2) );

571

}

572

}

573

574

void upnp::gauss_newton(const Mat * L_6x12, const Mat * Rho, double betas[4], double * f)

575

{

576

const int iterations_number = 50;

577

578

double a[6*4], b[6], x[4];

579

Mat * A = new Mat(6, 4, CV_64F, a);

580

Mat * B = new Mat(6, 1, CV_64F, b);

581

Mat * X = new Mat(4, 1, CV_64F, x);

582

583

for(int k = 0; k < iterations_number; k++)

584

{

585

compute_A_and_b_gauss_newton(L_6x12->ptr<double>(0), Rho->ptr<double>(0), betas, A, B, f[0]);

586

qr_solve(A, B, X);

587

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

588

betas[i] += x[i];

589

f[0] += x[3];

590

}

591

592

if (f[0] < 0) f[0] = -f[0];

593

fu = fv = f[0];

594

595

A->release();

596

delete A;

597

598

B->release();

599

delete B;

600

601

X->release();

602

delete X;

603

604

}

605

606

void upnp::compute_A_and_b_gauss_newton(const double * l_6x12, const double * rho,

607

const double betas[4], Mat * A, Mat * b, double const f)

608

{

609

610

for(int i = 0; i < 6; i++) {

611

const double * rowL = l_6x12 + i * 12;

612

double * rowA = A->ptr<double>(i);

613

614

rowA[0] = 2 * rowL[0] * betas[0] + rowL[1] * betas[1] + rowL[2] * betas[2] + f*f * ( 2 * rowL[6]*betas[0] + rowL[7]*betas[1] + rowL[8]*betas[2] );

615

rowA[1] = rowL[1] * betas[0] + 2 * rowL[3] * betas[1] + rowL[4] * betas[2] + f*f * ( rowL[7]*betas[0] + 2 * rowL[9]*betas[1] + rowL[10]*betas[2] );

616

rowA[2] = rowL[2] * betas[0] + rowL[4] * betas[1] + 2 * rowL[5] * betas[2] + f*f * ( rowL[8]*betas[0] + rowL[10]*betas[1] + 2 * rowL[11]*betas[2] );

617

rowA[3] = 2*f * ( rowL[6]*betas[0]*betas[0] + rowL[7]*betas[0]*betas[1] + rowL[8]*betas[0]*betas[2] + rowL[9]*betas[1]*betas[1] + rowL[10]*betas[1]*betas[2] + rowL[11]*betas[2]*betas[2] ) ;

618

619

*b->ptr<double>(i) = rho[i] -

620

(

621

rowL[0] * betas[0] * betas[0] +

622

rowL[1] * betas[0] * betas[1] +

623

rowL[2] * betas[0] * betas[2] +

624

rowL[3] * betas[1] * betas[1] +

625

rowL[4] * betas[1] * betas[2] +

626

rowL[5] * betas[2] * betas[2] +

627

f*f * rowL[6] * betas[0] * betas[0] +

628

f*f * rowL[7] * betas[0] * betas[1] +

629

f*f * rowL[8] * betas[0] * betas[2] +

630

f*f * rowL[9] * betas[1] * betas[1] +

631

f*f * rowL[10] * betas[1] * betas[2] +

632

f*f * rowL[11] * betas[2] * betas[2]

633

);

634

}

635

}

636

637

void upnp::compute_L_6x12(const double * ut, double * l_6x12)

638

{

639

const double * v[3];

640

641

v[0] = ut + 12 * 9;

642

v[1] = ut + 12 * 10;

643

v[2] = ut + 12 * 11;

644

645

double dv[3][6][3];

646

647

for(int i = 0; i < 3; i++) {

648

int a = 0, b = 1;

649

for(int j = 0; j < 6; j++) {

650

dv[i][j][0] = v[i][3 * a ] - v[i][3 * b];

651

dv[i][j][1] = v[i][3 * a + 1] - v[i][3 * b + 1];

652

dv[i][j][2] = v[i][3 * a + 2] - v[i][3 * b + 2];

653

654

b++;

655

if (b > 3) {

656

a++;

657

b = a + 1;

658

}

659

}

660

}

661

662

for(int i = 0; i < 6; i++) {

663

double * row = l_6x12 + 12 * i;

664

665

row[0] = dotXY(dv[0][i], dv[0][i]);

666

row[1] = 2.0f * dotXY(dv[0][i], dv[1][i]);

667

row[2] = dotXY(dv[1][i], dv[1][i]);

668

row[3] = 2.0f * dotXY(dv[0][i], dv[2][i]);

669

row[4] = 2.0f * dotXY(dv[1][i], dv[2][i]);

670

row[5] = dotXY(dv[2][i], dv[2][i]);

671

672

row[6] = dotZ(dv[0][i], dv[0][i]);

673

row[7] = 2.0f * dotZ(dv[0][i], dv[1][i]);

674

row[8] = 2.0f * dotZ(dv[0][i], dv[2][i]);

675

row[9] = dotZ(dv[1][i], dv[1][i]);

676

row[10] = 2.0f * dotZ(dv[1][i], dv[2][i]);

677

row[11] = dotZ(dv[2][i], dv[2][i]);

678

}

679

}

680

681

void upnp::compute_rho(double * rho)

682

{

683

rho[0] = dist2(cws[0], cws[1]);

684

rho[1] = dist2(cws[0], cws[2]);

685

rho[2] = dist2(cws[0], cws[3]);

686

rho[3] = dist2(cws[1], cws[2]);

687

rho[4] = dist2(cws[1], cws[3]);

688

rho[5] = dist2(cws[2], cws[3]);

689

}

690

691

double upnp::dist2(const double * p1, const double * p2)

692

{

693

return

694

(p1[0] - p2[0]) * (p1[0] - p2[0]) +

695

(p1[1] - p2[1]) * (p1[1] - p2[1]) +

696

(p1[2] - p2[2]) * (p1[2] - p2[2]);

697

}

698

699

double upnp::dot(const double * v1, const double * v2)

700

{

701

return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];

702

}

703

704

double upnp::dotXY(const double * v1, const double * v2)

705

{

706

return v1[0] * v2[0] + v1[1] * v2[1];

707

}

708

709

double upnp::dotZ(const double * v1, const double * v2)

710

{

711

return v1[2] * v2[2];

712

}

713

714

double upnp::sign(const double v)

715

{

716

return ( v < 0.0 ) ? -1.0 : ( v > 0.0 ) ? 1.0 : 0.0;

717

}

718

719

void upnp::qr_solve(Mat * A, Mat * b, Mat * X)

720

{

721

const int nr = A->rows;

722

const int nc = A->cols;

723

724

if (max_nr != 0 && max_nr < nr)

725

{

726

delete [] A1;

727

delete [] A2;

728

}

729

if (max_nr < nr)

730

{

731

max_nr = nr;

732

A1 = new double[nr];

733

A2 = new double[nr];

734

}

735

736

double * pA = A->ptr<double>(0), * ppAkk = pA;

737

for(int k = 0; k < nc; k++)

738

{

739

double * ppAik1 = ppAkk, eta = fabs(*ppAik1);

740

for(int i = k + 1; i < nr; i++)

741

{

742

double elt = fabs(*ppAik1);

743

if (eta < elt) eta = elt;

744

ppAik1 += nc;

745

}

746

if (eta == 0)

747

{

748

A1[k] = A2[k] = 0.0;

749

//cerr << "God damnit, A is singular, this shouldn't happen." << endl;

750

return;

751

}

752

else

753

{

754

double * ppAik2 = ppAkk, sum2 = 0.0, inv_eta = 1. / eta;

755

for(int i = k; i < nr; i++)

756

{

757

*ppAik2 *= inv_eta;

758

sum2 += *ppAik2 * *ppAik2;

759

ppAik2 += nc;

760

}

761

double sigma = sqrt(sum2);

762

if (*ppAkk < 0)

763

sigma = -sigma;

764

*ppAkk += sigma;

765

A1[k] = sigma * *ppAkk;

766

A2[k] = -eta * sigma;

767

for(int j = k + 1; j < nc; j++)

768

{

769

double * ppAik = ppAkk, sum = 0;

770

for(int i = k; i < nr; i++)

771

{

772

sum += *ppAik * ppAik[j - k];

773

ppAik += nc;

774

}

775

double tau = sum / A1[k];

776

ppAik = ppAkk;

777

for(int i = k; i < nr; i++)

778

{

779

ppAik[j - k] -= tau * *ppAik;

780

ppAik += nc;

781

}

782

}

783

}

784

ppAkk += nc + 1;

785

}

786

787

// b <- Qt b

788

double * ppAjj = pA, * pb = b->ptr<double>(0);

789

for(int j = 0; j < nc; j++)

790

{

791

double * ppAij = ppAjj, tau = 0;

792

for(int i = j; i < nr; i++)

793

{

794

tau += *ppAij * pb[i];

795

ppAij += nc;

796

}

797

tau /= A1[j];

798

ppAij = ppAjj;

799

for(int i = j; i < nr; i++)

800

{

801

pb[i] -= tau * *ppAij;

802

ppAij += nc;

803

}

804

ppAjj += nc + 1;

805

}

806

807

// X = R-1 b

808

double * pX = X->ptr<double>(0);

809

pX[nc - 1] = pb[nc - 1] / A2[nc - 1];

810

for(int i = nc - 2; i >= 0; i--)

811

{

812

double * ppAij = pA + i * nc + (i + 1), sum = 0;

813

814

for(int j = i + 1; j < nc; j++)

815

{

816

sum += *ppAij * pX[j];

817

ppAij++;

818

}

819

pX[i] = (pb[i] - sum) / A2[i];

820

}

821

}

Older »