static void make_glyph(const double *rij, const int dimi,const int dimj, double *xo,double *yo,double *zo,double *co,double *X,double *Y,double *Z) {
double x,y,z,mag;
for (int i=0; i<dimi; i++) {
for (int j=0; j<dimj; j++) {
x = X[i + dimi*j];
y = Y[i + dimi*j];
z = Z[i + dimi*j];
mag = x*x*rij(0,0) + y*y*rij(1,1) + z*z*rij(2,2) + 2*x*y*rij(0,1) + 2*x*z*rij(0,2) + 2*y*z*rij(1,2);
// mag = 1.0;
XO(i,j) = mag * x;
YO(i,j) = mag * y;
ZO(i,j) = mag * z;
CO(i,j) = mag;
}
}
}
double LCOrbits::ArmVelocity(int em, int rec, double trec)
{
LCVector ri(MT), rj(MT), vi(MT), vj(MT), rij(MT), nij(MT), n(MT);
ri = position(rec, trec);
vi = velocity(rec, trec);
rj = position(em, trec);
vj = velocity(em, trec);
rij = rj-ri;
nij = rij.unit();
n = nij * (-1.);
return(vj*n-vi*n);
}
double LCOrbits::ArmCompute(int em, int rec, double trec)
{
double tA(0.), tij;
if ((em<1)||(em>NARMMAX)||((rec<1)&&(rec>NARMMAX))){
std::cerr << "ERROR in LCOrbits::ArmCompute : Spacecraft number must be in [0," << NARMMAX << "] ! " << Endl;
throw std::invalid_argument ("ERROR in LCOrbits::ArmCompute : Spacecraft number must be in [0,NARMMAX] ! ");
}
/*
if(OrderArm != -10)
OrderArm = order_default;
if ((OrderArm<-2)||(OrderArm>2))
throw std::invalid_argument ("ERROR in LCOrbits::ArmCompute : Order must be 0 (for 0), 1 (for 1/2), 2 (for 1) or -1 (for analytical MLDC eccentric formulation)");
*/
if(OrderArm >= 0){
LCVector ri(MT), rj(MT), vi(MT), vj(MT), rij(MT), nij(MT), n(MT);
/*! **** Read positions and velocities and compute related quantities
* Index i refers to receiver and j to emitter
* \f{eqnarray*}{
* \vec{r}_{ij} & = & \vec{r}_j - \vec{r}_i = \vec{r}_{em} - \vec{r}_{rec} \\
* \vec{n}_{ij} & = & \vec{r}_{ij} / | \vec{r}_{ij} | \\
* \hat{n} & = & - \vec{n}_{ij} \\
* t_{ij} & = & - | \vec{r}_{ij} | / c = - | \vec{r}_{em} - \vec{r}_{rec} | / c
* \f}
*/
ri = position(rec, trec);
vi = velocity(rec, trec);
vi = vi/LC::c_SI;
rj = position(em, trec);
vj = velocity(em, trec);
vj = vj/LC::c_SI;
/*
ri.p[0] = 5.e9;
ri.p[1] = 0.;
ri.p[2] = 0.;
rj.p[0] = -1.e-5;
rj.p[1] = 0.;
rj.p[2] = 0.;
rij = rj-ri;
*/
rij = rj-ri;
nij = rij.unit();
n = nij*(-1.);
tij = rij.norm()/LC::c_SI;
tij = -tij;
/*
MT->o->precision(16);
Cout << "em = " << em << " -> rec = " << rec << " : " << rij.norm() << " ==> " << tij << Endl;
for(int i=0; i<3; i++)
Cout << "\t\t" << ri.p[i] << "\t\t" << rj.p[i] << "\t\t" << rij.p[i] << Endl;
*/
if (OrderArm>=0)
tA += ArmOrderContrib(0, tij, ri, rj, vi, vj, rij, nij, n);
if (OrderArm>=1)
tA += ArmOrderContrib(1, tij, ri, rj, vi, vj, rij, nij, n);
if (OrderArm>=2)
tA += ArmOrderContrib(2, tij, ri, rj, vi, vj, rij, nij, n);
return (tA);
}else{
return( ArmSpecific(em, rec, trec) );
}
}
bool mrpt::vision::pnp::rpnp::compute_pose(
Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_)
{
// selecting an edge $P_{ i1 }P_{ i2 }$ by n random sampling
int i1 = 0, i2 = 1;
double lmin =
Q(0, i1) * Q(0, i2) + Q(1, i1) * Q(1, i2) + Q(2, i1) * Q(2, i2);
Eigen::MatrixXi rij(n, 2);
R_ = Eigen::MatrixXd::Identity(3, 3);
t_ = Eigen::Vector3d::Zero();
for (int i = 0; i < n; i++)
for (int j = 0; j < 2; j++) rij(i, j) = rand() % n;
for (int ii = 0; ii < n; ii++)
{
int i = rij(ii, 0), j = rij(ii, 1);
if (i == j) continue;
double l = Q(0, i) * Q(0, j) + Q(1, i) * Q(1, j) + Q(2, i) * Q(2, j);
if (l < lmin)
{
i1 = i;
i2 = j;
lmin = l;
}
}
// calculating the rotation matrix of $O_aX_aY_aZ_a$.
Eigen::Vector3d p1, p2, p0, x, y, z, dum_vec;
p1 = P.col(i1);
p2 = P.col(i2);
p0 = (p1 + p2) / 2;
x = p2 - p0;
x /= x.norm();
if (std::abs(x(1)) < std::abs(x(2)))
{
dum_vec << 0, 1, 0;
z = x.cross(dum_vec);
z /= z.norm();
y = z.cross(x);
y /= y.norm();
}
else
{
dum_vec << 0, 0, 1;
y = dum_vec.cross(x);
y /= y.norm();
z = x.cross(y);
x /= x.norm();
}
Eigen::Matrix3d R0;
R0.col(0) = x;
R0.col(1) = y;
R0.col(2) = z;
for (int i = 0; i < n; i++) P.col(i) = R0.transpose() * (P.col(i) - p0);
// Dividing the n - point set into(n - 2) 3 - point subsets
// and setting up the P3P equations
Eigen::Vector3d v1 = Q.col(i1), v2 = Q.col(i2);
double cg1 = v1.dot(v2);
double sg1 = sqrt(1 - cg1 * cg1);
double D1 = (P.col(i1) - P.col(i2)).norm();
Eigen::MatrixXd D4(n - 2, 5);
int j = 0;
Eigen::Vector3d vi;
Eigen::VectorXd rowvec(5);
for (int i = 0; i < n; i++)
{
if (i == i1 || i == i2) continue;
vi = Q.col(i);
double cg2 = v1.dot(vi);
double cg3 = v2.dot(vi);
double sg2 = sqrt(1 - cg2 * cg2);
double D2 = (P.col(i1) - P.col(i)).norm();
double D3 = (P.col(i) - P.col(i2)).norm();
// get the coefficients of the P3P equation from each subset.
rowvec = getp3p(cg1, cg2, cg3, sg1, sg2, D1, D2, D3);
D4.row(j) = rowvec;
j += 1;
if (j > n - 3) break;
}
Eigen::VectorXd D7(8), dumvec(8), dumvec1(5);
D7.setZero();
for (int i = 0; i < n - 2; i++)
{
dumvec1 = D4.row(i);
dumvec = getpoly7(dumvec1);
D7 += dumvec;
}
Eigen::PolynomialSolver<double, 7> psolve(D7.reverse());
Eigen::VectorXcd comp_roots = psolve.roots().transpose();
Eigen::VectorXd real_comp, imag_comp;
real_comp = comp_roots.real();
imag_comp = comp_roots.imag();
Eigen::VectorXd::Index max_index;
double max_real = real_comp.cwiseAbs().maxCoeff(&max_index);
std::vector<double> act_roots_;
int cnt = 0;
for (int i = 0; i < imag_comp.size(); i++)
{
if (std::abs(imag_comp(i)) / max_real < 0.001)
{
act_roots_.push_back(real_comp(i));
cnt++;
}
}
double* ptr = &act_roots_[0];
Eigen::Map<Eigen::VectorXd> act_roots(ptr, cnt);
if (cnt == 0)
{
return false;
}
Eigen::VectorXd act_roots1(cnt);
act_roots1 << act_roots.segment(0, cnt);
std::vector<Eigen::Matrix3d> R_cum(cnt);
std::vector<Eigen::Vector3d> t_cum(cnt);
std::vector<double> err_cum(cnt);
for (int i = 0; i < cnt; i++)
{
double root = act_roots(i);
// Compute the rotation matrix
double d2 = cg1 + root;
Eigen::Vector3d unitx, unity, unitz;
unitx << 1, 0, 0;
unity << 0, 1, 0;
unitz << 0, 0, 1;
x = v2 * d2 - v1;
x /= x.norm();
if (std::abs(unity.dot(x)) < std::abs(unitz.dot(x)))
{
z = x.cross(unity);
z /= z.norm();
y = z.cross(x);
y / y.norm();
}
else
{
y = unitz.cross(x);
y /= y.norm();
z = x.cross(y);
z /= z.norm();
}
R.col(0) = x;
R.col(1) = y;
R.col(2) = z;
// calculating c, s, tx, ty, tz
Eigen::MatrixXd D(2 * n, 6);
D.setZero();
R0 = R.transpose();
Eigen::VectorXd r(
Eigen::Map<Eigen::VectorXd>(R0.data(), R0.cols() * R0.rows()));
for (int j = 0; j < n; j++)
{
double ui = img_pts(j, 0), vi = img_pts(j, 1), xi = P(0, j),
yi = P(1, j), zi = P(2, j);
D.row(2 * j) << -r(1) * yi + ui * (r(7) * yi + r(8) * zi) -
r(2) * zi,
-r(2) * yi + ui * (r(8) * yi - r(7) * zi) + r(1) * zi, -1, 0,
ui, ui * r(6) * xi - r(0) * xi;
D.row(2 * j + 1)
<< -r(4) * yi + vi * (r(7) * yi + r(8) * zi) - r(5) * zi,
-r(5) * yi + vi * (r(8) * yi - r(7) * zi) + r(4) * zi, 0, -1,
vi, vi * r(6) * xi - r(3) * xi;
}
Eigen::MatrixXd DTD = D.transpose() * D;
Eigen::EigenSolver<Eigen::MatrixXd> es(DTD);
Eigen::VectorXd Diag = es.pseudoEigenvalueMatrix().diagonal();
Eigen::MatrixXd V_mat = es.pseudoEigenvectors();
Eigen::MatrixXd::Index min_index;
Diag.minCoeff(&min_index);
Eigen::VectorXd V = V_mat.col(min_index);
V /= V(5);
double c = V(0), s = V(1);
t << V(2), V(3), V(4);
// calculating the camera pose by 3d alignment
Eigen::VectorXd xi, yi, zi;
xi = P.row(0);
yi = P.row(1);
zi = P.row(2);
Eigen::MatrixXd XXcs(3, n), XXc(3, n);
XXc.setZero();
XXcs.row(0) = r(0) * xi + (r(1) * c + r(2) * s) * yi +
(-r(1) * s + r(2) * c) * zi +
t(0) * Eigen::VectorXd::Ones(n);
XXcs.row(1) = r(3) * xi + (r(4) * c + r(5) * s) * yi +
(-r(4) * s + r(5) * c) * zi +
t(1) * Eigen::VectorXd::Ones(n);
XXcs.row(2) = r(6) * xi + (r(7) * c + r(8) * s) * yi +
(-r(7) * s + r(8) * c) * zi +
t(2) * Eigen::VectorXd::Ones(n);
for (int ii = 0; ii < n; ii++)
XXc.col(ii) = Q.col(ii) * XXcs.col(ii).norm();
Eigen::Matrix3d R2;
Eigen::Vector3d t2;
Eigen::MatrixXd XXw = obj_pts.transpose();
calcampose(XXc, XXw, R2, t2);
R_cum[i] = R2;
t_cum[i] = t2;
for (int k = 0; k < n; k++) XXc.col(k) = R2 * XXw.col(k) + t2;
Eigen::MatrixXd xxc(2, n);
xxc.row(0) = XXc.row(0).array() / XXc.row(2).array();
xxc.row(1) = XXc.row(1).array() / XXc.row(2).array();
double res = ((xxc.row(0) - img_pts.col(0).transpose()).norm() +
(xxc.row(1) - img_pts.col(1).transpose()).norm()) /
2;
err_cum[i] = res;
}
int pos_cum =
std::min_element(err_cum.begin(), err_cum.end()) - err_cum.begin();
R_ = R_cum[pos_cum];
t_ = t_cum[pos_cum];
return true;
}
