/*!
\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")) ;
}
}
/*!
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;
}
}
}
/*!
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]);
}
/*!
\brief set the value of the interaction matrix.
\param L_ : The matrix corresponding to the interaction matrix you computed.
\exception an exception is thrown if the number of row of the interaction
matrix is different from the dimension of the visual feature as specified
in the constructor
*/
void
vpGenericFeature::setInteractionMatrix(const vpMatrix &L_)
{
if (L_.getRows() != dim_s)
{
std::cout << L_.getRows() <<" " << dim_s << std::endl ;;
vpERROR_TRACE("size mismatch between interaction matrix size "
"and feature dimension");
throw(vpFeatureException(vpFeatureException::sizeMismatchError,
"size mismatch between interaction matrix size "
"and feature dimension"));
}
this->L = L_ ;
}
/*!
Apply a filter to an image.
\param I : Image to filter
\param If : Filtered image.
\param M : Filter coefficients.
*/
void
vpImageFilter::filter(const vpImage<unsigned char> &I,
vpImage<double>& If,
const vpMatrix& M)
{
unsigned int size = M.getRows() ;
unsigned int half_size = size/2 ;
If.resize(I.getHeight(),I.getWidth()) ;
If = 0 ;
for (unsigned int i=half_size ; i < I.getHeight()-half_size ; i++)
{
for (unsigned int j=half_size ; j < I.getWidth()-half_size ; j++)
{
double conv_x = 0 ;
for(unsigned int a = 0 ; a < size ; a++ )
for(unsigned int b = 0 ; b < size ; b++ )
{
double val = I[i-half_size+a][j-half_size+b] ;
conv_x += M[a][b] * val ;
}
If[i][j] = conv_x ;
}
}
}
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;
}
}
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;
}
}
/*!
Apply a filter to an image.
\param I : Image to filter
\param Iu : Filtered image along the horizontal axis (u = columns).
\param Iv : Filtered image along the vertical axis (v = rows).
\param M : Separate filter coefficients
*/
void
vpImageFilter::filter(const vpImage<double> &I,
vpImage<double>& Iu,
vpImage<double>& Iv,
const vpMatrix& M)
{
unsigned int size = M.getRows() ;
unsigned int half_size = size/2 ;
Iu.resize(I.getHeight(),I.getWidth()) ;
Iv.resize(I.getHeight(),I.getWidth()) ;
Iu = 0 ;
Iv = 0 ;
for (unsigned int v=half_size ; v < I.getHeight()-half_size ; v++)
{
for (unsigned int u=half_size ; u < I.getWidth()-half_size ; u++)
{
double conv_u = 0 ;
double conv_v = 0 ;
for(unsigned int a = 0 ; a < size ; a++ )
for(unsigned int b = 0 ; b < size ; b++ )
{
double val = I[v-half_size+a][u-half_size+b] ;
conv_u += M[a][b] * val ;
conv_v += M[b][a] * val ;
}
Iu[v][u] = conv_u ;
Iv[v][u] = conv_v ;
}
}
}
void KalmanFilter<DataTypes, DepthTypes>::predict(vpMatrix& stiffnessMatrix)
{
predictedForces = estimatedForces;
predictedPositions = estimatedPositions + stiffnessMatrix.pseudoInverse()*estimatedForces;
for (int k = 0; k < 2*predictedForces.getCols(); k++)
for (int l = 0; l < 2*predictedForces.getCols(); l++)
std::cout <<k<<" "<<l<<" "<< stiffnessMatrix[k][l] << std::endl;
estimatedState = estimatedPositions;
estimatedState.stack(estimatedForces);
predictedState = predictedPositions;
predictedState.stack(predictedForces);
for (int k = predictedForces.getCols(); k < 2*predictedForces.getCols(); k++)
for (int l = predictedForces.getCols(); l < 2*predictedForces.getCols(); l++)
J[k][l] = stiffnessMatrix[k-predictedForces.getCols()][l-predictedForces.getCols()];
PPred = J*PEst*J.transpose() + Q;
//PfPred = PsEst + Qf;
}
/*!
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;
}
/*!
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];
}
/*!
\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 ;
}
vpColVector computeSecondaryTaskManipulability(const vpMatrix &P, vpMatrix &J, std::vector <vpMatrix> &dJ,double & cond)
{
const unsigned int n = dJ.size();
vpColVector z(n);
double alpha = 10;
// vpMatrix v;
// vpColVector w;
// J.svd(w, v);
// //std::cout << "singular values:/n" << w << std::endl;
// cond = w[0]/w[5];
// double sqtr_detJJt = 1.0;
// for (unsigned int i = 0; i < n-1; i++)
// sqtr_detJJt *= w[i];
// std::cout << "sqtr_detJJt:/n" << sqtr_detJJt << std::endl;
// std::cout << "detJJt" << detJJt << std::endl;
double detJJt = (J * J.transpose()).det();
std::cout << "sqtr_detJJMulti" << sqrt(detJJt) << std::endl;
for (unsigned int i = 0; i < n; i++)
{
vpMatrix dJJinv = dJ[i] * J.pseudoInverse();
std::cout << dJ[i] << std::endl << dJ[i] << std::endl;
double trace = 0.0;
for (unsigned int k = 0; k < dJJinv.getCols(); k++)
trace += dJJinv[k][k];
std::cout << "trace" << i << " " << trace << std::endl;
z[i] = alpha * sqrt(detJJt) * trace;
}
return z;
}
// 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();
}
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;
}
/*!
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;
}
/*!
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 ;
}
/*!
\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];
}
/*!
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];
}
/*!
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;
}
/*!
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
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;
}
