static
void calculSolutionDementhon(double xi0, double yi0,
vpColVector &I, vpColVector &J,
vpHomogeneousMatrix &cMo )
{
#if (DEBUG_LEVEL1)
std::cout << "begin (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
#endif
double normI, normJ, normk, Z0;
vpColVector k(3);
// normalisation de I et J
normI = sqrt(I.sumSquare()) ;
normJ = sqrt(J.sumSquare()) ;
I/=normI;
J/=normJ;
k = vpColVector::cross(I,J) ; // k = I^I
Z0=2.0/(normI+normJ);
normk = sqrt(k.sumSquare()) ;
k /= normk ;
J = vpColVector::cross(k,I) ;
//calcul de la matrice de passage
cMo[0][0]=I[0];
cMo[0][1]=I[1];
cMo[0][2]=I[2];
cMo[0][3]=xi0*Z0;
cMo[1][0]=J[0];
cMo[1][1]=J[1];
cMo[1][2]=J[2];
cMo[1][3]=yi0*Z0;
cMo[2][0]=k[0];
cMo[2][1]=k[1];
cMo[2][2]=k[2];
cMo[2][3]=Z0;
#if (DEBUG_LEVEL1)
std::cout << "end (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
#endif
}
// Fill in the plot message for publishing data to be plotted in the /VS_errors topic
void makePlotMSG(std_msgs::Float64MultiArray &plotMsg,
vpColVector &taskError,
vpColVector &qdot,
vpColVector &q1dot,
vpColVector &q2dot,
vpMatrix &fJe,
vpVelocityTwistMatrix &eVf,
KDL::JntArray &kdlArmPosition
)
{
// add qdot - total joint velocity
for(unsigned int i=0; i<qdot.getRows(); i++)
{
plotMsg.data.push_back(qdot[i]);
}
// add q1dot - main task
for(unsigned int i=0; i<q1dot.getRows(); i++)
{
plotMsg.data.push_back(q1dot[i]);
}
// add q2dot - main task
for(unsigned int i=0; i<q2dot.getRows(); i++)
{
plotMsg.data.push_back(q2dot[i]);
}
// add the end-effector velocity
vpColVector vel;
vel = eVf*fJe*qdot;
for(unsigned int i=0; i<vel.getRows(); i++)
{
plotMsg.data.push_back(vel[i]);
}
// add the joint positions
for(unsigned int i=0; i<kdlArmPosition.rows(); i++)
{
plotMsg.data.push_back(kdlArmPosition.data[i]);
}
// add the error norm
plotMsg.data.push_back(taskError.sumSquare());
// add the errors
for(unsigned int i=0; i<taskError.getRows(); i++)
{
ROS_INFO_STREAM("=> makePlotMSG: adding taskError at position " << plotMsg.data.size());
plotMsg.data.push_back(taskError[i]);
}
}
void
vpMbKltTracker::computeVVSCheckLevenbergMarquardtKlt(const unsigned int iter, const unsigned int nbInfos,
const vpHomogeneousMatrix &cMoPrev, const vpColVector &error_prev, const vpHomogeneousMatrix &ctTc0_Prev,
double &mu, bool &reStartFromLastIncrement) {
if(iter != 0 && m_optimizationMethod == vpMbTracker::LEVENBERG_MARQUARDT_OPT){
if(m_error.sumSquare()/(double)(2*nbInfos) > error_prev.sumSquare()/(double)(2*nbInfos)){
mu *= 10.0;
if(mu > 1.0)
throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
cMo = cMoPrev;
m_error = error_prev;
ctTc0 = ctTc0_Prev;
reStartFromLastIncrement = true;
}
}
}
