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;
}
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);
}
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;}
}
}
}
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);
}
/*!
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 ;
}
/*!
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 ;
}
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();
}
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 ) );
}
}
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;
}
/** 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;
}
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;
}