/*! \brief Initialisation of a sub matrix \param m : parent matrix \param row : row offset \param col : col offset \param nrows : number of rows of the sub matrix \param ncols : number of columns of the sub matrix */ void vpSubMatrix::init(vpMatrix &m, const unsigned int & row, const unsigned int &col , const unsigned int & nrows , const unsigned int & ncols){ if(! m.data){ vpERROR_TRACE("\n\t\t SubMatrix parent matrix is not allocated") ; throw(vpMatrixException(vpMatrixException::subMatrixError, "\n\t\t SubMatrix parent matrix is not allocated")) ; } if(row+nrows <= m.getRows() && col+ncols <= m.getCols()){ data=m.data; parent =&m; rowNum = nrows; colNum = ncols; pRowNum=m.getRows(); pColNum=m.getCols(); if(rowPtrs) free(rowPtrs); rowPtrs=(double**) malloc(nrows * sizeof(double*)); for(unsigned int r=0;r<nrows;r++) rowPtrs[r]= m.data+col+(r+row)*pColNum; dsize = pRowNum*pColNum ; trsize =0 ; }else{ vpERROR_TRACE("Submatrix cannot be contain in parent matrix") ; throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,"Submatrix cannot be contain in parent matrix")) ; } }
/*! Constructor that creates a column vector from a m-by-1 matrix \e M. \exception vpException::dimensionError If the matrix is not a m-by-1 matrix. */ vpColVector::vpColVector (const vpMatrix &M) : vpArray2D<double>(M.getRows(), 1) { if(M.getCols()!=1) { throw(vpException(vpException::dimensionError, "Cannot construct a (%dx1) row vector from a (%dx%d) matrix", M.getRows(), M.getRows(), M.getCols())) ; } for(unsigned int i=0; i< M.getRows(); i++) (*this)[i] = M[i][0]; }
/*! Constructor that creates a row vector from a 1-by-n matrix \e M. \exception vpException::dimensionError If the matrix is not a 1-by-n matrix. */ vpRowVector::vpRowVector (const vpMatrix &M) : vpArray2D<double>(1, M.getCols()) { if(M.getRows()!=1) { throw(vpException(vpException::dimensionError, "Cannot construct a (1x%d) row vector from a (%dx%d) matrix", M.getCols(), M.getRows(), M.getCols())) ; } for(unsigned int j=0; j< M.getCols(); j++) (*this)[j] = M[0][j]; }
/*! 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]; }
/*! Operator that allows to multiply a velocity twist transformation matrix by a matrix. As shown in the example below, this operator can be used to compute the corresponding camera velocity skew from the joint velocities knowing the robot jacobian. \code #include <visp3/core/vpConfig.h> #include <visp3/robot/vpSimulatorCamera.h> #include <visp3/core/vpColVector.h> #include <visp3/core/vpVelocityTwistMatrix.h> int main() { vpSimulatorCamera robot; vpColVector q_vel(6); // Joint velocity on the 6 joints // ... q_vel need here to be initialized vpColVector c_v(6); // Velocity in the camera frame: vx,vy,vz,wx,wy,wz vpVelocityTwistMatrix cVe; // Velocity skew transformation from camera frame to end-effector robot.get_cVe(cVe); vpMatrix eJe; // Robot jacobian robot.get_eJe(eJe); // Compute the velocity in the camera frame c_v = cVe * eJe * q_vel; return 0; } \endcode \exception vpException::dimensionError If M is not a 6 rows dimension matrix. */ vpMatrix vpVelocityTwistMatrix::operator*(const vpMatrix &M) const { if (6 != M.getRows()) { throw(vpException(vpException::dimensionError, "Cannot multiply a (6x6) velocity twist matrix by a (%dx%d) matrix", M.getRows(), M.getCols())); } vpMatrix p(6, M.getCols()) ; for (unsigned int i=0;i<6;i++) for (unsigned int j=0;j<M.getCols();j++) { double s =0 ; for (unsigned int k=0;k<6;k++) s += rowPtrs[i][k] * M[k][j]; p[i][j] = s ; } return p; }
/*! initialise the camera from a calibration matrix. Using a calibration matrix leads to a camera without distorsion The K matrix in parameters must be like: \f$ K = \left(\begin{array}{ccc} p_x & 0 & u_0 \\ 0 & p_y & v_0 \\ 0 & 0 & 1 \end{array} \right) \f$ \param _K : the 3by3 calibration matrix */ void vpCameraParameters::initFromCalibrationMatrix(const vpMatrix& _K) { if(_K.getRows() != 3 || _K.getCols() != 3 ){ throw vpException(vpException::dimensionError, "bad size for calibration matrix"); } if( std::fabs(_K[2][2] - 1.0) > std::numeric_limits<double>::epsilon()){ throw vpException(vpException::badValue, "bad value: K[2][2] must be equal to 1"); } initPersProjWithoutDistortion (_K[0][0], _K[1][1], _K[0][2], _K[1][2]); }
/*! Operator that allows to multiply a skew transformation matrix by a matrix. \exception vpMatrixException::incorrectMatrixSizeError If M is not a 6 rows dimension matrix. */ vpMatrix vpForceTwistMatrix::operator*(const vpMatrix &M) const { if (6 != M.getRows()) { vpERROR_TRACE("vpForceTwistMatrix mismatch in vpForceTwistMatrix/vpMatrix multiply") ; throw(vpMatrixException::incorrectMatrixSizeError) ; } vpMatrix p(6, M.getCols()) ; for (unsigned int i=0;i<6;i++) { for (unsigned int j=0;j<M.getCols();j++) { double s =0 ; for (unsigned int k=0;k<6;k++) s += rowPtrs[i][k] * M[k][j]; p[i][j] = s ; } } return p; }
/*! Multiply a row vector by a matrix. \param M : Matrix. \warning The number of elements of the row vector must be equal to the number of rows of the matrix. \exception vpException::dimensionError If the number of elements of the row vector is not equal to the number of rows of the matrix. \return The resulting row vector. */ vpRowVector vpRowVector::operator*(const vpMatrix &M) const { vpRowVector c(M.getCols()); if (colNum != M.getRows()) { throw(vpException(vpException::dimensionError, "Cannot multiply (1x%d) row vector by (%dx%d) matrix", colNum, M.getRows(), M.getCols())) ; } c = 0.0; for (unsigned int i=0;i<colNum;i++) { double bi = data[i] ; // optimization em 5/12/2006 for (unsigned int j=0;j<M.getCols();j++) { c[j]+=bi*M[i][j]; } } return c ; }
bool test(const std::string &s, const vpMatrix &M, const std::vector<double> &bench) { static unsigned int cpt = 0; std::cout << "** Test " << ++cpt << std::endl; std::cout << s << "(" << M.getRows() << "," << M.getCols() << ") = \n" << M << std::endl; if(bench.size() != M.size()) { std::cout << "Test fails: bad size wrt bench" << std::endl; return false; } for (unsigned int i=0; i<M.size(); i++) { if (std::fabs(M.data[i]-bench[i]) > std::fabs(M.data[i])*std::numeric_limits<double>::epsilon()) { std::cout << "Test fails: bad content" << std::endl; return false; } } return true; }
// writes a matrix into a single line YAML format string okExperiment::singleLineYAML(const vpMatrix &M) { stringstream ss; unsigned int i,j; ss << "["; for(i=0;i<M.getRows();++i) { ss << "["; for(j=0;j<M.getCols()-1;++j) ss << M[i][j] << ", "; ss << M[i][j] << "]"; if(i != M.getRows()-1) ss << ","; } ss << "]"; return ss.str(); }
/*! \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]; }
static void lagrange (vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2) { if (DEBUG_LEVEL1) std::cout << "begin (CLagrange.cc)Lagrange(...) " << std::endl; try{ int i,imin; vpMatrix ata ; // A^T A ata = a.t()*a ; vpMatrix btb ; // B^T B btb = b.t()*b ; vpMatrix bta ; // B^T A bta = b.t()*a ; vpMatrix btb1 ; // (B^T B)^(-1) if (b.getRows() >= b.getCols()) btb1 = btb.inverseByLU() ; else btb1 = btb.pseudoInverse(); if (DEBUG_LEVEL1) { std::cout << " BTB1 * BTB : " << std::endl << btb1*btb << std::endl; std::cout << " BTB * BTB1 : " << std::endl << btb*btb1 << std::endl; } vpMatrix r ; // (B^T B)^(-1) B^T A r = btb1*bta ; vpMatrix e ; // - A^T B (B^T B)^(-1) B^T A e = - (a.t()*b) *r ; e += ata ; // calcul E = A^T A - A^T B (B^T B)^(-1) B^T A if (DEBUG_LEVEL1) { std::cout << " E :" << std::endl << e << std::endl; } // vpColVector sv ; // vpMatrix v ; e.svd(x1,ata) ;// destructif sur e // calcul du vecteur propre de E correspondant a la valeur propre min. /* calcul de SVmax */ imin = 0; // FC : Pourquoi calculer SVmax ?????? // double svm = 0.0; // for (i=0;i<x1.getRows();i++) // { // if (x1[i] > svm) { svm = x1[i]; imin = i; } // } // svm *= EPS; /* pour le rang */ for (i=0;i<x1.getRows();i++) if (x1[i] < x1[imin]) imin = i; if (DEBUG_LEVEL1) { printf("SV(E) : %.15lf %.15lf %.15lf\n",x1[0],x1[1],x1[2]); std::cout << " i_min " << imin << std::endl; } for (i=0;i<x1.getRows();i++) x1[i] = ata[i][imin]; x2 = - (r*x1) ; // X_2 = - (B^T B)^(-1) B^T A X_1 if (DEBUG_LEVEL1) { std::cout << " X1 : " << x1.t() << std::endl; std::cout << " V : " << std::endl << ata << std::endl; } } catch(...) { vpERROR_TRACE(" ") ; throw ; } if (DEBUG_LEVEL1) std::cout << "end (CLagrange.cc)Lagrange(...) " << std::endl; }
//! Constructor that take column j of matrix M. vpColVector::vpColVector (const vpMatrix &M, unsigned int j) : vpArray2D<double>(M.getRows(), 1) { for(unsigned int i=0; i< M.getCols(); i++) (*this)[i] = M[i][j]; }
/*! Get the view of the virtual camera. Be carefull, the image I is modified. The projected image is not added as an overlay! To take into account the projection of several images, a matrix \f$ zBuffer \f$ is given as argument. This matrix contains the z coordinates of all the pixel of the image \f$ I \f$ in the camera frame. During the projection, the pixels are updated if there is no other plan between the camera and the projected image. The matrix \f$ zBuffer \f$ is updated in this case. \param I : The image used to store the result. \param cam : The parameters of the virtual camera. \param zBuffer : A matrix containing the z coordinates of the pixels of the image \f$ I \f$ */ void vpImageSimulator::getImage(vpImage<vpRGBa> &I, const vpCameraParameters cam, vpMatrix &zBuffer) { if (I.getWidth() != (unsigned int)zBuffer.getCols() || I.getHeight() != (unsigned int)zBuffer.getRows()) throw (vpMatrixException(vpMatrixException::incorrectMatrixSizeError, " zBuffer must have the same size as the image I ! ")); int nb_point_dessine = 0; if (cleanPrevImage) { for (int i = (int)rect.getTop(); i < (int)rect.getBottom(); i++) { for (int j = (int)rect.getLeft(); j < (int)rect.getRight(); j++) { I[i][j] = bgColor; } } } if(visible) { getRoi(I.getWidth(),I.getHeight(),cam,pt,rect); double top = rect.getTop(); double bottom = rect.getBottom(); double left = rect.getLeft(); double right= rect.getRight(); vpRGBa *bitmap = I.bitmap; unsigned int width = I.getWidth(); vpImagePoint ip; for (int i = (int)top; i < (int)bottom; i++) { for (int j = (int)left; j < (int)right; j++) { double x=0,y=0; ip.set_ij(i,j); vpPixelMeterConversion::convertPoint(cam,ip, x,y); ip.set_ij(y,x); if (colorI == GRAY_SCALED) { unsigned char Ipixelplan; if(getPixel(ip,Ipixelplan)) { if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0) { vpRGBa pixelcolor; pixelcolor.R = Ipixelplan; pixelcolor.G = Ipixelplan; pixelcolor.B = Ipixelplan; *(bitmap+i*width+j) = pixelcolor; nb_point_dessine++; zBuffer[i][j] = Xinter_optim[2]; } } } else if (colorI == COLORED) { vpRGBa Ipixelplan; if(getPixel(ip,Ipixelplan)) { if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0) { *(bitmap+i*width+j) = Ipixelplan; nb_point_dessine++; zBuffer[i][j] = Xinter_optim[2]; } } } } } } }
void vpMbKltTracker::computeVVSPoseEstimation(const unsigned int iter, vpMatrix &L, const vpColVector &w, vpMatrix &L_true, vpMatrix &LVJ_true, double &normRes, double &normRes_1, vpColVector &w_true, vpColVector &R, vpMatrix <L, vpColVector <R, vpColVector &error_prev, vpColVector &v, double &mu, vpHomogeneousMatrix &cMoPrev, vpHomogeneousMatrix &ctTc0_Prev) { m_error = R; if(computeCovariance){ L_true = L; if(!isoJoIdentity){ vpVelocityTwistMatrix cVo; cVo.buildFrom(cMo); LVJ_true = (L*cVo*oJo); } } normRes_1 = normRes; normRes = 0; for (unsigned int i = 0; i < static_cast<unsigned int>(R.getRows()); i += 1){ w_true[i] = w[i]; R[i] = R[i] * w[i]; normRes += R[i]; } if((iter == 0) || compute_interaction){ for(unsigned int i=0; i<static_cast<unsigned int>(R.getRows()); i++){ for(unsigned int j=0; j<6; j++){ L[i][j] *= w[i]; } } } if(isoJoIdentity){ LTL = L.AtA(); computeJTR(L, R, LTR); switch(m_optimizationMethod){ case vpMbTracker::LEVENBERG_MARQUARDT_OPT: { vpMatrix LMA(LTL.getRows(), LTL.getCols()); LMA.eye(); vpMatrix LTLmuI = LTL + (LMA*mu); v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LTR; if(iter != 0) mu /= 10.0; error_prev = m_error; break; } case vpMbTracker::GAUSS_NEWTON_OPT: default: v = -lambda * LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon()) * LTR; } } else{ vpVelocityTwistMatrix cVo; cVo.buildFrom(cMo); vpMatrix LVJ = (L*cVo*oJo); vpMatrix LVJTLVJ = (LVJ).AtA(); vpColVector LVJTR; computeJTR(LVJ, R, LVJTR); switch(m_optimizationMethod){ case vpMbTracker::LEVENBERG_MARQUARDT_OPT: { vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols()); LMA.eye(); vpMatrix LTLmuI = LVJTLVJ + (LMA*mu); v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LVJTR; v = cVo * v; if(iter != 0) mu /= 10.0; error_prev = m_error; break; } case vpMbTracker::GAUSS_NEWTON_OPT: default: { v = -lambda*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR; v = cVo * v; break; } } } cMoPrev = cMo; ctTc0_Prev = ctTc0; ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0; cMo = ctTc0 * c0Mo; }
/*! Constructor that creates a row vector corresponding to row \e i of matrix \e M. */ vpRowVector::vpRowVector (const vpMatrix &M, unsigned int i) : vpArray2D<double>(1, M.getCols()) { for(unsigned int j=0; j< M.getCols(); j++) (*this)[j] = M[i][j]; }