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)) );
}
void __fastcall TForm1::Button1Click(TObject *Sender)
{
static int wyb;
resettablicy();
wyb = ComboBox1->ItemIndex;
if (wyb==1){
Ilosc = 2;
L[0].x = ex(-1.0);
L[0].y = ey(0.0);
L[0].vx= 0.0;
L[0].vy= -75.0;
L[1].x = ex(1.0);
L[1].y = ey(0.0);
L[1].vx= 0.0;
L[1].vy= 75.0;
}
}
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;
}
void player_bullets_draw(player_bullet *b, int liczba, BITMAP *bufor)
{
for (int i = 0; i < liczba; i++)
{
clear_to_color(b[i].obraz_rot, makecol(255, 0, 255));
rotate_sprite(b[i].obraz_rot, b[i].obraz,
(b[i].obraz_rot->w - b[i].obraz->w) / 2,
(b[i].obraz_rot->h - b[i].obraz->h) / 2,
itofix((int) (256.0 * b[i].rot / (2.0 * pi))));
masked_blit(b[i].obraz_rot, bufor, 0, 0,
ex(b[i].h * sin(b[i].a)) / 2 - b[i].obraz_rot->w / 2,
ey(b[i].h * cos(b[i].a)) / 2 - b[i].obraz_rot->h / 2,
b[i].obraz_rot->w, b[i].obraz_rot->h);
}
}
/*!
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 ;
}
/*!
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 ;
}
void draw_all_stations(station *stacje, int n, BITMAP *bufor)
{
for (int i = 0; i < n; i++)
{
if (!stacje[i].zywa)
continue;
clear_to_color(stacje[i].budynek_rot, makecol(255, 0, 255));
rotate_sprite(stacje[i].budynek_rot, stacje[i].budynek,
(stacje[i].budynek_rot->w - stacje[i].budynek->w) / 2,
(stacje[i].budynek_rot->h - stacje[i].budynek->h) / 2,
itofix((int) (256.0 * stacje[i].a / (2.0 * pi))));
masked_blit(stacje[i].budynek_rot, bufor, 0, 0,
ex(stacje[i].h * sin(stacje[i].a)) / 2
- stacje[i].budynek_rot->w / 2,
ey(stacje[i].h * cos(stacje[i].a)) / 2
- stacje[i].budynek_rot->h / 2,
stacje[i].budynek_rot->w, stacje[i].budynek_rot->h);
}
}
void ScriptWidget::timerEvent(QTimerEvent *)
{
QPointF player = m_scene->camera().pos();
QPointF entity = m_entity->pos();
QScriptValue px(m_engine, player.x());
QScriptValue py(m_engine, player.y());
QScriptValue ex(m_engine, entity.x());
QScriptValue ey(m_engine, entity.y());
QScriptValue time(m_engine, m_time.elapsed());
m_engine->globalObject().setProperty("player_x", px);
m_engine->globalObject().setProperty("player_y", py);
m_engine->globalObject().setProperty("my_x", ex);
m_engine->globalObject().setProperty("my_y", ey);
m_engine->globalObject().setProperty("time", time);
m_engine->evaluate(m_source);
if (m_engine->hasUncaughtException()) {
QString text = m_engine->uncaughtException().toString();
m_statusView->setText(text);
}
}
/*!
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 ;
}
glm::mat4
RGBDSensor::guess_eye_d_to_world(const ChessboardSampling& cbs, const Checkerboard& cb){
// find slowest ChessboardPose
const double time = cbs.searchSlowestTime(cbs.searchStartIR());
bool valid_pose;
glm::mat4 chessboard_pose = cb.pose_offset * cbs.interpolatePose(time,valid_pose);
if(!valid_pose){
std::cerr << "ERROR: could not interpolate valid pose" << std::endl;
exit(0);
}
// find pose of that board in kinect camera space
bool valid_ir;
ChessboardViewIR cbvir(cbs.interpolateIR(time, valid_ir));
if(!valid_ir){
std::cerr << "ERROR: could not interpolate valid ChessboardIR" << std::endl;
exit(0);
}
std::vector<glm::vec3> exs;
std::vector<glm::vec3> eys;
const float u = cbvir.corners[0].x;
const float v = cbvir.corners[0].y;
const float d = cbvir.corners[0].z;
std::cerr << "calc origin at " << u << ", " << v << ", " << d << std::endl;
glm::vec3 origin = calc_pos_d(u,v,d);
for(unsigned i = 1; i < (CB_WIDTH * CB_HEIGHT); ++i){
const float u = cbvir.corners[i].x;
const float v = cbvir.corners[i].y;
const float d = cbvir.corners[i].z;
glm::vec3 corner = calc_pos_d(u,v,d);
if(i < CB_WIDTH){
eys.push_back(glm::normalize(corner - origin));
}
else if(i % CB_WIDTH == 0){
exs.push_back(glm::normalize(corner - origin));
}
}
glm::vec3 ex(calcMean(exs));
glm::vec3 ey(calcMean(eys));
ex = glm::normalize(ex);
ey = glm::normalize(ey);
glm::vec3 ez = glm::cross(ex,ey);
ey = glm::cross(ez,ex);
std::cerr << "origin: " << origin << std::endl;
std::cerr << "ex: " << ex << std::endl;
std::cerr << "ey: " << ey << std::endl;
std::cerr << "ez: " << ez << std::endl;
glm::mat4 eye_d_to_world;
eye_d_to_world[0][0] = ex[0];
eye_d_to_world[0][1] = ex[1];
eye_d_to_world[0][2] = ex[2];
eye_d_to_world[1][0] = ey[0];
eye_d_to_world[1][1] = ey[1];
eye_d_to_world[1][2] = ey[2];
eye_d_to_world[2][0] = ez[0];
eye_d_to_world[2][1] = ez[1];
eye_d_to_world[2][2] = ez[2];
eye_d_to_world[3][0] = origin[0];
eye_d_to_world[3][1] = origin[1];
eye_d_to_world[3][2] = origin[2];
eye_d_to_world = glm::inverse(eye_d_to_world);
return chessboard_pose * eye_d_to_world;
}
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();
}
glm::mat4
RGBDSensor::guess_eye_d_to_world_static(const ChessboardSampling& cbs, const Checkerboard& cb){
if(cbs.getIRs().empty() || cbs.getPoses().empty()){
std::cerr << "ERROR in RGBDSensor::guess_eye_d_to_world_static: no Chessboard found" << std::endl;
exit(0);
}
glm::mat4 chessboard_pose = cb.pose_offset * cbs.getPoses()[0].mat;
ChessboardViewIR cbvir(cbs.getIRs()[0]);
std::vector<glm::vec3> exs;
std::vector<glm::vec3> eys;
const float u = cbvir.corners[0].x;
const float v = cbvir.corners[0].y;
const float d = cbvir.corners[0].z;
std::cerr << "calc origin at " << u << ", " << v << ", " << d << std::endl;
glm::vec3 origin = calc_pos_d(u,v,d);
for(unsigned i = 1; i < (CB_WIDTH * CB_HEIGHT); ++i){
const float u = cbvir.corners[i].x;
const float v = cbvir.corners[i].y;
const float d = cbvir.corners[i].z;
glm::vec3 corner = calc_pos_d(u,v,d);
if(i < CB_WIDTH){
eys.push_back(glm::normalize(corner - origin));
}
else if(i % CB_WIDTH == 0){
exs.push_back(glm::normalize(corner - origin));
}
}
glm::vec3 ex(calcMean(exs));
glm::vec3 ey(calcMean(eys));
ex = glm::normalize(ex);
ey = glm::normalize(ey);
glm::vec3 ez = glm::cross(ex,ey);
ey = glm::cross(ez,ex);
std::cerr << "origin: " << origin << std::endl;
std::cerr << "ex: " << ex << std::endl;
std::cerr << "ey: " << ey << std::endl;
std::cerr << "ez: " << ez << std::endl;
glm::mat4 eye_d_to_world;
eye_d_to_world[0][0] = ex[0];
eye_d_to_world[0][1] = ex[1];
eye_d_to_world[0][2] = ex[2];
eye_d_to_world[1][0] = ey[0];
eye_d_to_world[1][1] = ey[1];
eye_d_to_world[1][2] = ey[2];
eye_d_to_world[2][0] = ez[0];
eye_d_to_world[2][1] = ez[1];
eye_d_to_world[2][2] = ez[2];
eye_d_to_world[3][0] = origin[0];
eye_d_to_world[3][1] = origin[1];
eye_d_to_world[3][2] = origin[2];
eye_d_to_world = glm::inverse(eye_d_to_world);
return chessboard_pose * eye_d_to_world;
}
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 ) );
}
}
void Filter::Kalman(Joint joint, double &dx, double &dy)
{
Kalmans[Kalman_count++] = joint;
Kalman_count = Kalman_count % Kalman_limit;
Kalman_num++;
if (Kalman_num > Kalman_limit)
{
Kalman_num = Kalman_limit;
}
if (Kalman_num < Kalman_limit)
{
dx = joint.Position.X;
dy = joint.Position.Y;
return;
}
else
{
//X, Y
int haha;
haha = 1;
double x[5], y[5];
int pos = Kalman_count;
for (int i = 0; i < 5; i++)
{
x[i] = Kalmans[pos].Position.X;
y[i] = Kalmans[pos].Position.Y;
pos++;
pos = pos%Kalman_limit;
}
//求系数Ax, Ay
double Ax[5] = {
/*a0*/ x[0],
/*a1*/ 4 * (x[1] - x[0]) - 3 * x[2] + 4 * x[3] / 3 - x[4] / 4,
/*a2*/ 11 * x[4] / 24 - 7 * x[3] / 3 + 19 * x[2] / 4 - 13 * (x[1] - x[0]) / 3,
/*a3*/ x[4] / 3 - 7 * x[3] / 6 + x[2] - (x[1] - x[0]) / 2,
/*a4*/ (x[4] - 4 * x[3] + 6 * x[2] + 4 * (x[1] - x[0])) / 24
};
double Ay[5] = {
/*a0*/ y[0],
/*a1*/ 4 * (y[1] - y[0]) - 3 * y[2] + 4 * y[3] / 3 - y[4] / 4,
/*a2*/ 11 * y[4] / 24 - 7 * y[3] / 3 + 19 * y[2] / 4 - 13 * (y[1] - y[0]) / 3,
/*a3*/ y[4] / 3 - 7 * y[3] / 6 + y[2] - (y[1] - y[0]) / 2,
/*a4*/ (y[4] - 4 * y[3] + 6 * y[2] + 4 * (y[1] - y[0])) / 24
};
//求转换矩阵Fx, Fy
Matrix Fx(4, 4,
new double[16]{
1, 1, -0.5, (Ax[1] + 6 * Ax[3] - 4 * Ax[4]) / (24 * Ax[4]),
0, 1, 1, 0.5,
0, 0, 1, 1,
0, 0, 0, 1});
Matrix Fy(4, 4,
new double[16]{
1, 1, -0.5, (Ay[1] + 6 * Ay[3] - 4 * Ay[4]) / (24 * Ay[4]),
0, 1, 1, 0.5,
0, 0, 1, 1,
0, 0, 0, 1});
//求ε(t|t-1)
Matrix ex(4, 4), ey(4, 4);
ex = Fx*Kalman_ex*(!Fx);
ey = Fy*Kalman_ey*(!Fy);
//cout << "ex" << endl; ex.print();
//cout << "ey" << endl; ey.print();
Matrix Bx(4, 1), By(4, 1);
//cout << "!Kalman_C" << endl; (!Kalman_C).print();
//cout << "Kalman_vx" << endl; Kalman_vx.print();
//cout << "Kalman_C" << endl; Kalman_C.print();
//cout << "!Kalman_C" << endl; (!Kalman_C).print();
//cout << "Kalman_C*ex" << endl; (Kalman_C*ex).print();
//cout << "Kalman_C*ex*(!Kalman_C)" << endl; (Kalman_C*ex*(!Kalman_C)).print();
//cout << "(~(Kalman_vx + Kalman_C*ex*(!Kalman_C)))" << endl;
//(~(Kalman_vx + Kalman_C*ex*(!Kalman_C))).print();
Bx = ex*(!Kalman_C)*(~(Kalman_vx + Kalman_C*ex*(!Kalman_C)));
//cout << "Bx" << endl; Bx.print();
By = ey*(!Kalman_C)*(~(Kalman_vy + Kalman_C*ey*(!Kalman_C)));
Matrix I4(4, 4);
I4.SetIdentity();
Kalman_Sx = (I4 - Bx*Kalman_C)*(Fx*Kalman_Sx + Kalman_Gx) +
Bx*Matrix(1, 1, new double[1] {joint.Position.X});
//cout << "Kalman_Sx" << endl; Kalman_Sx.print();
Kalman_Sy = (I4 - By*Kalman_C)*(Fy*Kalman_Sy + Kalman_Gy) +
By*Matrix(1, 1, new double[1] {joint.Position.Y});
Kalman_ex = ex - Bx*Kalman_C*ex;
Kalman_ey = ey - By*Kalman_C*ey;
dx = Kalman_Sx.at(0, 0);
dy = Kalman_Sy.at(0, 0);
}
}
