示例#1
0
        void operator () (const Point &a,
                          const Point &b,
                          const Point &c) {
            Vector ex(1, 0, 0),
                   ey(0, 1, 0),
                   ez(0, 0, 1);

            Vector va = CGAL::ORIGIN - a;
            Vector vb = CGAL::ORIGIN - b;
            Vector vc = CGAL::ORIGIN - c;

            // Updating the volume...
            Vector nhat = CGAL::cross_product(b - a, c - a);
            vol += nhat * va;

            // ... and the centroid
            // X
            cx += (nhat * ex) * (
                CGAL::square(ex * (va + vb)) +
                CGAL::square(ex * (vb + vc)) +
                CGAL::square(ex * (vc + va)) );

            // Y
            cy += (nhat * ey) * (
                CGAL::square(ey * (va + vb)) +
                CGAL::square(ey * (vb + vc)) +
                CGAL::square(ey * (vc + va)) );

            // Z
            cz += (nhat * ez) * (
                CGAL::square(ez * (va + vb)) +
                CGAL::square(ez * (vb + vc)) +
                CGAL::square(ez * (vc + va)) );
        }
static std::vector<Point_3> mapPointsFromOXYplane(std::vector<Point_2> points,
		Vector_3 nu)
{
	DEBUG_START;

	ASSERT(!!Vector3d(nu.x(), nu.y(), nu.z()) && "nu is null vector");

	Vector_3 ez(0., 0, 1.);
	double length = sqrt(nu.squared_length());
	ASSERT(std::fpclassify(length) != FP_ZERO);
	nu = nu * 1. / length; /* Normalize std::vector \nu. */
	ASSERT(std::isfinite(nu.x()));
	ASSERT(std::isfinite(nu.y()));
	ASSERT(std::isfinite(nu.z()));
	Vector_3 tau = cross_product(nu, ez);

	std::vector<Point_3> pointsMapped;
	CGAL::Origin o;
	for (auto &point : points)
	{
		pointsMapped.push_back(o + tau * point.x() + ez * point.y());
	}
	DEBUG_END;
	return pointsMapped;
}
示例#3
0
文件: dbg.c 项目: PlanetAPL/a-plus
Z void callafunc(A func,A cbdata,A arg0,I n,A arg1,A arg2,A arg3) 
{
  E e;
  e = (E)(ma(7)); 
  e->n=5; e->f=(I)func; e->a[0]=(I)cbdata;
  e->a[1]=(I)arg0;
  e->a[2]=(I)((1<=n)?arg1:aplus_nl);
  e->a[3]=(I)((2<=n)?arg2:aplus_nl);
  e->a[4]=(I)((3<=n)?arg3:aplus_nl);
  dc((A)ez(ME(e))); mf((I *)e); 
}
示例#4
0
文件: radg3d.cpp 项目: aprodi/mcradia
void radTg3d::CheckAxesExchangeForSubdInLabFrame(double* SubdivArray, char& ConversionToPolyhedronsIsNeeded)
{
	ConversionToPolyhedronsIsNeeded = 0;

	radTrans ResTransf;
	short SomethingFound = 0;
	FindResTransfWithMultOne(ResTransf, SomethingFound);
	if(SomethingFound) 
	{
		const double ZeroTol = 1.E-13;
		TVector3d ex(1.,0.,0.), ey(0.,1.,0.), ez(0.,0.,1.);
		TVector3d ex1 = ResTransf.TrBiPoint_inv(ex);
		TVector3d ey1 = ResTransf.TrBiPoint_inv(ey);
		TVector3d ez1 = ResTransf.TrBiPoint_inv(ez);

		if(PracticallyEqualOrAnti(ex, ex1, ZeroTol))
		{
			if(!(PracticallyEqualOrAnti(ey, ey1, ZeroTol) || PracticallyEqualOrAnti(ey, ez1, ZeroTol)))
				ConversionToPolyhedronsIsNeeded = 1;
		}
		else if(PracticallyEqualOrAnti(ex, ey1, ZeroTol))
		{
			if(!(PracticallyEqualOrAnti(ey, ex1, ZeroTol) || PracticallyEqualOrAnti(ey, ez1, ZeroTol)))
				ConversionToPolyhedronsIsNeeded = 1;
		}
		else if(PracticallyEqualOrAnti(ex, ez1, ZeroTol))
		{
			if(!(PracticallyEqualOrAnti(ey, ex1, ZeroTol) || PracticallyEqualOrAnti(ey, ey1, ZeroTol)))
				ConversionToPolyhedronsIsNeeded = 1;
		}
		else ConversionToPolyhedronsIsNeeded = 1;

		if(!ConversionToPolyhedronsIsNeeded)
		{
			double &kx = SubdivArray[0], &ky = SubdivArray[2],  &kz = SubdivArray[4];
			double &qx = SubdivArray[1], &qy = SubdivArray[3],  &qz = SubdivArray[5];

			TVector3d kx_ex1 = kx*ex1, ky_ey1 = ky*ey1, kz_ez1 = kz*ez1;
			TVector3d k1Tot = kx_ex1 + ky_ey1 + kz_ez1;
			TVector3d qx_ex1 = qx*ex1, qy_ey1 = qy*ey1, qz_ez1 = qz*ez1;
			TVector3d q1Tot = qx_ex1 + qy_ey1 + qz_ez1;

			kx = ex*k1Tot; qx = ex*q1Tot;
			if(kx < 0.) { kx = -kx; qx = -1./qx;}
			ky = ey*k1Tot; qy = ey*q1Tot;
			if(ky < 0.) { ky = -ky; qy = -1./qy;}
			kz = ez*k1Tot; qz = ez*q1Tot;
			if(kz < 0.) { kz = -kz; qz = -1./qz;}
		}
	}
}
示例#5
0
Vector3D Matrix3D::solve3(Vector3D b)
{
	Vector3D ex(m[0][0], m[1][0], m[2][0]);
	Vector3D ey(m[0][1], m[1][1], m[2][1]);
	Vector3D ez(m[0][2], m[1][2], m[2][2]);
	double det = dot(ex, cross(ey, ez));
	if (det != 0)
		det = 1 / det;
	Vector3D x;
	x.x = det * dot(b, cross(ey, ez));
	x.y = det * dot(ex, cross(b, ez));
	x.z = det * dot(ex, cross(ey, b));
	return x;
}
/*!
  Compute the error \f$ (s-s^*)\f$ between the current and the desired
  visual features from a subset of the possible features.

  \param s_star : Desired 3D point visual feature.

  \param select : The error can be computed for a selection of a
  subset of the possible 3D point coordinate features.
  - To compute the error for all the three coordinates use
    vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3 
    dimension column vector.
  - To compute the error for only one of the coordinate
    feature \f$(X,Y, or Z)\f$ use one of the
    corresponding function selectX(), selectY() or selectZ(). In
    that case the error vector is a 1 dimension column vector.

  \return The error \f$ (s-s^*)\f$ between the current and the desired
  visual feature.

  The code below shows how to use this method to manipulate the \f$
  Z \f$ subset:

  \code
  // Creation of the current feature s
  vpFeaturePoint3D s;
  s.set_Z(0.8); // Initialization of the current Z feature

  // Creation of the desired feature s*. 
  vpFeatureTranslation s_star; 
  s_star.set_Z(1); // Initialization of the current Z* feature to Z*=1 meter 

  // Compute the interaction matrix for the Z coordinate feature
  vpMatrix L_Z = s.interaction( vpFeaturePoint3D::selectZ() );

  // Compute the error vector (s-s*) for the Z feature
  s.error(s_star, vpFeaturePoint3D::selectZ());
  \endcode

  To manipulate the subset features \f$s=(Y, Z)\f$,
  the code becomes:
  \code
  // Compute the interaction matrix for the Y, Z feature coordinates
  vpMatrix L_YZ = s.interaction( vpFeaturePoint3D::selectY() | vpFeaturePoint3D::selectZ() );

  // Compute the error vector e = (s-s*) for the Y, Z feature coordinates
  vpColVector e = s.error(s_star, vpFeaturePoint3D::selectY() | vpFeaturePoint3D::selectZ());
  \endcode

*/
vpColVector
vpFeaturePoint3D::error(const vpBasicFeature &s_star,
			const unsigned int select)
{
  vpColVector e(0) ;

  try{
    if (vpFeaturePoint3D::selectX() & select )
    {
      vpColVector ex(1) ;
      ex[0] = s[0] - s_star[0] ;

      e = vpMatrix::stackMatrices(e,ex) ;
    }

    if (vpFeaturePoint3D::selectY() & select )
    {
      vpColVector ey(1) ;
      ey[0] = s[1] - s_star[1] ;
      e =  vpMatrix::stackMatrices(e,ey) ;
    }

    if (vpFeaturePoint3D::selectZ() & select )
    {
      vpColVector ez(1) ;
      ez[0] = s[2] - s_star[2] ;
      e =  vpMatrix::stackMatrices(e,ez) ;
    }
  }
  catch(vpMatrixException me)
  {
    vpERROR_TRACE("caught a Matrix related error") ;
    std::cout <<std::endl << me << std::endl ;
    throw(me) ;
  }
  catch(vpException me)
  {
    vpERROR_TRACE("caught another error") ;
    std::cout <<std::endl << me << std::endl ;
    throw(me) ;
  }

  return e ;

}
static std::vector<Point_2> mapPointsToOXYplane(std::vector<Vector3d> points,
		Vector3d nu)
{
	DEBUG_START;
	Vector3d ez(0., 0., 1.);
	nu.norm(1.);
	Vector3d tau = nu % ez;

	double xCurr = 0., yCurr = 0.;
	std::vector<Point_2> pointsMapped;
	for(auto &point : points)
	{
		xCurr = point * tau;
		yCurr = point * ez;
		pointsMapped.push_back(Point_2(xCurr, yCurr));
	}
	DEBUG_END;
	return pointsMapped;
}
/*!
  Compute the error \f$ (s-s^*)\f$ between the current and the desired
  visual features from a subset of the possible features.

  - With the feature type cdMc:
  Since this visual feature \f$ s \f$ represent the 3D translation from the desired
  camera frame to the current one \f$^{c^*}t_{c} \f$, the desired
  visual feature \f$ s^* \f$ should be zero. Thus, the error is here
  equal to the current visual feature \f$ s \f$.

  - With the feature type cMo:
  In this case the desired feature is not necessary equal to zero. Thus, the error is here equal to \f$ s-s^* \f$.

  \param s_star : Desired visual feature.

  \param select : The error can be computed for a selection of a
  subset of the possible translation features.
  - To compute the error for all the three translation vector coordinates use
    vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3 
    dimension column vector.
  - To compute the error for only one of the translation vector coordinate
    feature \f$(t_x, t_y, t_z)\f$ use one of the
    corresponding function selectTx(), selectTy() or selectTz(). In
    that case the error vector is a 1 dimension column vector.

  \return The error \f$ (s-s^*)\f$ between the current and the desired
  visual feature.

  \exception vpFeatureException::badInitializationError : If the
  desired visual feature \f$ s^* \f$ is not equal to zero in the case of the feature type is cdMc or cMcd.

  The code below shows how to use this method to manipulate the \f$
  t_z \f$ subset in the case of the cdMc feature type. It can be used also with the cMo feature type. In that case just change vpFeatureTranslation::cdMc by vpFeatureTranslation::cMo during the declaration of the two vpFeatureTranslation features.

  \code
  // Creation of the current feature s
  vpFeatureTranslation s(vpFeatureTranslation::cdMc);
  s.set_TUz(0.3); // Initialization of the feature

  // Creation of the desired feature s*. By default this feature is 
  // initialized to zero
  vpFeatureTranslation s_star(vpFeatureTranslation::cdMc); 

  // Compute the interaction matrix for the t_z translation feature
  vpMatrix L_z = s.interaction( vpFeatureTranslation::selectTz() );

  // Compute the error vector (s-s*) for the t_z feature
  s.error(s_star, vpFeatureTranslation::selectTz());
  \endcode

  To manipulate the subset features \f$s=(t_y, t_z)\f$,
  the code becomes:
  \code
  // Compute the interaction matrix for the t_y, t_z features
  vpMatrix L_yz = s.interaction( vpFeatureTranslation::selectTy() | vpFeatureTranslation::selectTz() );

  // Compute the error vector e = (s-s*) for the t_y, t_z feature
  vpColVector e = s.error(s_star, vpFeatureTranslation::selectTy() | vpFeatureTranslation::selectTz());
  \endcode

*/
vpColVector
vpFeatureTranslation::error(const vpBasicFeature &s_star,
			    const unsigned int select)
{
  vpColVector e(0) ;

  if(translation == cdMc || translation == cMcd)
  {
    if (s_star.get_s().sumSquare() > 1e-6)
    {
      vpERROR_TRACE("s* should be zero ! ") ;
      throw(vpFeatureException(vpFeatureException::badInitializationError,
			       "s* should be zero !")) ;
    }
  }


  if (vpFeatureTranslation::selectTx() & select )
    {
      vpColVector ex(1) ;
      ex[0] = s[0]-s_star[0]  ;
      e = vpMatrix::stackMatrices(e,ex) ;
    }

  if (vpFeatureTranslation::selectTy() & select )
    {
      vpColVector ey(1) ;
      ey[0] = s[1]-s_star[1] ;
      e = vpMatrix::stackMatrices(e,ey) ;
    }

  if (vpFeatureTranslation::selectTz() & select )
    {
      vpColVector ez(1) ;
      ez[0] = s[2]-s_star[2] ;
      e = vpMatrix::stackMatrices(e,ez) ;
    }

  return e ;
}
static std::pair<ShadowContourDataPtr, Vector3d> balanceShadowContourData(ShadowContourDataPtr data)
{
	DEBUG_START;
	/* Construct polyhedron which facets are the contours. */
	PolyhedronPtr p(new Polyhedron(data));
	p->set_parent_polyhedron_in_facets();

	/* Calculate the mass center of contours. */
	SizeCalculator *sizeCalculator = new SizeCalculator(p);
	Vector3d center = sizeCalculator->calculateSurfaceCenter();
	DEBUG_PRINT("Center of contours = (%lf, %lf, %lf)",
		                center.x, center.y, center.z);

	/*
	 * Shift all contours on z component of the std::vector of mass center.
	 */
	Vector3d ez(0., 0., 1.);
	Vector3d shiftingVector = - (ez * center) * ez;
	auto dataShifted = shiftShadowContourData(data, shiftingVector);
	DEBUG_END;
	return std::pair<ShadowContourDataPtr, Vector3d>(dataShifted,
			shiftingVector);
}
示例#10
0
/*!
  
  Compute the error \f$ (s-s^*)\f$ between the current and the desired
  visual features from a subset of the possible features.

  Since this visual feature \f$ s \f$ represent either the rotation
  from the desired camera frame to the current camera frame
  \f$^{c^*}R_{c} \f$, or the rotation from the current camera frame to
  the desired camera frame \f$^{c}R_{c^*} \f$, the desired visual
  feature \f$ s^* \f$ should be zero. Thus, the error is here equal to
  the current visual feature \f$ s \f$.

  \param s_star : Desired visual visual feature that should be equal to zero.

  \param select : The error can be computed for a selection of a
  subset of the possible \f$ \theta u \f$ features.
  - To compute the error for all the three \f$ \theta u \f$ features use
    vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3 
    dimension column vector.
  - To compute the error for only one of the \f$ \theta u \f$ component 
    feature (\f$ \theta u_x, \theta u_y, \theta u_z\f$) use one of the
    corresponding function selectTUx(), selectTUy() or selectTUz(). In
    that case the error vector is a 1 dimension column vector.

  \return The error \f$ (s-s^*)\f$ between the current and the desired
  visual feature.

  \exception vpFeatureException::badInitializationError : If the
  desired visual feature \f$ s^* \f$ is not equal to zero.

  The code below shows how to use this method to manipulate the
  \f$\theta u_z \f$ subset:

  \code
  // Creation of the current feature s
  vpFeatureThetaU s(vpFeatureThetaU::cdRc);
  s.set_TUz(0.3);

  // Creation of the desired feature s^*. By default this feature is 
  // initialized to zero
  vpFeatureThetaU s_star(vpFeatureThetaU::cdRc); 

  // Compute the interaction matrix for the ThetaU_z feature
  vpMatrix L_z = s.interaction( vpFeatureThetaU::selectTUz() );

  // Compute the error vector (s-s*) for the ThetaU_z feature
  s.error(s_star, vpFeatureThetaU::selectTUz());
  \endcode

  To manipulate the subset features \f$s=(\theta u_y, \theta u_z)\f$,
  the code becomes:
  \code
  // Compute the interaction matrix for the ThetaU_y, ThetaU_z features
  vpMatrix L_yz = s.interaction( vpFeatureThetaU::selectTUy() | vpFeatureThetaU::selectTUz() );

  // Compute the error vector e = (s-s*) for the ThetaU_y, ThetaU_z feature
  vpColVector e = s.error(s_star, vpFeatureThetaU::selectTUy() | vpFeatureThetaU::selectTUz());
  \endcode

*/
vpColVector
vpFeatureThetaU::error(const vpBasicFeature &s_star,
		       const unsigned int select)
{

  if (fabs(s_star.get_s().sumSquare()) > 1e-6)
    {
      vpERROR_TRACE("s* should be zero ! ") ;
      throw(vpFeatureException(vpFeatureException::badInitializationError,
			       "s* should be zero !")) ;
    }

  vpColVector e(0) ;


  if (vpFeatureThetaU::selectTUx() & select )
    {
      vpColVector ex(1) ;
      ex[0] = s[0]  ;
      e = vpColVector::stack(e,ex) ;
    }

  if (vpFeatureThetaU::selectTUy() & select )
    {
      vpColVector ey(1) ;
      ey[0] = s[1] ;
      e = vpColVector::stack(e,ey) ;
    }

  if (vpFeatureThetaU::selectTUz() & select )
    {
      vpColVector ez(1) ;
      ez[0] = s[2] ;
      e = vpColVector::stack(e,ez) ;
    }
  return e ;
}
示例#11
0
/*!
  Compute the error \f$ (s-s^*)\f$ between the current and the desired
  visual features from a subset of the possible features.

  \param s_star : Desired 3D point visual feature.

  \param select : The error can be computed for a selection of a
  subset of the possible 3D point coordinate features.
  - To compute the error for all the three coordinates use
    vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3 
    dimension column vector.
  - To compute the error for only one of the coordinate
    feature \f$(X,Y, or Z)\f$ use one of the
    corresponding function selectX(), selectY() or selectZ(). In
    that case the error vector is a 1 dimension column vector.

  \return The error \f$ (s-s^*)\f$ between the current and the desired
  visual feature.

  The code below shows how to use this method to manipulate the \f$
  Z \f$ subset:

  \code
  // Creation of the current feature s
  vpFeaturePoint3D s;
  s.set_Z(0.8); // Initialization of the current Z feature

  // Creation of the desired feature s*. 
  vpFeatureTranslation s_star; 
  s_star.set_Z(1); // Initialization of the current Z* feature to Z*=1 meter 

  // Compute the interaction matrix for the Z coordinate feature
  vpMatrix L_Z = s.interaction( vpFeaturePoint3D::selectZ() );

  // Compute the error vector (s-s*) for the Z feature
  s.error(s_star, vpFeaturePoint3D::selectZ());
  \endcode

  To manipulate the subset features \f$s=(Y, Z)\f$,
  the code becomes:
  \code
  // Compute the interaction matrix for the Y, Z feature coordinates
  vpMatrix L_YZ = s.interaction( vpFeaturePoint3D::selectY() | vpFeaturePoint3D::selectZ() );

  // Compute the error vector e = (s-s*) for the Y, Z feature coordinates
  vpColVector e = s.error(s_star, vpFeaturePoint3D::selectY() | vpFeaturePoint3D::selectZ());
  \endcode

*/
vpColVector
vpFeaturePoint3D::error(const vpBasicFeature &s_star,
			const unsigned int select)
{
  vpColVector e(0) ;

  try{
    if (vpFeaturePoint3D::selectX() & select )
    {
      vpColVector ex(1) ;
      ex[0] = s[0] - s_star[0] ;

      e = vpColVector::stack(e,ex) ;
    }

    if (vpFeaturePoint3D::selectY() & select )
    {
      vpColVector ey(1) ;
      ey[0] = s[1] - s_star[1] ;
      e = vpColVector::stack(e,ey) ;
    }

    if (vpFeaturePoint3D::selectZ() & select )
    {
      vpColVector ez(1) ;
      ez[0] = s[2] - s_star[2] ;
      e = vpColVector::stack(e,ez) ;
    }
  }
  catch(...) {
    throw ;
  }

  return e ;

}
示例#12
0
void MagCal::calcMagComp()
{
    /*
     * Inspired by
     * http://davidegironi.blogspot.it/2013/01/magnetometer-calibration-helper-01-for.html#.UriTqkMjulM
     *
     * Ellipsoid fit from:
     * http://www.mathworks.com/matlabcentral/fileexchange/24693-ellipsoid-fit
     *
     * To use Eigen to convert matlab code, have a look at Eigen/AsciiQuickReference.txt
     */

    if (mMagSamples.size() < 9) {
        QMessageBox::warning(this, "Magnetometer compensation",
                             "Too few points.");
        return;
    }

    int samples = mMagSamples.size();
    Eigen::VectorXd ex(samples);
    Eigen::VectorXd ey(samples);
    Eigen::VectorXd ez(samples);

    for (int i = 0;i < samples;i++) {
        ex(i) = mMagSamples.at(i).at(0);
        ey(i) = mMagSamples.at(i).at(1);
        ez(i) = mMagSamples.at(i).at(2);
    }

    Eigen::MatrixXd eD(samples, 9);

    for (int i = 0;i < samples;i++) {
        eD(i, 0) = ex(i) * ex(i);
        eD(i, 1) = ey(i) * ey(i);
        eD(i, 2) = ez(i) * ez(i);
        eD(i, 3) = 2.0 * ex(i) * ey(i);
        eD(i, 4) = 2.0 * ex(i) * ez(i);
        eD(i, 5) = 2.0 * ey(i) * ez(i);
        eD(i, 6) = 2.0 * ex(i);
        eD(i, 7) = 2.0 * ey(i);
        eD(i, 8) = 2.0 * ez(i);
    }

    Eigen::MatrixXd etmp1 = eD.transpose() * eD;
    Eigen::MatrixXd etmp2 = eD.transpose() * Eigen::MatrixXd::Ones(samples, 1);
    Eigen::VectorXd eV = etmp1.lu().solve(etmp2);

    Eigen::MatrixXd eA(4, 4);
    eA(0,0)=eV(0);   eA(0,1)=eV(3);   eA(0,2)=eV(4);   eA(0,3)=eV(6);
    eA(1,0)=eV(3);   eA(1,1)=eV(1);   eA(1,2)=eV(5);   eA(1,3)=eV(7);
    eA(2,0)=eV(4);   eA(2,1)=eV(5);   eA(2,2)=eV(2);   eA(2,3)=eV(8);
    eA(3,0)=eV(6);   eA(3,1)=eV(7);   eA(3,2)=eV(8);   eA(3,3)=-1.0;

    Eigen::MatrixXd eCenter = -eA.topLeftCorner(3, 3).lu().solve(eV.segment(6, 3));
    Eigen::MatrixXd eT = Eigen::MatrixXd::Identity(4, 4);
    eT(3, 0) = eCenter(0);
    eT(3, 1) = eCenter(1);
    eT(3, 2) = eCenter(2);

    Eigen::MatrixXd eR = eT * eA * eT.transpose();

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eEv(eR.topLeftCorner(3, 3) * (-1.0 / eR(3, 3)));
    Eigen::MatrixXd eVecs = eEv.eigenvectors();
    Eigen::MatrixXd eVals = eEv.eigenvalues();

    Eigen::MatrixXd eRadii(3, 1);
    eRadii(0) = sqrt(1.0 / eVals(0));
    eRadii(1) = sqrt(1.0 / eVals(1));
    eRadii(2) = sqrt(1.0 / eVals(2));

    Eigen::MatrixXd eScale = eRadii.asDiagonal().inverse() * eRadii.minCoeff();
    Eigen::MatrixXd eComp = eVecs * eScale * eVecs.transpose();

    mMagComp.resize(9);
    mMagComp[0] = eComp(0, 0);
    mMagComp[1] = eComp(0, 1);
    mMagComp[2] = eComp(0, 2);

    mMagComp[3] = eComp(1, 0);
    mMagComp[4] = eComp(1, 1);
    mMagComp[5] = eComp(1, 2);

    mMagComp[6] = eComp(2, 0);
    mMagComp[7] = eComp(2, 1);
    mMagComp[8] = eComp(2, 2);

    mMagCompCenter.resize(3);
    mMagCompCenter[0] = eCenter(0, 0);
    mMagCompCenter[1] = eCenter(1, 0);
    mMagCompCenter[2] = eCenter(2, 0);

    QVector<double> magX, magY, magZ;

    for (int i = 0;i < mMagSamples.size();i++) {
        double mx = mMagSamples.at(i).at(0);
        double my = mMagSamples.at(i).at(1);
        double mz = mMagSamples.at(i).at(2);

        mx -= mMagCompCenter.at(0);
        my -= mMagCompCenter.at(1);
        mz -= mMagCompCenter.at(2);

        magX.append(mx * mMagComp.at(0) + my * mMagComp.at(1) + mz * mMagComp.at(2));
        magY.append(mx * mMagComp.at(3) + my * mMagComp.at(4) + mz * mMagComp.at(5));
        magZ.append(mx * mMagComp.at(6) + my * mMagComp.at(7) + mz * mMagComp.at(8));
    }

    ui->magSampXyPlot->graph(1)->setData(magX, magY);
    ui->magSampXzPlot->graph(1)->setData(magX, magZ);
    ui->magSampYzPlot->graph(1)->setData(magY, magZ);

    updateMagPlots();
}
示例#13
0
void LCGWSpinBBHNR1::ConvertSpinDirToSSB(int iSpin)
{
	double thS, phS, th, ph;
	double iota, stheta, nez;
	double thBS, phBS;
	double phi0(0.);
	LCVector ex(MT), ey(MT), spin(MT), nW(MT), ez(MT);
	
	
	if(iSpin==1){
		thS = thS1;
		phS = phS1;
	}
	if(iSpin==2){
		thS = thS2;
		phS = phS2;
	}
	
	th = M_PI/2. - Beta;
	ph = Lambda;
	
	
	//! **** Compute \f$ \theta_L \f$ and \f$ \phi_L \f$ :
	//! ** Declaration of coordinates of ^k, ^theta, ^phi and ^Ln in SSB frame and cos / sin of some angles
	double x_k, y_k, z_k;
	double x_th, y_th, z_th;
	double x_ph, y_ph, z_ph;
	// double LnBx, LnBy, LnBz;
	double sPhS, cPhS, sPsi, cPsi, sThd, cThd; 
	
	sPhS = sin(phiS);
	cPhS = cos(phiS);
	sPsi = sin(2.*M_PI-NRPolarization);
	cPsi = cos(2.*M_PI-NRPolarization);
	sThd = sin(Thd);
	cThd = cos(Thd);
	
	x_k = - sThS * cPhS;
	y_k = - sThS * sPhS;
	z_k = - cThS;
	
	x_th = cThS * cPhS;
	y_th = cThS * sPhS;
	z_th = - sThS;
    
	x_ph = -y_k * z_th + z_k * y_th;
	y_ph = -z_k * x_th + x_k * z_th;
	z_ph = -x_k * y_th + y_k * x_th;
    
	
    
	
	
	//! ** Following convention of Sofiane for the angle psi (^p = cos psi ^theta + sin psi ^phi) :
	//	 LnBx = (- sThd * sPsi * x_th + sThd * cPsi * x_ph + cThd * x_k  );
	//	 LnBy = (- sThd * sPsi * y_th + sThd * cPsi * y_ph + cThd * y_k  );
	//	 LnBz = (- sThd * sPsi * z_th + sThd * cPsi * z_ph + cThd * z_k  );
	
	
	/*  
	 // new
	 
	 LnBx = (- sThd * sPsi * x_th - sThd * cPsi * x_ph + cThd * x_k  );
	 LnBy = (- sThd * sPsi * y_th - sThd * cPsi * y_ph + cThd * y_k  );
	 LnBz = (- sThd * sPsi * z_th - sThd * cPsi * z_ph + cThd * z_k  );
	 */
    
    /*
	 //! ** Following convention of Antoine for the angle psi (^p = cos psi ^theta - sin psi ^phi) :
	 LnBx = -( sThd * cPsi * x_th - sThd * sPsi * x_ph - cThd * x_k  );
	 LnBy = -( sThd * cPsi * y_th - sThd * sPsi * y_ph - cThd * y_k  );
	 LnBz = -( sThd * cPsi * z_th - sThd * sPsi * z_ph - cThd * z_k  );   
	 */
  	
	// with all angles
	
	
    LnB.x( -LnN.y()*cPsi*x_th+LnN.y()*sPsi*x_ph+LnN.x()*cThd*sPsi*x_th+LnN.x()*cThd*cPsi*x_ph-LnN.z()*sThd*sPsi*x_th-LnN.z()*sThd*cPsi*x_ph+x_k*LnN.x()*sThd+x_k*LnN.z()*cThd );
    LnB.y( -LnN.y()*cPsi*y_th+LnN.y()*sPsi*y_ph+LnN.x()*cThd*sPsi*y_th+LnN.x()*cThd*cPsi*y_ph-LnN.z()*sThd*sPsi*y_th-LnN.z()*sThd*cPsi*y_ph+y_k*LnN.x()*sThd+y_k*LnN.z()*cThd );
    LnB.z( -LnN.y()*cPsi*z_th+LnN.y()*sPsi*z_ph+LnN.x()*cThd*sPsi*z_th+LnN.x()*cThd*cPsi*z_ph-LnN.z()*sThd*sPsi*z_th-LnN.z()*sThd*cPsi*z_ph+z_k*LnN.x()*sThd+z_k*LnN.z()*cThd );
    
    
    double n_LnB = LnB.norm();
    
	//	thBL =   atan2(sqrt(LnBx*LnBx + LnBy*LnBy),LnBz);  // acos(LnBz/n_LnB); corrected by Sofiane
    thBL =  acos(LnB.z()/n_LnB);
    
	//    if(thBL<0.)
	//        thBL +-M_PI;
    
	//	phBL =M_PI*(1-LnBy/fabs(LnBy)) + LnBy/fabs(LnBy) * acos(LnBx / n_LnB / sqrt(1. - LnBz*LnBz / (n_LnB*n_LnB)) );  //  atan2(LnBy,LnBx); corrected by Sofiane
	
    phBL = LnB.ph();
    
    
    if(phBL<0.)
  		phBL += 2.*M_PI;
	
	//  double up = cThS*sin(thBL)*cos(phiS-phBL) - cos(thBL)*sThS;   /: commented by Sofiane
	//  double down = sin(thBL)*sin(phiS - phBL);    // commented by Sofiane
    
    ConvertLS1S2dirNR2SSBSofVer(Phd, thetaS, phiS, thetaJ, phiJ, LnN, S1N, S2N, AmpL, AmpS1, AmpS2, LnB, S1B, S2B);  // see how to use it in more elegant way
    
    
    double up =  cos(Phd - phiS);    // introduced by Sofiane
    double down = cos(thetaS)*sin(Phd - phiS);  // introduced by Sofiane
    
    PSIN =atan2(up,down);
    
    if(PSIN<0.)  
        PSIN +=2*M_PI;
	
	
	iota = acos(sin(th)*sin(thBL)*cos(ph - phBL) + cos(th)*cos(thBL)); 
	
    
	stheta = fabs(sin(iota));
	if (iota == 0.0)
		Cout << "WARNING in LCGWSpinBBHNR1::ConvertSpinDirToSSB : degenerate case need to consider separately: L colinear with n !" << Endl;
	nW.p[0] = sin(th)*cos(ph) ;
	nW.p[1] = sin(th)*sin(ph) ;
	nW.p[2] = cos(th) ;
	ez.p[0] = sin(thBL)*cos(phBL) ;  
	ez.p[1] = sin(thBL)*sin(phBL) ;
	ez.p[2] = cos(thBL) ;
	ey.p[0] = (nW.p[1]*ez.p[2] - nW.p[2]*ez.p[1])/stheta ;
	ey.p[1] = (nW.p[2]*ez.p[0] - nW.p[0]*ez.p[2])/stheta ;
	ey.p[2] = (nW.p[0]*ez.p[1] - nW.p[1]*ez.p[0])/stheta ;
	
	nez = nW.p[0]*ez.p[0] + nW.p[1]*ez.p[1] + nW.p[2]*ez.p[2] ;
	ex.p[0] = (ez.p[0]*nez - nW.p[0])/stheta ;
	ex.p[1] = (ez.p[1]*nez - nW.p[1])/stheta ;
	ex.p[2] = (ez.p[2]*nez - nW.p[2])/stheta ;
	
	//! ** if it is eccentric orbit, we need to rotate ex, ey by -phi0
	
	for(int i=0; i<3; i++)
		spin.p[i] = sin(thS)*cos(phS)*( cos(phi0)*ex.p[i] - sin(phi0)*ey.p[i] ) + sin(thS)*sin(phS)*( sin(phi0)*ex.p[i] + cos(phi0)*ey.p[i] ) + cos(thS)*ez.p[i] ;
	
	thBS = acos(spin.p[2]);
	phBS = atan2(spin.p[1], spin.p[0]);
	if (phBS < 0.)
		phBS = phBS + 2.*M_PI;
	
	if(iSpin==1){
		thBS1 = thBS;
		phBS1 = phBS;
	}
	if(iSpin==2){
		thBS2 = thBS;
		phBS2 = phBS;
	}	
	
	S1B.x( sin(thBS1)*cos(phBS1) );
	S1B.y( sin(thBS1)*sin(phBS1) );
	S1B.z( cos(thBS1) );
	
	S2B.x( sin(thBS2)*cos(phBS2) );
	S2B.y( sin(thBS2)*sin(phBS2) );
	S2B.z( cos(thBS2) );
	
	
}
   void QuatOpsTest::testQuatDiv()
   {
      // test matrix div version against quat div version, 
      // make sure they all work the same.
      // note: this test depends on the xforms functions working
      {
         const float eps = 0.0001f;
         gmtl::Matrix44f q3( gmtl::makeRot<gmtl::Matrix44f>( gmtl::AxisAnglef( gmtl::Math::deg2Rad( 90.0f ), 0,1,0 ) ) ), 
                           q4( gmtl::makeRot<gmtl::Matrix44f>( gmtl::AxisAnglef( gmtl::Math::deg2Rad( 90.0f ), 0,0,1 ) ) ), q6;
         gmtl::Vec3f sx( 6,0,0 ), sy( 0,4,0 ), sz( 0,0,9 ), ex( 0,0,6 ), ey( -4,0,0 ), ez( 0,-9,0 ), tx, ty, tz;

         gmtl::invert( q3 ); // there is no matrix div, so do this to simulate it.

         // make sure the mult() func works
         gmtl::mult( q6, q4, q3 );
         tx = q6 * sx;
         ty = q6 * sy;
         tz = q6 * sz;
         CPPUNIT_ASSERT( gmtl::isEqual( ex, tx, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ey, ty, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ez, tz, eps ) );

         // make sure the operator* works too
         q6 = q4 * q3;
         tx = q6 * sx;
         ty = q6 * sy;
         tz = q6 * sz;
         CPPUNIT_ASSERT( gmtl::isEqual( ex, tx, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ey, ty, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ez, tz, eps ) );

         // make sure the operator*= works too
         q6 = q4;
         q6 *= q3;
         tx = q6 * sx;
         ty = q6 * sy;
         tz = q6 * sz;
         CPPUNIT_ASSERT( gmtl::isEqual( ex, tx, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ey, ty, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ez, tz, eps ) );
      }
      {
         const float eps = 0.0001f;
         gmtl::Quat<float> q3( gmtl::makeRot<gmtl::Quatf>( gmtl::AxisAnglef( gmtl::Math::deg2Rad( 90.0f ), 0,1,0 ) ) ), 
                           q4( gmtl::makeRot<gmtl::Quatf>( gmtl::AxisAnglef( gmtl::Math::deg2Rad( 90.0f ), 0,0,1 ) ) ), q5, q6;
         gmtl::Vec3f sx( 6,0,0 ), sy( 0,4,0 ), sz( 0,0,9 ), ex( 0,0,6 ), ey( -4,0,0 ), ez( 0,-9,0 ), tx, ty, tz;

         // make sure the mult() func works
         gmtl::div( q6, q4, q3 ); 
         tx = q6 * sx;
         ty = q6 * sy;
         tz = q6 * sz;
         CPPUNIT_ASSERT( gmtl::isEqual( ex, tx, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ey, ty, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ez, tz, eps ) );

         // make sure the operator* works too
         q6 = q4 / q3; 
         tx = q6 * sx;
         ty = q6 * sy;
         tz = q6 * sz;
         CPPUNIT_ASSERT( gmtl::isEqual( ex, tx, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ey, ty, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ez, tz, eps ) );

         // make sure the operator*= works too
         q6 = q4;
         q6 /= q3;
         tx = q6 * sx;
         ty = q6 * sy;
         tz = q6 * sz;
         CPPUNIT_ASSERT( gmtl::isEqual( ex, tx, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ey, ty, eps ) );
         CPPUNIT_ASSERT( gmtl::isEqual( ez, tz, eps ) );
      }
   }
