MinimalBoundingSphere::MinimalBoundingSphere(MathLib::Point3d const& p,
MathLib::Point3d const& q, MathLib::Point3d const& r)
{
MathLib::Vector3 const a(p,r);
MathLib::Vector3 const b(p,q);
MathLib::Vector3 const cross_ab(crossProduct(a,b));
if (cross_ab.getLength() > 0)
{
double const denom = 2.0 * scalarProduct(cross_ab,cross_ab);
MathLib::Vector3 const o = (scalarProduct(b,b) * crossProduct(cross_ab, a)
+ scalarProduct(a,a) * crossProduct(b, cross_ab))
* (1.0 / denom);
_radius = o.getLength() + std::numeric_limits<double>::epsilon();
_center = MathLib::Vector3(p) + o;
}
else
{
MinimalBoundingSphere two_pnts_sphere;
if (a.getLength() > b.getLength())
two_pnts_sphere = MinimalBoundingSphere(p,r);
else
two_pnts_sphere = MinimalBoundingSphere(p,q);
_radius = two_pnts_sphere.getRadius();
_center = two_pnts_sphere.getCenter();
}
}
MinimalBoundingSphere::MinimalBoundingSphere(
MathLib::Point3d const& p, MathLib::Point3d const& q)
: _radius(std::numeric_limits<double>::epsilon()), _center(p)
{
MathLib::Vector3 const a(p, q);
if (a.getLength() > 0)
{
MathLib::Vector3 const o(0.5*a);
_radius = o.getLength() + std::numeric_limits<double>::epsilon();
_center = MathLib::Vector3(p) + o;
}
}
double sKinematics::CalAngle(MathLib::Vector3 vBase, MathLib::Vector3 vMeasure)
{
double theta;
MathLib::Vector3 vOrtho;
MathLib::Vector3 vDir;
theta = acos( vBase.Dot(vMeasure) / (vBase.Norm()*vMeasure.Norm()) );
vOrtho = vBase.Cross(vMeasure);
vOrtho.Normalize();
vDir = vOrtho.Cross(vBase);
if( vDir.Dot(vMeasure) > 0 ) return theta;
else return -theta;
}
MinimalBoundingSphere::MinimalBoundingSphere(MathLib::Point3d const& p,
MathLib::Point3d const& q,
MathLib::Point3d const& r,
MathLib::Point3d const& s)
{
MathLib::Vector3 const a(p, q);
MathLib::Vector3 const b(p, r);
MathLib::Vector3 const c(p, s);
if (!GeoLib::isCoplanar(p, q, r, s))
{
double const denom = 2.0 * GeoLib::scalarTriple(a,b,c);
MathLib::Vector3 const o = (scalarProduct(c,c) * crossProduct(a,b)
+ scalarProduct(b,b) * crossProduct(c,a)
+ scalarProduct(a,a) * crossProduct(b,c))
* (1.0 / denom);
_radius = o.getLength() + std::numeric_limits<double>::epsilon();
_center = MathLib::Vector3(p) + o;
}
else
{
MinimalBoundingSphere const pqr(p, q , r);
MinimalBoundingSphere const pqs(p, q , s);
MinimalBoundingSphere const prs(p, r , s);
MinimalBoundingSphere const qrs(q, r , s);
_radius = pqr.getRadius();
_center = pqr.getCenter();
if (_radius < pqs.getRadius())
{
_radius = pqs.getRadius();
_center = pqs.getCenter();
}
if (_radius < prs.getRadius())
{
_radius = prs.getRadius();
_center = prs.getCenter();
}
if (_radius < qrs.getRadius())
{
_radius = qrs.getRadius();
_center = qrs.getCenter();
}
}
}
void MeshSurfaceExtraction::get2DSurfaceElements(const std::vector<MeshLib::Element*> &all_elements, std::vector<MeshLib::Element*> &sfc_elements, const MathLib::Vector3 &dir, double angle, unsigned mesh_dimension)
{
if (mesh_dimension<2 || mesh_dimension>3)
ERR("Cannot handle meshes of dimension %i", mesh_dimension);
bool const complete_surface = (MathLib::scalarProduct(dir, dir) == 0);
double const pi (boost::math::constants::pi<double>());
double const cos_theta (std::cos(angle * pi / 180.0));
MathLib::Vector3 const norm_dir (dir.getNormalizedVector());
for (auto elem = all_elements.cbegin(); elem != all_elements.cend(); ++elem)
{
const unsigned element_dimension ((*elem)->getDimension());
if (element_dimension < mesh_dimension)
continue;
if (element_dimension == 2)
{
if (!complete_surface)
{
MeshLib::Element* face = *elem;
if (MathLib::scalarProduct(FaceRule::getSurfaceNormal(face).getNormalizedVector(), norm_dir) > cos_theta)
continue;
}
sfc_elements.push_back(*elem);
}
else
{
if (!(*elem)->isBoundaryElement())
continue;
const unsigned nFaces ((*elem)->getNFaces());
for (unsigned j=0; j<nFaces; ++j)
{
if ((*elem)->getNeighbor(j) != nullptr)
continue;
auto const face = std::unique_ptr<MeshLib::Element const>{(*elem)->getFace(j)};
if (!complete_surface)
{
if (MathLib::scalarProduct(FaceRule::getSurfaceNormal(face.get()).getNormalizedVector(), norm_dir) < cos_theta)
{
continue;
}
}
if (face->getGeomType() == MeshElemType::TRIANGLE)
sfc_elements.push_back(new MeshLib::Tri(*static_cast<const MeshLib::Tri*>(face.get())));
else
sfc_elements.push_back(new MeshLib::Quad(*static_cast<const MeshLib::Quad*>(face.get())));
}
}
}
}
void HandSkinControllerThread::PredictPoseFromAngle(){
// Apply the model and rock the field
bool bdm = bDebugMode;
bDebugMode = false;
if(bDebugMode) cout << "Predicting pose from angle *********"<<endl;
// Count the number of valid contact
// Reliability measure for each fingertip
double validFTip[FINGERS_COUNT];
int validFTipCount = 0;
if(bNoMissingCheck){
// If we don't care, let's move on
for(int i=0;i<FINGERS_COUNT;i++){
validFTip[i] = 1.0;
}
validFTipCount = FINGERS_COUNT;
}else{
for(int i=0;i<FINGERS_COUNT;i++){
// Higher than min threshold?
if(mSFingerTip[i][0] >= FTIPTHRESHOLD){
validFTipCount++;
// Higher than max threshold?
if(mSFingerTip[i][0] >= FTIPTHRESHOLDMAX){
validFTip[i] = 1.0;
}else{
// Save validity between 0 and 1
validFTip[i] = (mSFingerTip[i][0]-FTIPTHRESHOLD)/(FTIPTHRESHOLDMAX-FTIPTHRESHOLD);
}
}else{
validFTip[i] = 0.0;
}
}
}
// Additional input weight matrix using the realiability measure
MathLib::Matrix inWeights(validFTipCount*3,validFTipCount*3);
inWeights.Zero();
int cnt = 0;
for(int i=0;i<FINGERS_COUNT;i++){
if(validFTip[i]>0){
inWeights(cnt*3+0,i*3+0) = validFTip[i];
inWeights(cnt*3+1,i*3+1) = validFTip[i];
inWeights(cnt*3+2,i*3+2) = validFTip[i];
cnt++;
}
}
// Some variables
MathLib::Vector inComp(validFTipCount*3);
MathLib::Vector outComp(FINGERS_COUNT*2+1+FINGERS_COUNT*1);
MathLib::Vector inV(validFTipCount*3);
MathLib::Vector outV(FINGERS_COUNT*2+1+FINGERS_COUNT*1); outV.Zero();
MathLib::Matrix sigma(FINGERS_COUNT*2+1+FINGERS_COUNT*1,FINGERS_COUNT*2+1+FINGERS_COUNT*1);
// Output components indices of the gmm
for(int i=0;i<outComp.Size();i++)
outComp(i) = i;
// Input components indices of the gmm
cnt = 0;
for(int i=0;i<FINGERS_COUNT;i++){
if(validFTip[i]>0){
inComp(cnt++) = outComp.Size()+i*3+0;
inComp(cnt++) = outComp.Size()+i*3+1;
inComp(cnt++) = outComp.Size()+i*3+2;
}
}
if(bDebugMode){ cout <<"Vld: "; for(int i=0;i<FINGERS_COUNT;i++) cout << validFTip[i]<<" "; cout<<endl;}
if(bDebugMode) cout <<"InC: " << inComp<<endl;
// Filling the input vector with sensor data
cnt = 0;
for(int i=0;i<FINGERS_COUNT;i++){
if(validFTip[i]>0){
inV(cnt++) = mSFingerTipDir[i][0];
inV(cnt++) = mSFingerTipDir[i][1];
inV(cnt++) = mSFingerTipDir[i][2];
}
}
if(bDebugMode) cout <<"InV: "<< inV<<endl;
MathLib::Vector grad,gradNoExp;
MathLib::Vector inV2,inV0;
inV2 = inV;
inV0 = inV;
if(validFTipCount>0){
// We have at least one valid input
if(bGradientAscent){
// We enjoy gradient ascent
// Get the probability that input query point is in the model
double val = mGMModel.GetPx(inV2,inComp,&inWeights);
// Iterate until we're happy
int iter = 0;
int iterNoExp = 0;
while(1){
// Get the gradient of current point
val = mGMModel.GetGradX(inV2,inComp,grad,&inWeights,&gradNoExp);
// Get the membership of the current input query point,
// if higher than threshold we're done
if(val >= exp(-0.5*2.5*2.5))
break;
// Norm of the gradient
double norm = grad.Norm();
if(val<1e-12){
// Too small? use another safer version in gradNoExp
grad = gradNoExp;
norm = gradNoExp.Norm();
norm = 1.0/norm;
iterNoExp++;
if(!isnormal(norm)){
// Still a not - number... big failure... but we
// exit still...
iter = -1;
break;
}
}else{
// Big enough? Normalize
norm = 1.0/norm;
}
// Normalize
grad *= (-norm);
// Scale the gradient to a little step size and add it
// to the query point
grad.ScaleAddTo(0.01,inV2);
// Normalization of the query point (i.e. the contact normals
// should be better unit norm
int cnt2 = 0;
for(int i=0;i<FINGERS_COUNT;i++){
if(validFTip[i]>0){
MathLib::Vector3 vn;
vn(0) = inV2(cnt2+0);
vn(1) = inV2(cnt2+1);
vn(2) = inV2(cnt2+2);
vn.Normalize();
inV2(cnt2+0) = vn(0);
inV2(cnt2+1) = vn(1);
inV2(cnt2+2) = vn(2);
cnt2 += 3;
}
}
iter++;
if(iter>300){
// Too much iteration.... again bg failure, exit gracefully
break;
}
}
if(bDebugMode) cout << " Gradient Descent: # iterations : "<<iter<< "("<<iterNoExp<<")"<<endl;
if((iter<0)||(iter>300)){
mSFingerDesCoupling.Identity();
if(bDebugMode){
cout << " Failed: Membership Value (threshold: "<<exp(-0.5*2.5*2.5)<<") -> "<<val <<endl;
cout << endl<<endl;
}
bDebugMode= bdm;
return;
}
}
}
if(bDebugMode) cout <<"Input: "<< inV<<endl;
// Finally do the regression :)
inV = inV2;
double res = mGMModel.doRegression( inV , inComp, outComp, outV, sigma,&inWeights);
if(bDebugMode) cout << "Confidence: "<<res<<endl;
if(bDebugMode) cout << "Output: "<<outV<<endl;
// Desired position
for(int i=0;i<FINGERS_COUNT;i++){
mSFingerDesPos[i][0] = outV(1+i*2+0);// / mPosFactor;
mSFingerDesPos[i][1] = outV(1+i*2+1);// / mPosFactor;
}
// Desired pressure
for(int i=0;i<FINGERS_COUNT;i++){
mSFingerDesTip[i][0] = outV(1+FINGERS_COUNT*2+i)+2.0;
// Tweaking the output sigma matrice (identiting the pressure part,
// and keeping the position part only (for the force-position control
// switch))
sigma.SetRow(0.0,1+FINGERS_COUNT*2+i);
sigma.SetColumn(0.0,1+FINGERS_COUNT*2+i);
sigma(1+FINGERS_COUNT*2+i,1+FINGERS_COUNT*2+i) = 1.0;
}
// Inverting the sigma matrix
MathLib::Matrix nsigma(sigma);
nsigma.Identity();
nsigma *= 10.0; // A bit more power in the diagonal
nsigma += sigma;
// Inverse and store it for later use
nsigma.InverseSymmetric(mSFingerDesCoupling);
// More coping fo data here and there
mJointsTargetPredPos = mJointsRest;
for(int i=mJointsFingerOffset+1;i<mJointsSize-1;i++){
mJointsTargetPredPos(i) = outV(i-mJointsFingerOffset-1);
}
bDebugMode= bdm;
}
