/*!
\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];
}
