void algDirICP_IMLOP::ICP_UpdateParameters_PostRegister(vctFrm3 &Freg)
{
// base class
algDirICP::ICP_UpdateParameters_PostRegister(Freg);
double sumSqrDist_PostRegister = 0.0;
double sumNormProducts_PostRegister = 0.0;
for (unsigned int s = 0; s < nSamples; s++)
{
sumSqrDist_PostRegister +=
(samplePtsXfmd.Element(s) - matchPts.Element(s)).NormSquare();
sumNormProducts_PostRegister +=
vctDotProduct(sampleNormsXfmd.Element(s), matchNorms.Element(s));
}
//matchDistSD_PostRegister =
// (sumSqrDist_PostRegister/nSamples) - matchDistAvg_PostRegister*matchDistAvg_PostRegister;
//matchDegErrSD_PostRegister =
// (sumSqrDegErr_PostRegister / nSamples) - matchDegErrAvg_PostRegister*matchDegErrAvg_PostRegister;
// update noise model
UpdateNoiseModel(sumSqrDist_PostRegister, sumNormProducts_PostRegister);
// update monitoring variables
errFuncNormWeight = k;
errFuncPosWeight = B;
}
double alg2D_DirPDTree_vonMises_Edges::FindClosestPointOnDatum(
const vct2 &v, const vct2 &n,
vct2 &closest, vct2 &closestNorm,
int datum)
{
// cost: k*(1-N'*Nclosest) + ||v - closest||^2 / (2*sigma2)
// Project point onto the edge
// (storing lambda value in temp buffer)
closest = pDirTree->GetEdge(datum).ProjectOnEdge(v); //, &searchLambdas.Element(datum));
closestNorm = pDirTree->GetEdge(datum).Norm;
// Is this a permitted match?
double dTheta = acos(vctDotProduct(n, closestNorm));
if (dTheta > dThetaMax)
{
return std::numeric_limits<double>::max();
}
else
{
bPermittedMatchFound = true;
return k*(1.0 - n*closestNorm) + (v - closest).NormSquare() / (2.0*sigma2);
}
// This doesn't work, i.e. we can't keep returning the best match among the non-permitted
// matches because then when a permitted match finally comes along, then it may have
// higher error, which would prevent it from being chosen. The only way to accomplish the
// above is to modify the core PD tree search routine, which I don't want to do.
// => only return match errors for permitted matches.
//// is this a permitted match?
//double matchError = k*(1.0 - n*closestNorm) + (v - closest).NormSquare() / (2.0*sigma2);
//double dTheta = acos(vctDotProduct(n, closestNorm));
//if (dTheta > dThetaMax)
//{
// if (bPermittedMatchFound)
// { // skip this match as long as some other permitted match has been already been found
// // do this by returning an astronomical match error
// matchError = std::numeric_limits<double>::max();
// }
//}
//else
//{
// bPermittedMatchFound = true;
//}
//return matchError;
}
// finds the point on this datum with lowest match error
// and returns the match error and closest point
// Note: negative match error is a possibility
double cisstAlgorithmICP_IMLP_PointCloud::FindClosestPointOnDatum(
const vct3 &v,
vct3 &closest,
int datum)
{
// first iteration variables that don't change
static const vct3x3 I_5(vct3x3::Eye()*0.5); // 0.5*I
// Datum is only a single point
static vct3 d;
static vct3x3 M, Minv;
double det_M;
if (bFirstIter_Matches)
{ // use isotropic noise model for first iteration
// M = Mx + NodeEigMax*I = 2*I = V*S*V' => S = 2*I, V = I
// Minv = N'*N = I*(1/2)*I => N = sqrt(1/2)*I = 0.7071*I
//M = I2; // actually isn't needed
Minv = I_5;
det_M = 8;
}
else
{ // Use noise model of node
// compute noise model for this datum
// M = R*Mxi*R' + Myi
M = sample_RMxRt_sigma2 + pTree->DatumCov(datum);
ComputeCovDecomposition_NonIter(M, Minv, det_M);
}
// return match error
// log term plus square Mahalanobis distance
closest = pTree->points.Element(datum);
d = (v - closest);
return log(det_M) + vctDotProduct(d, Minv*d);
}
robManipulator::Errno mtsIntuitiveResearchKitECM::InverseKinematics(vctDoubleVec & jointSet,
const vctFrm4x4 & cartesianGoal)
{
// re-align desired frame to 4 axis direction to reduce free space
vctDouble3 shaft = cartesianGoal.Translation();
shaft.NormalizedSelf();
const vctDouble3 z = cartesianGoal.Rotation().Column(2).Ref<3>(); // last column of rotation matrix
vctDouble3 axis;
axis.CrossProductOf(z, shaft);
const double angle = acos(vctDotProduct(z, shaft));
const vctMatRot3 reAlign(vctAxAnRot3(axis, angle, VCT_NORMALIZE));
vctFrm4x4 newGoal;
newGoal.Translation().Assign(cartesianGoal.Translation());
newGoal.Rotation().ProductOf(reAlign, cartesianGoal.Rotation());
if (Manipulator.InverseKinematics(jointSet, newGoal) == robManipulator::ESUCCESS) {
// find closest solution mod 2 pi
const double difference = JointGet[3] - jointSet[3];
const double differenceInTurns = nearbyint(difference / (2.0 * cmnPI));
jointSet[3] = jointSet[3] + differenceInTurns * 2.0 * cmnPI;
// make sure we are away from RCM point, this test is
// simplistic
if (jointSet[2] < 40.0 * cmn_mm) {
jointSet[2] = 40.0 * cmn_mm;
}
#if 0
vctFrm4x4 forward = Manipulator.ForwardKinematics(jointSet);
vctDouble3 diff;
diff.DifferenceOf(forward.Translation(), newGoal.Translation());
std::cerr << cmnInternalTo_mm(diff.Norm()) << "mm ";
#endif
return robManipulator::ESUCCESS;
}
return robManipulator::EFAILURE;
}
// Determine if the given parallelpiped face intersects a sphere
// of given radius centered at the origin
// n - face normal (not a unit normal)
// v0..v3 - face vertices
// sqrRadius - square radius of sphere
//
// n0..n3 - in-plane edge normals (removed)
//
// Note: vertex ordering is arranged s.t. face edges are numbered as
// below and egdes follow a CCW sequencing about the face normal.
// e0 - [v0,v1]
// e1 - [v1,v2]
// e2 - [v2,v3]
// e3 - [v3,v0]
//
bool Ellipsoid_OBB_Intersection_Solver::IntersectionSphereFace(
const vct3 &n,
const vct3 &v0, const vct3 &v1,
const vct3 &v2, const vct3 &v3,
double radius, double sqrRadius)
{
// static variables for run-time efficiency
static int vsblEdges[2];
static int numVsblEdges, numProcessedEdges;
static double q_signed_mag;
static vct3 q;
static unsigned int maxEl;
static double sqrDist;
//static double radius;
static double n_0,n_1,n_2,nmax;
// Quick escape
// check distance from origin to face plane
//radius = sqrt(sqrRadius); // TODO: more efficient to send this as an argument
q_signed_mag = vctDotProduct(v0,n); // signed distance to plane
if (fabs(q_signed_mag) > radius)
{ // sphere does not intersect face
return false;
}
q = q_signed_mag*n; // projection of origin onto face plane
//std::cout << "q = [" << q << "]" << std::endl;
//std::cout << "v(1,:) = [" << v0 << "]" << std::endl;
//std::cout << "v(2,:) = [" << v1 << "]" << std::endl;
//std::cout << "v(3,:) = [" << v2 << "]" << std::endl;
//std::cout << "v(4,:) = [" << v3 << "]" << std::endl;
#if 1
// Determine edges visible from q (at most 2)
// by projecting face to the closest axis-aligned plane
// (determined by largest element of n)
n_0 = fabs(n[0]);
n_1 = fabs(n[1]);
n_2 = fabs(n[2]);
if (n_0 > n_1)
{
nmax = n_0;
maxEl = 0;
}
else
{
nmax = n_1;
maxEl = 1;
}
if (n_2 > nmax)
{
maxEl = 2;
}
switch (maxEl)
{
case 0:
{ // project to y-z plane
// q0 <-- q[1]
// q1 <-- q[2]
numVsblEdges = FindVisibleEdges( q[1],q[2],
v0[1],v0[2],v1[1],v1[2],v2[1],v2[2],v3[1],v3[2],
//n0[1],n0[2],n1[1],n1[2],n2[1],n2[2],n3[1],n3[2],
vsblEdges, (n[0]>0) );
break;
}
case 1:
{ // project to z-x plane
// q0 <-- q[2]
// q1 <-- q[0]
numVsblEdges = FindVisibleEdges( q[2],q[0],
v0[2],v0[0],v1[2],v1[0],v2[2],v2[0],v3[2],v3[0],
vsblEdges, (n[1]>0) );
break;
}
case 2:
{ // project to x-y plane
// q0 <-- q[0]
// q1 <-- q[1]
numVsblEdges = FindVisibleEdges( q[0],q[1],
v0[0],v0[1],v1[0],v1[1],v2[0],v2[1],v3[0],v3[1],
vsblEdges, (n[2]>0) );
break;
}
default:
std::cout << "Code Error!" << std::endl;
assert(0);
}
if (numVsblEdges == 0)
{ // no edges visible from origin projection => projection lies within
// face => positive intersection
// Note: distance to plane is already known to be < sphere radius
return true;
}
// Compute distance from origin to each visible edge
// Note: the code below assumes visible edges have been tested
// and added to visible edge array in order e0,e2,e1,e3
numProcessedEdges = 0;
if ( vsblEdges[numProcessedEdges] == 0 ) // edge 0
{
// compute distance to edge 0
if ( SquareDistanceToEdge( v0,v1 ) < sqrRadius )
{ // positive intersection
return true;
}
// go to next visible edge
numProcessedEdges++;
if (numProcessedEdges == numVsblEdges)
{ // no intersection
return false;
}
}
if ( vsblEdges[numProcessedEdges] == 2 ) // edge 2
{
// compute distance to edge 2
if ( SquareDistanceToEdge( v2,v3 ) < sqrRadius )
{ // positive intersection
return true;
}
// go to next visible edge
numProcessedEdges++;
if (numProcessedEdges == numVsblEdges)
{ // no intersection
return false;
}
}
if ( vsblEdges[numProcessedEdges] == 1 ) // edge 1
{
// compute distance to edge 1
if ( SquareDistanceToEdge( v1,v2 ) < sqrRadius )
{ // positive intersection
return true;
}
// go to next visible edge
numProcessedEdges++;
if (numProcessedEdges == numVsblEdges)
{ // no intersection
return false;
}
}
if ( vsblEdges[numProcessedEdges] == 3 ) // edge 3
{
// compute distance to edge 3
if ( SquareDistanceToEdge( v3,v0 ) < sqrRadius )
{ // positive intersection
return true;
}
// go to next visible edge
numProcessedEdges++;
if (numProcessedEdges == numVsblEdges)
{ // no intersection
return false;
}
}
// should never reach here
std::cout << "Code Error!" << std::endl;
assert(0);
return false;
#else
// This code is a modification and simplification of the visible edge finding
// method; quite unexpectedly, it runs consistently slower than the standard
// methodd, thus it is not used.
// (Increases a typical registration runtime from 5.7 to 6.7 seconds!)
// Note: the undefined variables in this section were presented to this
// function as argument variables when this variation is used.
// Determine edge visibility by transforming q partially back to node coordinates.
// Visibility check in this space is trivial since edges are perpendicular and
// axis aligned; this method is more efficient because it does not
// require computing a new projection for the four corners of the
// parallelegram and it also does not require computing the edge normals
// following the projection, since the edges are known to be perpendicular
// in the node coordinate space.
// Note: it appears this multiplication takes longer than all the extra
// manipulations performed in the standard method.
static vct3 qnode;
qnode = Anode_sphere*q + Tnode_sphere;
numVsblEdges = 0;
//std::cout << "MinCorner = [" << OBB.MinCorner << "]" << std::endl;
//std::cout << "MaxCorner = [" << OBB.MaxCorner << "]" << std::endl;
//std::cout << "vnode(1,:) = [" << Anode_sphere*v0 + Tnode_sphere << "]" << std::endl;
//std::cout << "vnode(2,:) = [" << Anode_sphere*v1 + Tnode_sphere << "]" << std::endl;
//std::cout << "vnode(3,:) = [" << Anode_sphere*v2 + Tnode_sphere << "]" << std::endl;
//std::cout << "vnode(4,:) = [" << Anode_sphere*v3 + Tnode_sphere << "]" << std::endl;
//std::cout << "qnode = [" << qnode << "]" << std::endl;
switch( FaceNumber )
{
case 1: // X+ face
{
if (qnode[2] > OBB.MaxCorner[2]) // upper edge
{ // edge 0 visible
// compute distance to edge
if ( SquareDistanceToEdge( v0,v1 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
else if (qnode[2] < OBB.MinCorner[2]) // lower edge
{ // edge 2 visible
// compute distance to edge
if ( SquareDistanceToEdge( v2,v3 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
if (qnode[1] < OBB.MinCorner[1]) // left edge
{ // edge 1 visible
// compute distance to edge
if ( SquareDistanceToEdge( v1,v2 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
else if (qnode[1] > OBB.MaxCorner[1]) // right edge
{ // edge 3 visible
// compute distance to edge
if ( SquareDistanceToEdge( v3,v0 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
break;
}
case 2: // X- face
{
if (qnode[2] > OBB.MaxCorner[2]) // upper edge
{ // edge 0 visible
// compute distance to edge
if ( SquareDistanceToEdge( v0,v1 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
else if (qnode[2] < OBB.MinCorner[2]) // lower edge
{ // edge 2 visible
// compute distance to edge
if ( SquareDistanceToEdge( v2,v3 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
if (qnode[1] > OBB.MaxCorner[1]) // left edge
{ // edge 1 visible
// compute distance to edge
if ( SquareDistanceToEdge( v1,v2 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
else if (qnode[1] < OBB.MinCorner[1]) // right edge
{ // edge 3 visible
// compute distance to edge
if ( SquareDistanceToEdge( v3,v0 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
break;
}
case 3: // Y+ face
{
if (qnode[2] > OBB.MaxCorner[2]) // upper edge
{ // edge 0 visible
// compute distance to edge
if ( SquareDistanceToEdge( v0,v1 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
else if (qnode[2] < OBB.MinCorner[2]) // lower edge
{ // edge 2 visible
// compute distance to edge
if ( SquareDistanceToEdge( v2,v3 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
if (qnode[0] > OBB.MaxCorner[0]) // left edge
{ // edge 1 visible
// compute distance to edge
if ( SquareDistanceToEdge( v1,v2 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
else if (qnode[0] < OBB.MinCorner[0]) // right edge
{ // edge 3 visible
// compute distance to edge
if ( SquareDistanceToEdge( v3,v0 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
break;
}
case 4: // Y- face
{
if (qnode[2] > OBB.MaxCorner[2]) // upper edge
{ // edge 0 visible
// compute distance to edge
if ( SquareDistanceToEdge( v0,v1 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
else if (qnode[2] < OBB.MinCorner[2]) // lower edge
{ // edge 2 visible
// compute distance to edge
if ( SquareDistanceToEdge( v2,v3 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
if (qnode[0] < OBB.MinCorner[0]) // left edge
{ // edge 1 visible
// compute distance to edge
if ( SquareDistanceToEdge( v1,v2 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
else if (qnode[0] > OBB.MaxCorner[0]) // right edge
{ // edge 3 visible
// compute distance to edge
if ( SquareDistanceToEdge( v3,v0 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
break;
}
case 5: // Z+ face
{
if (qnode[0] < OBB.MinCorner[0]) // upper edge
{ // edge 0 visible
// compute distance to edge
if ( SquareDistanceToEdge( v0,v1 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
else if (qnode[0] > OBB.MaxCorner[0]) // lower edge
{ // edge 2 visible
// compute distance to edge
if ( SquareDistanceToEdge( v2,v3 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
if (qnode[1] < OBB.MinCorner[1]) // left edge
{ // edge 1 visible
// compute distance to edge
if ( SquareDistanceToEdge( v1,v2 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
else if (qnode[1] > OBB.MaxCorner[1]) // right edge
{ // edge 3 visible
// compute distance to edge
if ( SquareDistanceToEdge( v3,v0 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
break;
}
case 6: // Z- face
{
if (qnode[0] > OBB.MaxCorner[0]) // upper edge
{ // edge 0 visible
// compute distance to edge
if ( SquareDistanceToEdge( v0,v1 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
else if (qnode[0] < OBB.MinCorner[0]) // lower edge
{ // edge 2 visible
// compute distance to edge
if ( SquareDistanceToEdge( v2,v3 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
if (qnode[1] < OBB.MinCorner[1]) // left edge
{ // edge 1 visible
// compute distance to edge
if ( SquareDistanceToEdge( v1,v2 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
else if (qnode[1] > OBB.MaxCorner[1]) // right edge
{ // edge 3 visible
// compute distance to edge
if ( SquareDistanceToEdge( v3,v0 ) < sqrRadius )
{ // positive intersection
return true;
}
numVsblEdges++;
}
break;
}
}
if (numVsblEdges == 0)
{ // trivial intersection since no visible edges
return true;
}
else
{
// no intersection
return false;
}
#endif
}
void mexInterface_AlgPDTree_MLP_Mesh::ComputeMatches()
{
// Find the point on the model having lowest match error
// for each sample point
#ifdef ValidatePDTreeSearch
numInvalidDatums = 0;
numValidDatums = 0;
#endif
unsigned int nSamples = samplePtsXfmd.size();
unsigned int nodesSearched = 0;
minNodesSearched = std::numeric_limits<unsigned int>::max();
maxNodesSearched = std::numeric_limits<unsigned int>::min();
avgNodesSearched = 0;
vct3x3 dummyMat;
vct3 sampleCovEig;
for (unsigned int s = 0; s < nSamples; s++)
{
// inform algorithm beginning new match
//SamplePreMatch(s);
ComputeCovEigenDecomposition_NonIter(sampleCovXfmd[s], sampleCovEig, dummyMat);
pAlg->InitializeSampleSearch(sampleCovXfmd[s], sampleCovEig);
// Find best match for this sample
matchDatums.Element(s) = pTree->FindClosestDatum(
samplePtsXfmd.Element(s), matchPts.Element(s),
matchDatums.Element(s),
matchErrors.Element(s),
nodesSearched);
avgNodesSearched += nodesSearched;
minNodesSearched = (nodesSearched < minNodesSearched) ? nodesSearched : minNodesSearched;
maxNodesSearched = (nodesSearched > maxNodesSearched) ? nodesSearched : maxNodesSearched;
#ifdef ValidatePDTreeSearch
validDatum = pDirTree->ValidateClosestDatum(
samplePtsXfmd.Element(s), sampleNormsXfmd.Element(s),
validPoint, validNorm);
if (validDatum != matchDatums.Element(s))
{
// It is possible to have different datums for same point if the match
// lies on a datum edge; if this is the case, then the search didn't
// actually fail
// Note: cannot compare two double values for exact equality due to
// inexact representation of decimal values in binary arithmetic
searchError = (validPoint - matchPts.Element(s)).NormSquare();
if (searchError > doubleEps)
{
numInvalidDatums++;
//matchDist = (matchPts.Element(s)-samplePtsXfmd.Element(s)).Norm();
//matchAng = acos( vctDotProduct(matchNorms.Element(s),sampleNormsXfmd.Element(s)) );
validDist = (validPoint - samplePtsXfmd.Element(s)).Norm();
validAng = acos(vctDotProduct(validNorm, sampleNormsXfmd.Element(s)));
vct2 tmp1, tmp2;
//searchError = algorithm->FindClosestPointOnDatum(
// samplePtsXfmd.Element(s), sampleNormsXfmd.Element(s),
// tmp1, tmp2, matchDatums.Element(s));
validError = pDirTree->pAlgorithm->FindClosestPointOnDatum(
samplePtsXfmd.Element(s), sampleNormsXfmd.Element(s),
tmp1, tmp2, validDatum);
validFS << "Correspondence Search Data: (foundMatch / validMatch)" << std::endl
<< " MatchError = " << matchErrors.Element(s) << "/" << validError << std::endl
<< " dPos = " << dist_PostMatch.Element(s) << "/" << validDist << std::endl
<< " dAng = " << dTheta*180.0 / cmnPI << "/" << validAng*180.0 / cmnPI << std::endl
<< " XfmSamplePoint = [" << samplePtsXfmd.Element(s) << "]" << std::endl
<< " XfmSampleNorm = [" << sampleNormsXfmd.Element(s) << "]" << std::endl
<< " MatchPoint = [" << matchPts.Element(s) << "]" << std::endl
<< " MatchNorm = [" << matchNorms.Element(s) << "]" << std::endl
<< " MatchDatum = " << matchDatums.Element(s) << std::endl
<< " ValidPoint = [" << validPoint << "]" << std::endl
<< " ValidNorm = [" << validNorm << "]" << std::endl
<< " ValidDatum = " << validDatum << std::endl
<< " SampleIndex = " << s << std::endl;
//validFS << "Fact = [" << std::endl << Fact << "]" << std::endl;
DirPDTreeNode *termNode = 0;
pDirTree->FindTerminalNode(validDatum, &termNode);
if (!termNode)
{
std::cout << "ERROR: did not find terminal node for datum: " << validDatum << std::endl;
assert(0);
}
validFS << " Valid Terminal Node:" << std::endl;
validFS << " MinCorner: " << termNode->Bounds.MinCorner << std::endl;
validFS << " MaxCorner: " << termNode->Bounds.MaxCorner << std::endl;
validFS << " NData: " << termNode->NData << std::endl;
validFS << "Fnode_valid = [" << std::endl << termNode->F << "]" << std::endl;
}
else
{
numValidDatums++;
}
}
else
{
numValidDatums++;
}
#endif
// save lambda value of match for computing 3D match equivalent
matchNorms.Element(s) = pTree->MeshP->faceNormals(matchDatums[s]);
//SamplePostMatch(s);
}
avgNodesSearched /= nSamples;
#ifdef ValidatePDTreeSearch
validPercent = (double)numValidDatums / (double)nSamples;
validFS << "iter " << validIter << ": NumMatches(valid/invalid): "
<< numValidDatums << "/" << numInvalidDatums << " valid% = "
<< validPercent << std::endl;
validIter++;
#endif
}
void mtsTeleOperationECM::RunEnabled(void)
{
if (mIsClutched) {
return;
}
/* --- Forces on MTMs --- */
const vct3 frictionForceCoeff(-10.0, -10.0, -40.0);
const double distanceForceCoeff = 150.0;
//-1- vector between MTMs
vct3 vectorLR;
vectorLR.DifferenceOf(mMTMR.PositionCartesianCurrent.Position().Translation(),
mMTML.PositionCartesianCurrent.Position().Translation());
// -2- mid-point, aka center of image
vct3 c;
c.SumOf(mMTMR.PositionCartesianCurrent.Position().Translation(),
mMTML.PositionCartesianCurrent.Position().Translation());
c.Multiply(0.5);
vct3 directionC = c.Normalized();
// -3- image up vector
vct3 up;
up.CrossProductOf(vectorLR, c);
up.NormalizedSelf();
// -4- Width of image
vct3 side;
side.CrossProductOf(c, up);
side.NormalizedSelf();
// -5- find desired position for L and R
vct3 goalL(c);
goalL.AddProductOf(-mInitial.w, side);
goalL.AddProductOf(-mInitial.d, directionC);
vct3 goalR(c);
goalR.AddProductOf(mInitial.w, side);
goalR.AddProductOf(mInitial.d, directionC);
// compute forces on L and R based on error in position
vct3 forceFriction;
vct3 force;
prmForceCartesianSet wrenchR, wrenchL;
// MTMR
// apply force
force.DifferenceOf(goalR,
mMTMR.PositionCartesianCurrent.Position().Translation());
force.Multiply(distanceForceCoeff);
wrenchR.Force().Ref<3>(0).Assign(force);
// add friction force
forceFriction.ElementwiseProductOf(frictionForceCoeff,
mMTMR.VelocityCartesianCurrent.VelocityLinear());
wrenchR.Force().Ref<3>(0).Add(forceFriction);
// apply
mMTMR.SetWrenchBody(wrenchR);
// MTML
// apply force
force.DifferenceOf(goalL,
mMTML.PositionCartesianCurrent.Position().Translation());
force.Multiply(distanceForceCoeff);
wrenchL.Force().Ref<3>(0).Assign(force);
// add friction force
forceFriction.ElementwiseProductOf(frictionForceCoeff,
mMTML.VelocityCartesianCurrent.VelocityLinear());
wrenchL.Force().Ref<3>(0).Add(forceFriction);
// apply
mMTML.SetWrenchBody(wrenchL);
/* --- Joint Control --- */
static const vct3 normXZ(0.0, 1.0, 0.0);
static const vct3 normYZ(1.0, 0.0, 0.0);
static const vct3 normXY(0.0, 0.0, 1.0);
// Initial ECM joints
vctVec goalJoints(mInitial.ECMPositionJoint);
// Change in directions and joints
vctVec changeJoints(4);
vctVec changeDir(4);
vct3 crossN; // normal to direction of motion
// - Direction 0 - left/right, movement in the XZ plane
vct3 lr(c[0], 0.0, c[2]);
lr.NormalizedSelf();
if (mInitial.Lr.AlmostEqual(lr)) {
changeDir[0] = 0.0;
} else {
changeDir[0] = -acos(vctDotProduct(mInitial.Lr, lr));
crossN = vctCrossProduct(mInitial.Lr, lr);
if (vctDotProduct(normXZ, crossN) < 0.0) {
changeDir[0] = -changeDir[0];
}
}
// - Direction 1 - up/down, movement in the YZ plane
vct3 ud(0.0, c[1], c[2]);
ud.NormalizedSelf();
if (mInitial.Ud.AlmostEqual(ud)) {
changeDir[1] = 0.0;
} else {
changeDir[1] = acos(vctDotProduct(mInitial.Ud, ud));
crossN = vctCrossProduct(mInitial.Ud, ud);
if (vctDotProduct(normYZ, crossN) < 0.0) {
changeDir[1] = -changeDir[1];
}
}
// - Direction 2 - in/out
changeDir[2] = mScale * (mInitial.C.Norm() - c.Norm());
// - Direction 3 - cc/ccw, movement in the XY plane
vct3 cw(up[0], up[1], 0);
cw.NormalizedSelf();
if (mInitial.Cw.AlmostEqual(cw)) {
changeDir[3] = 0.0;
} else {
changeDir[3] = -acos(vctDotProduct(mInitial.Cw, cw));
crossN = vctCrossProduct(mInitial.Cw, cw);
if (vctDotProduct(normXY, crossN) < 0) {
changeDir[3] = -changeDir[3];
}
}
// adjusting movement for camera orientation
double totalChangeJoint3 = changeDir[3] + mInitial.ECMPositionJoint[3];
changeJoints[0] = changeDir[0] * cos(totalChangeJoint3) - changeDir[1] * sin(totalChangeJoint3);
changeJoints[1] = changeDir[1] * cos(totalChangeJoint3) + changeDir[0] * sin(totalChangeJoint3);
changeJoints[2] = changeDir[2];
changeJoints[3] = changeDir[3];
goalJoints.Add(changeJoints);
mECM.PositionJointSet.Goal().ForceAssign(goalJoints);
mECM.SetPositionJoint(mECM.PositionJointSet);
/* --- Lock Orientation --- */
//Calculate new rotations of MTMs
vctMatRot3 currMTMLRot;
vctMatRot3 currMTMRRot;
// Current ECM Rotation
vctEulerZYXRotation3 finalEulerAngles;
vctMatrixRotation3<double> currECMRot;
vctMatrixRotation3<double> finalECMRot;
finalEulerAngles.Assign(goalJoints[3], goalJoints[0], goalJoints[1]);
vctEulerToMatrixRotation3(finalEulerAngles, finalECMRot);
currECMRot = finalECMRot * mInitial.ECMRotEuler.Inverse();
// Set MTM Orientation
currMTMLRot = currECMRot.Inverse() * mInitial.MTMLRot;
currMTMRRot = currECMRot.Inverse() * mInitial.MTMRRot;
// set cartesian effort parameters
mMTML.SetWrenchBodyOrientationAbsolute(true);
mMTML.LockOrientation(currMTMLRot);
mMTMR.SetWrenchBodyOrientationAbsolute(true);
mMTMR.LockOrientation(currMTMRRot);
}
void mtsTeleOperationECM::EnterEnabled(void)
{
// set cartesian effort parameters
mMTML.SetWrenchBodyOrientationAbsolute(true);
mMTML.LockOrientation(mMTML.PositionCartesianCurrent.Position().Rotation());
mMTMR.SetWrenchBodyOrientationAbsolute(true);
mMTMR.LockOrientation(mMTMR.PositionCartesianCurrent.Position().Rotation());
// initial state for MTM force feedback
// -1- initial distance between MTMs
vct3 vectorLR;
vectorLR.DifferenceOf(mMTMR.PositionCartesianCurrent.Position().Translation(),
mMTML.PositionCartesianCurrent.Position().Translation());
// -2- mid-point, aka center of image
mInitial.C.SumOf(mMTMR.PositionCartesianCurrent.Position().Translation(),
mMTML.PositionCartesianCurrent.Position().Translation());
mInitial.C.Multiply(0.5);
// -3- image up vector
mInitial.Up.CrossProductOf(vectorLR, mInitial.C);
mInitial.Up.NormalizedSelf();
// -4- width of image, depth of arms wrt image plan
vct3 side;
side.CrossProductOf(mInitial.C, mInitial.Up);
side.NormalizedSelf();
mInitial.w = 0.5 * vctDotProduct(side, vectorLR);
mInitial.d = 0.5 * vctDotProduct(mInitial.C.Normalized(), vectorLR);
// projections
mInitial.Lr.Assign(mInitial.C[0], 0, mInitial.C[2]);
mInitial.Lr.NormalizedSelf();
mInitial.Ud.Assign(0, mInitial.C[1], mInitial.C[2]);
mInitial.Ud.NormalizedSelf();
mInitial.Cw.Assign(mInitial.Up[0], mInitial.Up[1], 0);
mInitial.Cw.NormalizedSelf();
mInitial.ECMPositionJoint = mECM.StateJointDesired.Position();
// -5- store current rotation matrix for MTML, MTMR, and ECM
vctEulerZYXRotation3 eulerAngles;
eulerAngles.Assign(mInitial.ECMPositionJoint[3], mInitial.ECMPositionJoint[0], mInitial.ECMPositionJoint[1]);
vctEulerToMatrixRotation3(eulerAngles, mInitial.ECMRotEuler);
mInitial.MTMLRot = mMTML.PositionCartesianCurrent.Position().Rotation();
mInitial.MTMRRot = mMTMR.PositionCartesianCurrent.Position().Rotation();
#if 0
std::cerr << CMN_LOG_DETAILS << std::endl
<< "L: " << mMTML.PositionCartesianCurrent.Position().Translation() << std::endl
<< "R: " << mMTMR.PositionCartesianCurrent.Position().Translation() << std::endl
<< "C: " << mInitial.C << std::endl
<< "Up: " << mInitial.Up << std::endl
<< "w: " << mInitial.w << std::endl
<< "d: " << mInitial.d << std::endl
<< "Si: " << side << std::endl;
#endif
mInitial.ECMPositionJoint = mECM.StateJointDesired.Position();
// check if by any chance the clutch pedal is pressed
if (mIsClutched) {
Clutch(true);
} else {
SetFollowing(true);
}
}
double algDirICP_IMLOP::ICP_EvaluateErrorFunction()
{
//// Return the negative log likelihood of the vonMises-Fisher
//// and Gaussian distributions under the assumption
//// of independence between the two distributions
////
//// Negative Log-Likelihood:
//// -log[ C * exp( k*dot(Ny,Nx) - B*||Y-X||^2 ) ]
//// where C = product of normalizations terms
//// for Fisher and 3D Gaussian distributions
//// C = [k/(2*PI*(e^k-e^-k))]*[1/(2*PI*sigma^2)^(3/2)]
//// B = 1/(2*sigma^2)
//double logC;
//static const double log2PI = log(2 * cmnPI); // compute this once for efficiency
#ifdef TEST_STD_ICP
//// Test Standard ICP Condition:
//// compute the log of the normalization constant C for
//// only a Gaussian distribution
//// (k=0 and orientations are not being considered)
//logC = -(3.0/2.0)*log(2*cmnPI*sigma2);
// include match errors of good samples and of thresholded outliers
// to improve smoothness of cost function
return B*(pICP->gSumSqrDist_PostMatch + pICP->oSumSqrDist_PostMatch); //- nSamples*logC;
#endif
//// compute normalization constant
//double logEkE_k = 0.0;
//if (k >= 5)
//{ // exp(k)>>exp(-k) => exp(k)-exp(-k) ~= exp(k)
// // => log(exp(k)-exp(-k)) ~= log(exp(k)) = k
// logEkE_k = k;
//}
//else
//{
// logEkE_k = log(exp(k) - exp(-k));
//}
//// to avoid overflow/underflow, calculate logC directly
//// (rather than calculating C and then logC)
//logC = log(k) - (5.0 / 2.0)*log2PI - logEkE_k - (3.0 / 2.0)*log(sigma2);
vct3 residual;
double sumSqrDist = 0.0;
double sumNormProducts = 0.0;
for (unsigned int s = 0; s < nGoodSamples; s++)
{
residual = goodSamplePts.Element(s) - goodMatchPts.Element(s);
sumSqrDist += residual.NormSquare();
sumNormProducts += vctDotProduct(goodSampleNorms.Element(s), goodMatchNorms.Element(s));
}
// include match errors of good samples and of thresholded outliers
// to improve smoothness of cost function
// NOTE: adding an extra k*nSamples to the cost produces a cost function >= 0
return B*(sumSqrDist)+k*(nSamples - sumNormProducts); // -logC*nSamples;
//// including match errors of thresholded outliers helps improve
//// smoothness of cost function
//return B*(gSumSqrDist_PostMatch + oSumSqrDist_PostMatch)
// - k*(gSumNormProducts_PostMatch + oSumNormProducts_PostMatch)
// - logC*nSamples;
// This method for computing logC is unstable
// It produces C=0 => infinity for log(C) when k is large
// The problem comes when computing log(exp(k)-exp(-k))
// in that exp(k) blows up for big k
//// von Mises-Fisher normalizing constant
//double Cp = k/(2*cmnPI*(exp(k)-exp(-k)));
//// Gaussian normalizing constant
//double Cn = pow((2*cmnPI*sigma2),3/2);
//C = Cp/Cn;
}
double algDirICP_IMLOP::ComputeRpos()
{
#define NMLZ_METHOD 1 // Normalization method
// NOTE: could improve efficiency by computing this value as an
// optional output argument in the vMFG P2P Registration
// function
// Compute angular match error of sample positions relative
// to the center of rotation; this is to include angular error
// of position matches measured from center of rotation of
// Procrustes problem as added indicator of rotational uncertainty
vctDynamicVectorRef<vct3> S(samplePtsXfmd);
vctDynamicVectorRef<vct3> C(matchPts);
unsigned int N = S.size();
vct3 Smean = vctCentroid(S);
vct3 Cmean = vctCentroid(C);
vct3 Sp, Cp;
double dotProducts = 0.0;
double nmlzTerm = 0.0;
for (unsigned int i = 0; i < N; i++)
{
Sp.DifferenceOf(S[i], Smean);
Cp.DifferenceOf(C[i], Cmean);
// Do not scale down to unit vectors, as points farther
// away should have greater effect on rotation.
// R calculation assumes unit vector normalization => need some
// form of normalization in the end.
// TODO: should normalization be a linear or square term?
#if (NMLZ_METHOD == 1)
// use square normalization
// this is the best solution, as it works with the data directly
// i.e. the orientations do not have to be normalized prior to
// taking their dot product
dotProducts += vctDotProduct(Sp, Cp);
nmlzTerm += Sp.Norm()*Cp.Norm();
#elif (NMLZ_METHOD == 2)
// use linear normalization
double avgLen = (Sp.Norm() + Cp.Norm()) / 2.0;
dotProducts += avgLen*vctDotProduct(Sp.Normalized(), Cp.Normalized());
nmlzTerm += avgLen;
#elif (NMLZ_METHOD == 3)
// use unit vectors
dotProducts += vctDotProduct(Sp.Normalized(), Cp.Normalized());
#else
std::cout << "Error: normalization method unrecognized" << std::endl;
#endif
}
#if (NMLZ_METHOD == 3)
nmlzTerm = N;
#endif
double Rpos = dotProducts / nmlzTerm;
// 0 <= Rpos <= 1
Rpos = Rpos < 0.0 ? 0.0 : Rpos;
Rpos = Rpos > 1.0 ? 1.0 : Rpos;
//double posCircSD = sqrt(-2*log(Rpos));
return Rpos;
}
