Example #1
0
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;
    }
  }
}
Example #2
0
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);	
}
Example #3
0
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) );
	}
}
Example #4
0
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;
}