示例#15
0
RTC::ReturnCode_t ReferenceForceUpdater::onExecute(RTC::UniqueId ec_id)
{
  loop ++;

  // check dataport input
  for (unsigned int i=0; i<m_forceIn.size(); i++){
    if ( m_forceIn[i]->isNew() ) {
      m_forceIn[i]->read();
    }
    if ( m_ref_forceIn[i]->isNew() ) {
      m_ref_forceIn[i]->read();
    }
  }
  if (m_basePosIn.isNew()) {
    m_basePosIn.read();
  }
  if (m_baseRpyIn.isNew()) {
    m_baseRpyIn.read();
  }
  if (m_rpyIn.isNew()) {
    m_rpyIn.read();
  }
  if (m_qRefIn.isNew()) {
    m_qRefIn.read();
  }

  //syncronize m_robot to the real robot
  if ( m_qRef.data.length() ==  m_robot->numJoints() ) {
    Guard guard(m_mutex);

    // Interpolator
    for (std::map<std::string, ReferenceForceUpdaterParam>::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) {
      std::string arm = itr->first;
      size_t arm_idx = ee_index_map[arm];
      bool transition_interpolator_isEmpty = transition_interpolator[arm]->isEmpty();
      if ( ! transition_interpolator_isEmpty ) {
        transition_interpolator[arm]->get(&transition_interpolator_ratio[arm_idx], true);
        if ( transition_interpolator[arm]->isEmpty() && m_RFUParam[arm].is_active && m_RFUParam[arm].is_stopping ) {
          std::cerr << "[" << m_profile.instance_name << "] ReferenceForceUpdater active => inactive." << std::endl;
          m_RFUParam[arm].is_active = false;
          m_RFUParam[arm].is_stopping = false;
        }
      }
    }
    // If RFU is not active
    {
      bool all_arm_is_not_active = true;
      for (std::map<std::string, ReferenceForceUpdaterParam>::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) {
        std::string arm = itr->first;
        size_t arm_idx = ee_index_map[arm];
        if ( m_RFUParam[arm].is_active ) all_arm_is_not_active = false;
        else for (unsigned int j=0; j<3; j++ ) ref_force[arm_idx](j) = m_ref_force_in[arm_idx].data[j];
      }
      //determin ref_force_out from ref_force_in
      if ( all_arm_is_not_active ) {
        for (unsigned int i=0; i<m_ref_force_in.size(); i++ ){
          for (unsigned int j=0; j<6; j++ ) {
            m_ref_force_out[i].data[j] = m_ref_force_in[i].data[j];
          }
          m_ref_force_out[i].tm = m_ref_force_in[i].tm;
          m_ref_forceOut[i]->write();
        }
        return RTC::RTC_OK;
      }
    }

    // If RFU is active
    {
      hrp::dvector qorg(m_robot->numJoints());

      // reference model
      for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
        qorg[i] = m_robot->joint(i)->q;
        m_robot->joint(i)->q = m_qRef.data[i];
      }
      m_robot->rootLink()->p = hrp::Vector3(m_basePos.data.x, m_basePos.data.y, m_basePos.data.z);
      m_robot->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y);
      m_robot->calcForwardKinematics();
      if ( (ee_map.find("rleg") != ee_map.end() && ee_map.find("lleg") != ee_map.end()) // if legged robot
           && !use_sh_base_pos_rpy ) {
        // TODO
        //  Tempolarily modify root coords to fix foot pos rot
        //  This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272

        // get current foot mid pos + rot
        std::vector<hrp::Vector3> foot_pos;
        std::vector<hrp::Matrix33> foot_rot;
        std::vector<std::string> leg_names;
        leg_names.push_back("rleg");
        leg_names.push_back("lleg");
        for (size_t i = 0; i < leg_names.size(); i++) {
          hrp::Link* target_link = m_robot->link(ee_map[leg_names[i]].target_name);
          foot_pos.push_back(target_link->p + target_link->R * ee_map[leg_names[i]].localPos);
          foot_rot.push_back(target_link->R * ee_map[leg_names[i]].localR);
        }
        hrp::Vector3 current_foot_mid_pos ((foot_pos[0]+foot_pos[1])/2.0);
        hrp::Matrix33 current_foot_mid_rot;
        rats::mid_rot(current_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]);
        // calculate fix pos + rot
        hrp::Vector3 new_foot_mid_pos(current_foot_mid_pos);
        hrp::Matrix33 new_foot_mid_rot;
        {
          hrp::Vector3 ex = hrp::Vector3::UnitX();
          hrp::Vector3 ez = hrp::Vector3::UnitZ();
          hrp::Vector3 xv1 (current_foot_mid_rot * ex);
          xv1(2) = 0.0;
          xv1.normalize();
          hrp::Vector3 yv1(ez.cross(xv1));
          new_foot_mid_rot(0,0) = xv1(0); new_foot_mid_rot(1,0) = xv1(1); new_foot_mid_rot(2,0) = xv1(2);
          new_foot_mid_rot(0,1) = yv1(0); new_foot_mid_rot(1,1) = yv1(1); new_foot_mid_rot(2,1) = yv1(2);
          new_foot_mid_rot(0,2) = ez(0); new_foot_mid_rot(1,2) = ez(1); new_foot_mid_rot(2,2) = ez(2);
        }
        // fix root pos + rot to fix "coords" = "current_foot_mid_xx"
        hrp::Matrix33 tmpR (new_foot_mid_rot * current_foot_mid_rot.transpose());
        m_robot->rootLink()->p = new_foot_mid_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos);
        rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
        m_robot->calcForwardKinematics();
      }
    }

    for (std::map<std::string, ReferenceForceUpdaterParam>::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) {
      // Update reference force
      hrp::Vector3 internal_force = hrp::Vector3::Zero();
      std::string arm = itr->first;
      size_t arm_idx = ee_index_map[arm];
      double interpolation_time = 0;
      bool is_active = m_RFUParam[arm].is_active;
      if ( is_active && loop % m_RFUParam[arm].update_count == 0 ) {
        hrp::Link* target_link = m_robot->link(ee_map[arm].target_name);
        hrp::Vector3 abs_motion_dir, tmp_act_force, df;
        hrp::Matrix33 ee_rot, sensor_rot;
        ee_rot = target_link->R * ee_map[arm].localR;
        if ( m_RFUParam[arm].frame=="local" )
            abs_motion_dir = ee_rot * m_RFUParam[arm].motion_dir;
        else {
            hrp::Matrix33 current_foot_mid_rot;
            std::vector<hrp::Matrix33> foot_rot;
            std::vector<std::string> leg_names;
            leg_names.push_back("rleg");
            leg_names.push_back("lleg");
            for (size_t i = 0; i < leg_names.size(); i++) {
                hrp::Link* target_link = m_robot->link(ee_map[leg_names[i]].target_name);
                foot_rot.push_back(target_link->R * ee_map[leg_names[i]].localR);
            }
            rats::mid_rot(current_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]);
            abs_motion_dir = current_foot_mid_rot * m_RFUParam[arm].motion_dir;
        }
        for (size_t i = 0; i < 3; i++ ) tmp_act_force(i) = m_force[arm_idx].data[i];
        hrp::Sensor* sensor = m_robot->sensor(hrp::Sensor::FORCE, arm_idx);
        sensor_rot = sensor->link->R * sensor->localR;
        tmp_act_force = sensor_rot * tmp_act_force;
        // Calc abs force diff
        df = tmp_act_force - ref_force[arm_idx];
        double inner_product = 0;
        if ( ! std::fabs((abs_motion_dir.norm() - 0.0)) < 1e-5 ) {
          abs_motion_dir.normalize();
          inner_product = df.dot(abs_motion_dir);
          if ( ! (inner_product < 0 && ref_force[arm_idx].dot(abs_motion_dir) < 0.0) ) {
            hrp::Vector3 in_f = ee_rot * internal_force;
            ref_force[arm_idx] = ref_force[arm_idx].dot(abs_motion_dir) * abs_motion_dir + in_f + (m_RFUParam[arm].p_gain * inner_product * transition_interpolator_ratio[arm_idx]) * abs_motion_dir;
            interpolation_time = (1/m_RFUParam[arm].update_freq) * m_RFUParam[arm].update_time_ratio;
            if ( ref_force_interpolator[arm]->isEmpty() ) {
              ref_force_interpolator[arm]->setGoal(ref_force[arm_idx].data(), interpolation_time, true);
            }
          }
        }
        if ( DEBUGP ) {
          std::cerr << "[" << m_profile.instance_name << "] Updating reference force" << std::endl;
          std::cerr << "[" << m_profile.instance_name << "]   inner_product = " << inner_product << ", ref_force = " << ref_force[arm_idx].dot(abs_motion_dir) << ", interpolation_time = " << interpolation_time << "[s]" << std::endl;
          std::cerr << "[" << m_profile.instance_name << "]   new ref_force = " << ref_force[arm_idx].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
          std::cerr << "[" << m_profile.instance_name << "]   act_force = " << tmp_act_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
          std::cerr << "[" << m_profile.instance_name << "]   df = " << df.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "    [", "]")) << std::endl;
        }
      }
      if (!ref_force_interpolator[arm]->isEmpty()) {
        ref_force_interpolator[arm]->get(ref_force[arm_idx].data(), true);
      }
    }
  }

  //determin ref_force_out from ref_force_in
  for (unsigned int i=0; i<m_ref_force_in.size(); i++ ){
    for (unsigned int j=0; j<6; j++ ) {
      m_ref_force_out[i].data[j] = m_ref_force_in[i].data[j];
    }
    for (unsigned int j=0; j<3; j++ ) {
      m_ref_force_out[i].data[j] = ref_force[i](j) * transition_interpolator_ratio[i] + m_ref_force_in[i].data[j] * (1-transition_interpolator_ratio[i]);
    }
    m_ref_force_out[i].tm = m_ref_force_in[i].tm;
    m_ref_forceOut[i]->write();
  }

  return RTC::RTC_OK;
}
示例#16
0
/** method to cacluate the detectors parameters and add them to the detectors
*averages
*@param spDet    -- shared pointer to the Mantid Detector
*@param Observer -- sample position or the centre of the polar system of
*coordinates to calculate detector's parameters.
*/
void AvrgDetector::addDetInfo(const Geometry::IDetector_const_sptr &spDet,
                              const Kernel::V3D &Observer) {
  m_nComponents++;
  Kernel::V3D detPos = spDet->getPos();
  Kernel::V3D toDet = (detPos - Observer);

  double dist2Det, Polar, Azimut, ringPolar, ringAzim;
  // identify the detector' position in the beam coordinate system:
  toDet.getSpherical(dist2Det, Polar, Azimut);
  if (m_nComponents <= 1) {
    m_FlightPathSum = dist2Det;
    m_PolarSum = Polar;
    m_AzimutSum = Azimut;

    m_AzimBase = Polar;
    m_PolarBase = Azimut;
    ringPolar = Polar;
    ringAzim = Azimut;
  } else {
    ringPolar = nearAngle(m_AzimBase, Polar);
    ringAzim = nearAngle(m_PolarBase, Azimut);
    m_FlightPathSum += dist2Det;
    m_PolarSum += ringPolar;
    m_AzimutSum += ringAzim;
  }

  // centre of the azimuthal ring (the ring  detectors form around the beam)
  Kernel::V3D ringCentre(0, 0, toDet.Z());

  // Get the bounding box
  Geometry::BoundingBox bbox;
  std::vector<Kernel::V3D> coord(3);

  Kernel::V3D er(0, 1, 0), e_th,
      ez(0, 0, 1); // ez along beamline, which is always oz;
  if (dist2Det)
    er = toDet / dist2Det; // direction to the detector
  Kernel::V3D e_tg =
      er.cross_prod(ez); // tangential to the ring and anticloakwise;
  e_tg.normalize();
  // make orthogonal -- projections are calculated in this coordinate system
  ez = e_tg.cross_prod(er);

  coord[0] = er;   // new X
  coord[1] = ez;   // new y
  coord[2] = e_tg; // new z
  bbox.setBoxAlignment(ringCentre, coord);

  spDet->getBoundingBox(bbox);

  // linear extensions of the bounding box orientied tangentially to the equal
  // scattering angle circle
  double azimMin = bbox.zMin();
  double azimMax = bbox.zMax();
  double polarMin = bbox.yMin(); // bounding box has been rotated according to
                                 // coord above, so z is along e_tg
  double polarMax = bbox.yMax();

  if (m_useSphericalSizes) {
    if (dist2Det == 0)
      dist2Det = 1;

    // convert to angular units
    double polarHalfSize =
        rad2deg * atan2(0.5 * (polarMax - polarMin), dist2Det);
    double azimHalfSize = rad2deg * atan2(0.5 * (azimMax - azimMin), dist2Det);

    polarMin = ringPolar - polarHalfSize;
    polarMax = ringPolar + polarHalfSize;
    azimMin = ringAzim - azimHalfSize;
    azimMax = ringAzim + azimHalfSize;
  }
  if (m_AzimMin > azimMin)
    m_AzimMin = azimMin;
  if (m_AzimMax < azimMax)
    m_AzimMax = azimMax;

  if (m_PolarMin > polarMin)
    m_PolarMin = polarMin;
  if (m_PolarMax < polarMax)
    m_PolarMax = polarMax;
}
示例#17
0
文件: ninf_full.cpp 项目: akemala/DMP
int main(int argc, char **argv)
{
  USING_NAMESPACE_ACADO
  const double KKT_tol = 1e-6;

  //READ THE DEMO LENGTHS & nBF FROM THE COMMAND LINE
  std::deque<std::string> args(argv + 1, argv + argc + !argc);
  const int nBF=atoi(args[0].c_str());
  args.pop_front();
  const int nD=(int)args.size();
  int nS=0;
  for(int i=0; i<nD;i++)
    nS+=atoi(args[i].c_str());


  //READ DATA
  std::string path=DATA_PATH;
  Matrix D = readFromFile((path+"demos.dat").c_str()); //d(:,0)=time, d(:,1)=x, d(:,2)=dx, d(:,3)=ddx;
  Vector pI = readFromFile((path+"initial_guess.dat").c_str());
  Matrix A = readFromFile((path+"inequality_constraint_matrix.dat").c_str());
  Vector b = readFromFile((path+"inequality_constraint_vector.dat").c_str());
  Matrix S = readFromFile((path+"scale_matrix.dat").c_str());

  //RELEVANT INDEX SETS
  std::vector<std::vector<int> > d_ind=getDemoInd(args);
  std::vector<int> a_ind=getAInd(nBF,nD);
  std::vector<int> b_ind=getBInd(nBF,nD);
  std::vector<std::vector<int> > w_ind=getWInd(nBF,nD);
  std::vector<int> r_ind=getRInd(nBF,nD);
  std::vector<int> c_ind=getCInd(nBF,nD);

  //PARAMETER & OBJECTIVE FUNCTION
 Parameter p(2*nD+nBF*(2+nD)+1,1); 

 Matrix BM(nS,2*nD+nBF*(2+nD)+1); BM.setZero();
 Expression B(BM);
 double t,x,dx;

 for (int d=0; d<nD; d++)
   for(int s=0;s<(int)d_ind[d].size();s++)
     {
       t=D(d_ind[d][s],0);
       x=D(d_ind[d][s],1);
       dx=D(d_ind[d][s],2);

       B(d_ind[d][s],a_ind[d])=x;
       B(d_ind[d][s],b_ind[d])=dx;

       for(int n=0;n<nBF;n++){
	  B(d_ind[d][s],w_ind[d][n])=(-0.5*(t-p(c_ind[n])*S(c_ind[n],c_ind[n])).getPowInt(2)/(p(r_ind[n])*p(r_ind[n])*S(r_ind[n],r_ind[n])*S(r_ind[n],r_ind[n]))).getExp();
	 // std::cout<<d<<std::endl;
	 //std::cout<< S(r_ind[d],r_ind[d])<<std::endl;
       }
     }           

 Expression f;
 f<<B*S*p-D.getCol(3);

 Expression ez(nS);
 for (int i=0; i<nS; i++)
   ez(i)=p(2*nD+nBF*(2+nD));


 Vector e(nS); e.setAll(1.0);
 Vector null(nS); null.setAll(0.0);


  NLP nlp;
  nlp.minimize(p(2*nD+nBF*(2+nD)));
  nlp.subjectTo(f - ez <= null);
  nlp.subjectTo(f + ez >= null);
  //nlp.subjectTo(A*S*p <= b);
  
  //ALGORITHM 
  ParameterEstimationAlgorithm algorithm(nlp);
  VariablesGrid initial_guess(2*nD+nBF*(2+nD)+1,0.0,0.0,1 );
  initial_guess.setVector( 0,S.getInverse()*pI );
  algorithm.initializeParameters(initial_guess);
  
  // OPTIONS
  algorithm.set( KKT_TOLERANCE, KKT_tol);
  algorithm.set( ABSOLUTE_TOLERANCE, 1e-4);
  algorithm.set( PRINTLEVEL,HIGH);
  algorithm.set( MAX_NUM_ITERATIONS, 2000 );
  algorithm.set (PRINT_COPYRIGHT, NO);
  // algorithm.set (PRINT_SCP_METHOD_PROFILE, YES);
  algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN ); 
  algorithm.set(GLOBALIZATION_STRATEGY, GS_LINESEARCH ); 
  algorithm.set(LINESEARCH_TOLERANCE, 1e-2 ); 
  algorithm.set(INFEASIBLE_QP_HANDLING,IQH_RELAX_L2);
  algorithm.set(FEASIBILITY_CHECK,BT_TRUE);


  // LOGGING
  LogRecord logRecord( LOG_AT_EACH_ITERATION,(path+"log.dat").c_str(),PS_PLAIN);
  logRecord << LOG_OBJECTIVE_VALUE;
  algorithm << logRecord;
 
  //SOLVING
  double clock1 = clock();
  algorithm.solve();
  double clock2 = clock();
  Vector solution;
  algorithm.getParameters(solution);
  // solution.print("optimal solution \n");
  solution.printToFile((path+"solution.dat").c_str(),"",PS_PLAIN);

  printf("\n computation time (ACADO) = %.16e \n", (clock2-clock1)/CLOCKS_PER_SEC);

  return 0;
}