/*!
Compute and return the interaction matrix \f$ L_I \f$. The computation is made
thanks to the values of the luminance features \f$ I \f$
*/
void
vpFeatureLuminance::interaction(vpMatrix &L)
{
double x,y,Ix,Iy,Zinv;
L.resize(dim_s,6) ;
for(int m = 0; m< L.getRows(); m++)
{
Ix = pixInfo[m].Ix;
Iy = pixInfo[m].Iy;
x = pixInfo[m].x ;
y = pixInfo[m].y ;
Zinv = 1 / pixInfo[m].Z;
{
L[m][0] = Ix * Zinv;
L[m][1] = Iy * Zinv;
L[m][2] = -(x*Ix+y*Iy)*Zinv;
L[m][3] = -Ix*x*y-(1+y*y)*Iy;
L[m][4] = (1+x*x)*Ix + Iy*x*y;
L[m][5] = Iy*x-Ix*y;
}
}
}
void
vpBiclops::get_fJe(const vpColVector &q, vpMatrix &fJe)
{
if (q.getRows() != 2) {
vpERROR_TRACE("Bad dimension for biclops articular vector");
throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
}
fJe.resize(6,2) ;
double s1 = sin(q[0]) ;
double c1 = cos(q[0]) ;
fJe = 0;
if (dh_model_ == DH1)
{
fJe[3][1] = -s1;
fJe[4][1] = c1;
fJe[5][0] = 1;
}
else
{
fJe[3][1] = s1;
fJe[4][1] = -c1;
fJe[5][0] = 1;
}
}
void convertEigenMatToVisp(const Eigen::MatrixXd& em, vpMatrix& vmat)
{
vmat.resize(em.rows(), em.cols());
for (int i = 0; i < em.rows(); ++i)
for (int j = 0; j < em.cols(); ++j)
vmat[i][j] = em(i,j);
}
/*!
Get the robot jacobian expressed in the end-effector frame.
\warning Re is not the embedded camera frame. It corresponds to the frame
associated to the tilt axis (see also get_cMe).
\param q : Articular position for pan and tilt axis.
\param eJe : Jacobian between end effector frame and end effector frame (on
tilt axis).
*/
void
vpBiclops::get_eJe(const vpColVector &q, vpMatrix &eJe)
{
eJe.resize(6,2) ;
if (q.getRows() != 2) {
vpERROR_TRACE("Bad dimension for biclops articular vector");
throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
}
double s2 = sin(q[1]) ;
double c2 = cos(q[1]) ;
eJe = 0;
if (dh_model_ == DH1)
{
eJe[3][0] = -c2;
eJe[4][1] = 1;
eJe[5][0] = -s2;
}
else
{
eJe[3][0] = -c2;
eJe[4][1] = -1;
eJe[5][0] = s2;
}
}
/*!
Get the inverse jacobian.
\f[
{^f}J_e^+ = \left[\begin{array}{cccccc}
-(a_1s_1+d_3c_1)/(a_1^2+d_3^2) & (a_1c_1-d_3s_1)/(a_1^2+d_3^2) & 0&0&0&0 \\
0 & 0 & 1 & 0 & 0 & 0 \\
(a_1s_1+d_3c_1)/(a_1^2+d_3^2) & -(a_1c_1-d_3s_1)/(a_1^2+d_3^2) & 0&0&0&1 \\
0 & 0 & 0 & c_{14} & s_{14} & 0 \\
\end{array}
\right]
\f]
\param q : Articular position of the four joints: q[0] corresponds to
the first rotation (joint 1 with value \f$q_1\f$) of the turret
around the vertical axis, while q[1] corresponds to the vertical
translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
correspond to the pan and tilt of the camera (respectively joint 4
and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
q[3] are expressed in radians. The translation q[1] is expressed in
meters.
\param fJe_inverse : Inverse robot jacobian expressed in the robot
reference frame.
\sa get_eJe() and get_fJe()
*/
void vpAfma4::get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
{
fJe_inverse.resize(4, 6) ;
fJe_inverse = 0;
double q1 = q[0]; // rot touret
double q4 = q[2]; // pan
double c1 = cos(q1);
double s1 = sin(q1);
double c14 = cos(q1 + q4);
double s14 = sin(q1 + q4);
double det = this->_a1 * this->_a1 + this->_d3 * this->_d3;
fJe_inverse[0][0] = (-s1*this->_a1 - c1*this->_d3)/det;
fJe_inverse[0][1] = (c1*this->_a1 - s1*this->_d3)/det;
fJe_inverse[1][2] = fJe_inverse[2][5] = 1.;
fJe_inverse[2][0] = - fJe_inverse[0][0];
fJe_inverse[2][1] = - fJe_inverse[0][1];
fJe_inverse[3][3] = c14;
fJe_inverse[3][4] = s14;
}
void
vpAfma4::get_fJe(const vpColVector &q, vpMatrix &fJe) const
{
fJe.resize(6,4) ;
double q1 = q[0]; // rot touret
double q4 = q[2]; // pan
double c1 = cos(q1);
double s1 = sin(q1);
double c14 = cos(q1 + q4);
double s14 = sin(q1 + q4);
fJe = 0;
fJe[0][0] = -s1*this->_a1 - c1*this->_d3;
fJe[1][0] = c1*this->_a1 - s1*this->_d3;
fJe[2][1] = 1.0;
fJe[3][3] = c14;
fJe[4][3] = s14;
fJe[5][0] = fJe[5][2] = 1.0;
}
/*!
\relates vpMatrix
Compute the skew symmetric matrix \f$M\f$ of translation vector \f$t\f$
(matrice de pre-produit vectoriel).
\f[ \mbox{if} \quad {\bf t} = \left( \begin{array}{c} t_x \\ t_y \\ t_z
\end{array}\right), \quad \mbox{then} \qquad
M = \left( \begin{array}{ccc}
0 & -t_z & t_y \\
t_z & 0 & -t_x \\
-t_y & t_x & 0
\end{array}\right)
\f]
\param t : Translation vector in input used to compute the skew symmetric
matrix M.
\param M : Skew symmetric matrix of translation vector \f$t\f$.
*/
void
vpTranslationVector::skew(const vpTranslationVector &t,vpMatrix &M)
{
M.resize(3,3) ;
M[0][0] = 0 ; M[0][1] = -t[2] ; M[0][2] = t[1] ;
M[1][0] = t[2] ; M[1][1] = 0 ; M[1][2] = -t[0] ;
M[2][0] = -t[1] ; M[2][1] = t[0] ; M[2][2] = 0 ;
}
/*!
Reshape the column vector in a matrix.
\param M : the reshaped matrix.
\param nrows : number of rows of the matrix.
\param ncols : number of columns of the matrix.
*/
void vpColVector::reshape(vpMatrix & M,const unsigned int &nrows,const unsigned int &ncols){
if(dsize!=nrows*ncols) {
throw(vpException(vpException::dimensionError,
"Cannot reshape (%dx1) column vector in (%dx%d) matrix",
rowNum, M.getRows(), M.getCols())) ;
}
try {
if ((M.getRows() != nrows) || (M.getCols() != ncols)) M.resize(nrows,ncols);
}
catch(...) {
throw ;
}
for(unsigned int j =0; j< ncols; j++)
for(unsigned int i =0; i< nrows; i++)
M[i][j]=data[j*nrows+i];
}
/*!
Get the robot jacobian expressed in the robot reference frame
\param q : Articular position for pan and tilt axis.
\param fJe : Jacobian between reference frame (or fix frame) and end effector
frame (on tilt axis).
*/
void
vpPtu46::get_fJe(const vpColVector &q, vpMatrix &fJe)
{
if (q.getRows() != 2) {
vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
}
fJe.resize(6,2) ;
double s1 = sin(q[0]) ;
double c1 = cos(q[0]) ;
fJe = 0;
fJe[3][1] = s1;
fJe[4][1] = -c1;
fJe[5][0] = 1;
}
/*!
Get the robot jacobian expressed in the end-effector frame.
\warning Re is not the embedded camera frame. It corresponds to the frame
associated to the tilt axis (see also get_cMe).
\param q : Articular position for pan and tilt axis.
\param eJe : Jacobian between end effector frame and end effector frame (on
tilt axis).
*/
void
vpPtu46::get_eJe(const vpColVector &q, vpMatrix &eJe) const
{
eJe.resize(6,2) ;
if (q.getRows() != 2) {
vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
}
double s2 = sin(q[1]) ;
double c2 = cos(q[1]) ;
eJe = 0;
eJe[3][0] = c2;
eJe[4][1] = 1;
eJe[5][0] = s2;
}
/*!
\brief reshape the colvector in a matrix
\param m : the reshaped Matrix
\param nrows : number of rows of the matrix
\param ncols : number of columns of the matrix
*/
void vpColVector::reshape(vpMatrix & m,const unsigned int &nrows,const unsigned int &ncols){
if(dsize!=nrows*ncols)
{
vpERROR_TRACE("\n\t\t vpColVector mismatch size for reshape vpSubColVector in a vpMatrix") ;
throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
"\n\t\t \n\t\t vpColVector mismatch size for reshape vpSubColVector in a vpMatrix")) ;
}
try
{
if ((m.getRows() != nrows) || (m.getCols() != ncols)) m.resize(nrows,ncols);
}
catch(vpException me)
{
vpERROR_TRACE("Error caught") ;
std::cout << me << std::endl ;
throw ;
}
for(unsigned int j =0; j< ncols; j++)
for(unsigned int i =0; i< nrows; i++)
m[i][j]=data[j*ncols+i];
}
/*!
Get the robot jacobian expressed in the end-effector frame:
\f[
{^e}J_e = \left[\begin{array}{cccc}
-c_5(a_1c_4+d_3s_4) & -s_5 & 0 & 0 \\
s_5(a_1c_4+d_3s_4) & -c_5 & 0 & 0 \\
a_1s_4-d_3c_4 & 0 & 0 & 0 \\
-s_5 & 0 & -s_5 & 0 \\
-c_5 & 0 & -c_5 & 0 \\
0 & 0 & 0 & 1 \\
\end{array}
\right]
\f]
\param q : Articular position of the four joints: q[0] corresponds to
the first rotation (joint 1 with value \f$q_1\f$) of the turret
around the vertical axis, while q[1] corresponds to the vertical
translation (joint 2 with value \f$q_2\f$), while q[2] and q[3]
correspond to the pan and tilt of the camera (respectively joint 4
and 5 with values \f$q_4\f$ and \f$q_5\f$). Rotations q[0], q[2] and
q[3] are expressed in radians. The translation q[1] is expressed in
meters.
\param eJe : Robot jacobian expressed in the end-effector frame, with:
\f[
{^e}J_e = \left[\begin{array}{cc}
{^f}R_e^T & 0_{3 \times 3} \\
0_{3 \times 3} & {^f}R_e^T \\
\end{array}
\right] {^f}J_e
\f]
\sa get_fJe()
*/
void
vpAfma4::get_eJe(const vpColVector &q, vpMatrix &eJe) const
{
double q4 = q[2]; // pan
double q5 = q[3]; // tilt
double c4 = cos(q4);
double s4 = sin(q4);
double c5 = cos(q5);
double s5 = sin(q5);
eJe.resize(6, 4);
eJe = 0;
eJe[0][0] = -(this->_a1*c4 + this->_d3*s4)*c5;
eJe[0][1] = -s5;
eJe[1][0] = (this->_a1*c4 + this->_d3*s4)*s5;
eJe[1][1] = -c5;
eJe[2][0] = (this->_a1*s4 - this->_d3*c4);
eJe[3][0] = eJe[3][2] = -s5;
eJe[4][0] = eJe[4][2] = -c5;
eJe[5][3] = 1.;
}
static void
computeInteractionMatrixFromList (/*const*/ vpList<vpBasicFeature *> & featureList,
/*const*/ vpList<int> & featureSelectionList,
vpMatrix & L)
{
if (featureList.empty())
{
vpERROR_TRACE("feature list empty, cannot compute Ls") ;
throw(vpServoException(vpServoException::noFeatureError,
"feature list empty, cannot compute Ls")) ;
}
/* The matrix dimension is not known before the affectation loop.
* It thus should be allocated on the flight, in the loop.
* The first assumption is that the size has not changed. A double
* reallocation (realloc(dim*2)) is done if necessary. In particulary,
* [log_2(dim)+1] reallocations are done for the first matrix computation.
* If the allocated size is too large, a correction is done after the loop.
* The algotithmic cost is linear in affectation, logarthmic in allocation
* numbers and linear in allocation size.
*/
/* First assumption: matrix dimensions have not changed. If 0, they are
* initialized to dim 1.*/
int rowL = L .getRows();
const int colL = 6;
if (0 == rowL) { rowL = 1; L .resize(rowL, colL);}
/* vectTmp is used to store the return values of functions get_s() and
* error(). */
vpMatrix matrixTmp;
int rowMatrixTmp, colMatrixTmp;
/* The cursor are the number of the next case of the vector array to
* be affected. A memory reallocation should be done when cursor
* is out of the vector-array range.*/
int cursorL = 0;
for (featureList.front() ,featureSelectionList.front() ;
!featureList.outside();
featureSelectionList.next(),featureList.next() )
{
vpBasicFeature * sPTR = featureList.value() ;
const int select = featureSelectionList.value() ;
/* Get s. */
matrixTmp = sPTR->interaction(select);
rowMatrixTmp = matrixTmp .getRows();
colMatrixTmp = matrixTmp .getCols();
/* Check the matrix L size, and realloc if needed. */
while (rowMatrixTmp + cursorL > rowL)
{ rowL *= 2; L .resize (rowL,colL,false); vpDEBUG_TRACE(15,"Realloc!"); }
/* Copy the temporarily matrix into L. */
for (int k = 0; k < rowMatrixTmp; ++k, ++cursorL)
for (int j = 0; j < colMatrixTmp; ++j)
{ L[cursorL][j] = matrixTmp[k][j]; }
}
L.resize (cursorL,colL,false);
return ;
}
