/*!
\brief Initialisation of a the subColVector
\param v : parent col vector
\param offset : offset where subColVector start in the parent colVector
\param nrows : size of the subColVector
*/
void vpSubColVector::init(vpColVector &v,
const unsigned int & offset,
const unsigned int & nrows){
if(!v.data){
vpERROR_TRACE("\n\t\t vpSubColvector parent vpColVector has been destroyed");
throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
"\n\t\t \n\t\t vpSubColvector parent vpColVector has been destroyed")) ;
}
if(offset+nrows<=v.getRows()){
data=v.data+offset;
rowNum=nrows;
colNum = 1;
pRowNum=v.getRows();
parent=&v;
if(rowPtrs){
free(rowPtrs);
}
rowPtrs=(double**)malloc( parent->getRows() * sizeof(double*));
for(unsigned int i=0;i<nrows;i++)
rowPtrs[i]=v.data+i+offset;
dsize = rowNum ;
trsize =0 ;
}else{
vpERROR_TRACE("SubColVector cannot be contain in parent ColVector") ;
throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,"SubColVector cannot be contain in parent ColVector")) ;
}
}
/*!
Insert a column vector.
\param i : Index of the first element to introduce. This index starts from 0.
\param v : Column vector to insert.
The following example shows how to use this function:
\code
#include <visp3/core/vpColVector.h>
int main()
{
vpColVector v(4);
for (unsigned int i=0; i < v.size(); i++)
v[i] = i;
std::cout << "v: " << v.t() << std::endl;
vpColVector w(2);
for (unsigned int i=0; i < w.size(); i++)
w[i] = i+10;
std::cout << "w: " << w.t() << std::endl;
v.insert(1, w);
std::cout << "v: " << v.t() << std::endl;
}
\endcode
It produces the following output:
\code
v: 0 1 2 3
w: 10 11
v: 0 10 11 3
\endcode
*/
void vpColVector::insert(unsigned int i, const vpColVector &v)
{
if (i+v.size() > this->size())
throw(vpException(vpException::dimensionError, "Unable to insert a column vector"));
for (unsigned int j=0; j < v.size(); j++)
(*this)[i+j] = v[j];
}
// Fit model to this random selection of data points.
void
vpHomography::computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M)
{
unsigned int i ;
unsigned int n = x.getRows()/4 ;
std::vector<double> xa(4), xb(4);
std::vector<double> ya(4), yb(4);
unsigned int n2 = n * 2;
unsigned int ind2;
for(i=0 ; i < 4 ; i++)
{
ind2 = 2 * ind[i];
xb[i] = x[ind2] ;
yb[i] = x[ind2+1] ;
xa[i] = x[n2+ind2] ;
ya[i] = x[n2+ind2+1] ;
}
vpHomography aHb ;
try {
vpHomography::HLM(xb, yb, xa, ya, true, aHb);
}
catch(...)
{
aHb.setIdentity();
}
M.resize(9);
for (i=0 ; i <9 ; i++)
{
M[i] = aHb.data[i] ;
}
aHb /= aHb[2][2] ;
}
/*!
Compute end return the dot product of two column vectors:
\f[ a \cdot b = \sum_{i=0}^n a_i * b_i\f] where \e n is the dimension of both vectors.
\exception vpException::dimensionError If the vector dimension differ.
*/
double
vpColVector::dotProd(const vpColVector &a, const vpColVector &b)
{
if (a.data==NULL) {
throw(vpException(vpException::fatalError,
"Cannot compute the dot product: first vector empty")) ;
}
if (b.data==NULL) {
throw(vpException(vpException::fatalError,
"Cannot compute the dot product: second vector empty")) ;
}
if (a.size() != b.size()) {
throw(vpException(vpException::dimensionError,
"Cannot compute the dot product between column vectors with different dimensions (%d) and (%d)",
a.size(), b.size()));
}
double *ad = a.data ; double *bd = b.data ;
double c = 0 ;
for (unsigned int i=0 ; i < a.getRows() ; i++)
c += *(ad++)* *(bd++) ;
// vpMatrix c = (a.t() * b);
// return c[0][0];
return c ;
}
/*!
Stack column vectors.
\param A : Initial vector.
\param B : Vector to stack at the end of A.
\param C : Resulting stacked vector \f$C = [A B]^T\f$.
\code
vpColVector A(3);
vpColVector B(5);
vpColVector C;
vpColVector::stack(A, B, C); // C = [A B]T
// C is now an 8 dimension column vector
\endcode
\sa stack(const vpColVector &)
\sa stack(const vpColVector &, const vpColVector &)
*/
void vpColVector::stack(const vpColVector &A, const vpColVector &B, vpColVector &C)
{
unsigned int nrA = A.getRows();
unsigned int nrB = B.getRows();
if (nrA == 0 && nrB == 0) {
C.resize(0);
return;
}
if (nrB == 0) {
C = A;
return;
}
if (nrA == 0) {
C = B;
return;
}
// General case
C.resize(nrA + nrB);
for (unsigned int i=0; i<nrA; i++)
C[i] = A[i];
for (unsigned int i=0; i<nrB; i++)
C[nrA+i] = B[i];
}
/*!
Compute and return the cross product of two vectors \f$a \times b\f$.
\param a : 3-dimension column vector.
\param b : 3-dimension column vector.
\return The cross product \f$a \times b\f$.
\exception vpException::dimensionError If the vectors dimension is not equal to 3.
*/
vpColVector vpColVector::crossProd(const vpColVector &a, const vpColVector &b)
{
if (a.getRows() != 3 || b.getRows() != 3) {
throw(vpException(vpException::dimensionError,
"Cannot compute the cross product between column vector with dimension %d and %d",
a.getRows(), b.getRows()));
}
return vpColVector::skew(a) * b;
}
/*!
This function enables you to add new points in all curves of a plot. These points are drawn with the parameters of the curves.
\param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
\param x : The coordinate of the new points along the x axis and given in the user unit system.
\param vy : The coordinates of the new points along the y axis and given in the user unit system.
\param vz : The coordinates of the new points along the z axis and given in the user unit system.
*/
void vpPlot::plot(const int graphNum, const double x, const vpColVector vy, const vpColVector vz)
{
if((graphList+graphNum)->curveNbr == vy.getRows() && (graphList+graphNum)->curveNbr == vz.getRows())
{
for(int i = 0;i < vy.getRows();++i)
this->plot(graphNum, i, x, vy[i], vz[i]);
}
else
vpTRACE("error in plot vector : not the right dimension");
}
/*!
This function enables you to add new points in all curves of a plot. These points are drawn with the parameters of the curves.
\param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
\param x : The coordinate of the new points along the x axis and given in the user unit system.
\param v_y : y coordinates vector. The coordinates of the new points along the y axis and given in the user unit system.
*/
void vpPlot::plot(const unsigned int graphNum,
const double x, const vpColVector &v_y)
{
if((graphList+graphNum)->curveNbr == v_y.getRows())
{
for(unsigned int i = 0;i < v_y.getRows();++i)
this->plot(graphNum, i, x, v_y[i]);
}
else
vpTRACE("error in plot vector : not the right dimension");
}
// 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]);
}
}
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
}
void
vpMbKltTracker::computeVVSWeights(const unsigned int iter, const unsigned int nbInfos, const vpColVector &R,
vpColVector &w_true, vpColVector &w, vpRobust &robust) {
if(iter == 0){
w_true.resize(2*nbInfos);
w.resize(2*nbInfos);
w = 1;
w_true = 1;
}
robust.setIteration(iter);
robust.setThreshold(2/cam.get_px());
robust.MEstimator( vpRobust::TUKEY, R, w);
}
//! operator substraction of two vectors V = A-v
vpColVector vpColVector::operator-(const vpColVector &m) const
{
if (getRows() != m.getRows() ) {
throw(vpException(vpException::dimensionError,
"Bad size during vpColVector (%dx1) and vpColVector (%dx1) substraction",
getRows(), m.getRows())) ;
}
vpColVector v(rowNum);
for (unsigned int i=0;i<rowNum;i++)
v[i] = (*this)[i] - m[i];
return v;
}
/*!
This function enables you to add new points in all curves of a plot. These points are drawn with the parameters of the curves.
\param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
\param x : The coordinate of the new points along the x axis and given in the user unit system.
\param v_y : y coordinates vector. The coordinates of the new points along the y axis and given in the user unit system.
\param v_z : z coordinates vector. The coordinates of the new points along the z axis and given in the user unit system.
*/
vpMouseButton::vpMouseButtonType
vpPlot::plot(const unsigned int graphNum, const double x, const vpColVector &v_y, const vpColVector &v_z)
{
vpMouseButton::vpMouseButtonType button = vpMouseButton::none;
if((graphList+graphNum)->curveNbr == v_y.getRows() && (graphList+graphNum)->curveNbr == v_z.getRows())
{
for(unsigned int i = 0;i < v_y.getRows();++i)
button = this->plot(graphNum, i, x, v_y[i], v_z[i]);
}
else
vpTRACE("error in plot vector : not the right dimension");
return button;
}
/*!
Compute the mean value of all the elements of the vector.
*/
double vpColVector::mean(const vpColVector &v)
{
if (v.data==NULL) {
throw(vpException(vpException::fatalError,
"Cannot compute column vector mean: vector empty")) ;
}
double mean = 0 ;
double *vd = v.data ;
for (unsigned int i=0 ; i < v.getRows() ; i++)
mean += *(vd++) ;
return mean/v.getRows();
}
/*
Change the velocity frame
\brief :
The velocituy results from the visual servoing control
law. It is basically expressed in the camera frame
Using the current measurements, we can compute the
transformation between the camera frame in the waist
wMc and the correponding Twist wVc
and use it to change the velocity frame.
return : 0 if ok
-1 if the input velocity is not of size 6
warning : attention nothing garanties the size
of poseHeadInFoot and poseWaistInFoot
*/
int HRP2ComputeControlLawProcess::changeVelocityFrame(const vpColVector& velCam,
vpColVector& velWaist,
const double *poseHeadInFoot,
const double *poseWaistInFoot,
vpHomogeneousMatrix &afMh )
{
// test on the input velocity
if(velCam.getRows()!=6)
{
return -1;
}
velWaist.resize(6);
//Express rotation in theta U
vpRxyzVector headInFootRxyz (poseHeadInFoot[3],
poseHeadInFoot[4],
poseHeadInFoot[5]);
vpRxyzVector waistInFootRxyz (poseWaistInFoot[3],
poseWaistInFoot[4],
poseWaistInFoot[5]);
vpThetaUVector headInFootThU (headInFootRxyz);
vpThetaUVector waistInFootThU (waistInFootRxyz);
//Create translation vectors
vpTranslationVector headInFootT (poseHeadInFoot[0],
poseHeadInFoot[1],
poseHeadInFoot[2]);
vpTranslationVector waistInFootT(poseWaistInFoot[0],
poseWaistInFoot[1],
poseWaistInFoot[2]);
//Create the corresponding homogeneousMatrix
vpHomogeneousMatrix fMh (headInFootT ,headInFootThU);
vpHomogeneousMatrix fMw (waistInFootT, waistInFootThU);
afMh=fMh;
//Compute the position of the head in the waist
vpHomogeneousMatrix wMh = fMw.inverse()*fMh;
//Compute the associate twist matrix
vpVelocityTwistMatrix wVh (wMh);
//The position of the current camera in the head should
// have been previously loaded
//Change the velocity frame
velWaist = wVh*m_hVc*velCam;
return 0;
}
// Evaluate distances between points and model.
double
vpHomography::computeResidual(vpColVector &x, vpColVector &M, vpColVector &d)
{
unsigned int i ;
unsigned int n = x.getRows()/4 ;
unsigned int n2 = n * 2;
unsigned int i2;
vpColVector *pa;
vpColVector *pb;
pa = new vpColVector [n];
pb = new vpColVector [n];
for( i=0 ; i < n ; i++)
{
i2 = 2 * i;
pb[i].resize(3) ;
pb[i][0] = x[i2] ;
pb[i][1] = x[i2+1] ;
pb[i][2] = 1;
pa[i].resize(3) ;
pa[i][0] = x[n2+i2] ;
pa[i][1] = x[n2+i2+1] ;
pa[i][2] = 1;
}
vpMatrix aHb(3,3) ;
for (i=0 ; i <9 ; i++)
{
aHb.data[i] = M[i];
}
aHb /= aHb[2][2];
d.resize(n);
vpColVector Hpb ;
for (i=0 ; i <n ; i++)
{
Hpb = aHb*pb[i] ;
Hpb /= Hpb[2] ;
d[i] = sqrt((pa[i] - Hpb ).sumSquare()) ;
}
delete [] pa;
delete [] pb;
return 0 ;
}
/*!
Get the robot displacement since the last call of this method.
\warning The first call of this method gives not a good value for the
displacement.
\param frame The frame in which the measured displacement is expressed.
\param d The displacement:
- In articular, the dimension of q is 2 (the number of axis of the robot)
with respectively d[0] (pan displacement), d[1] (tilt displacement).
- In camera frame, the dimension of d is 6 (tx, ty, ty, tux, tuy, tuz).
Translations are expressed in meters, rotations in radians with the theta U
representation.
\exception vpRobotException::wrongStateError If a not supported frame type is
given.
\sa getArticularDisplacement(), getCameraDisplacement()
*/
void
vpRobotBiclops::getDisplacement(vpRobot::vpControlFrameType frame,
vpColVector &d)
{
vpColVector q_current; // current position
getPosition(vpRobot::ARTICULAR_FRAME, q_current);
switch(frame) {
case vpRobot::ARTICULAR_FRAME:
d.resize(vpBiclops::ndof);
d = q_current - q_previous;
break ;
case vpRobot::CAMERA_FRAME: {
d.resize(6);
vpHomogeneousMatrix fMc_current;
vpHomogeneousMatrix fMc_previous;
fMc_current = vpBiclops::get_fMc(q_current);
fMc_previous = vpBiclops::get_fMc(q_previous);
vpHomogeneousMatrix c_previousMc_current;
// fMc_c = fMc_p * c_pMc_c
// => c_pMc_c = (fMc_p)^-1 * fMc_c
c_previousMc_current = fMc_previous.inverse() * fMc_current;
// Compute the instantaneous velocity from this homogeneous matrix.
d = vpExponentialMap::inverse( c_previousMc_current );
break ;
}
case vpRobot::REFERENCE_FRAME:
vpERROR_TRACE ("Cannot get a velocity in the reference frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot get a velocity in the reference frame:"
"functionality not implemented");
break ;
case vpRobot::MIXT_FRAME:
vpERROR_TRACE ("Cannot get a velocity in the mixt frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot get a velocity in the mixt frame:"
"functionality not implemented");
break ;
}
q_previous = q_current; // Update for next call of this method
}
//! Operator that allows to add two column vectors.
vpColVector
vpColVector::operator+(const vpColVector &v) const
{
if (getRows() != v.getRows() ) {
throw(vpException(vpException::dimensionError,
"Cannot add (%dx1) column vector to (%dx1) column vector",
getRows(), v.getRows())) ;
}
vpColVector r(rowNum);
for (unsigned int i=0;i<rowNum;i++)
r[i] = (*this)[i] + v[i];
return r;
}
/*!
Operator that performs the dot product between two column vectors.
\exception vpException::dimensionError If the vector dimension differ.
\sa dotProd()
*/
double
vpColVector::operator*(const vpColVector &v) const
{
if (size() != v.size()) {
throw(vpException(vpException::dimensionError,
"Cannot compute the dot product between column vectors with different dimensions (%d) and (%d)",
size(), v.size()));
}
double r = 0 ;
for (unsigned int i=0;i<rowNum;i++)
r += (*this)[i] * v[i];
return r;
}
/*!
Compute the median value of all the element of the vector
*/
double
vpColVector::median(const vpColVector &v)
{
if (v.data==NULL)
{
vpERROR_TRACE("vpColVector v non initialized") ;
throw(vpMatrixException(vpMatrixException::notInitializedError)) ;
}
unsigned int i,j;
unsigned int inf, sup;
unsigned int n = v.getRows() ;
vpColVector infsup(n) ;
for (i=0;i<v.getRows();i++)
{
// We compute the number of element gretear (sup) than the current
// value and the number of element smaller (inf) than the current
// value
inf = sup = 0;
for (j=0;j<v.getRows();j++)
{
if (i != j)
{
if (v[i] <= v[j]) inf++;
if (v[i] >= v[j]) sup++;
}
}
// We compute then difference between inf and sup
// the median should be for |inf-sup| = 0 (1 if an even number of element)
// which means that there are the same number of element in the array
// that are greater and smaller that this value.
infsup[i] = fabs((double)(inf-sup)); //EM gcc 4.3
}
// seek for the smaller value of |inf-sup| (should be 0 or 1)
unsigned int imin=0 ; // index of the median in the array
// min cannot be greater than the number of element
double min = v.getRows();
for (i=0;i<v.getRows();i++)
if (infsup[i] < min)
{
min = infsup[i];
imin = i ;
}
// return the median
return(v[imin]);
}
/*!
Get the robot displacement since the last call of this method.
\warning The first call of this method gives not a good value for the
displacement.
\param frame The frame in which the measured displacement is expressed.
\param d The displacement:
- In articular, the dimension of q is 2 (the number of axis of the robot)
with respectively d[0] (pan displacement), d[1] (tilt displacement).
- In camera frame, the dimension of d is 6 (tx, ty, ty, rx, ry,
rz). Translations are expressed in meters, rotations in radians.
\exception vpRobotException::wrongStateError If a not supported frame type is
given.
\sa getArticularDisplacement(), getCameraDisplacement()
*/
void
vpRobotPtu46::getDisplacement(vpRobot::vpControlFrameType frame,
vpColVector &d)
{
double d_[6];
switch (frame)
{
case vpRobot::CAMERA_FRAME:
{
d.resize (6);
ptu.measureDpl(d_, PTU_CAMERA_FRAME);
d[0]=d_[0];
d[1]=d_[1];
d[2]=d_[2];
d[3]=d_[3];
d[4]=d_[4];
d[5]=d_[5];
break ;
}
case vpRobot::ARTICULAR_FRAME:
{
ptu.measureDpl(d_, PTU_ARTICULAR_FRAME);
d.resize (vpPtu46::ndof);
d[0]=d_[0]; // pan
d[1]=d_[1]; // tilt
break ;
}
case vpRobot::REFERENCE_FRAME:
{
vpERROR_TRACE ("Cannot get a displacement in the reference frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot get a displacement in the reference frame:"
"functionality not implemented");
break ;
}
case vpRobot::MIXT_FRAME:
{
vpERROR_TRACE ("Cannot get a displacement in the mixt frame: "
"functionality not implemented");
throw vpRobotException (vpRobotException::wrongStateError,
"Cannot get a displacement in the reference frame:"
"functionality not implemented");
break ;
}
}
}
/*!
Compute the median value of all the elements of the vector.
*/
double
vpColVector::median(const vpColVector &v)
{
if (v.data==NULL) {
throw(vpException(vpException::fatalError,
"Cannot compute column vector median: vector empty")) ;
}
std::vector<double> vectorOfDoubles(v.size());
for(unsigned int i = 0; i < v.size(); i++) {
vectorOfDoubles[i] = v[i];
}
return vpMath::getMedian(vectorOfDoubles);
}
/*
Get the current position of the robot.
\param frame : Control frame type in which to get the position, either :
- in the camera cartesien frame,
- joint (articular) coordinates of each axes (not implemented)
- in a reference or fixed cartesien frame attached to the robot base
- in a mixt cartesien frame (translation in reference frame, and rotation in camera frame)
\param position : Measured position of the robot:
- in camera cartesien frame, a 6 dimension vector, set to 0.
- in articular, this functionality is not implemented.
- in reference frame, a 6 dimension vector, the first 3 values correspond to
the translation tx, ty, tz in meters (like a vpTranslationVector), and the
last 3 values to the rx, ry, rz rotation (like a vpRxyzVector).
*/
void vpSimulatorPioneer::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
{
q.resize (6);
switch (frame) {
case vpRobot::CAMERA_FRAME :
q = 0;
break;
case vpRobot::ARTICULAR_FRAME :
std::cout << "ARTICULAR_FRAME is not implemented in vpSimulatorPioneer::getPosition()" << std::endl;
break;
case vpRobot::REFERENCE_FRAME : {
// Convert wMc_ to a position
// From fMc extract the pose
vpRotationMatrix wRc;
this->wMc_.extract(wRc);
vpRxyzVector rxyz;
rxyz.buildFrom(wRc);
for (unsigned int i=0; i < 3; i++) {
q[i] = this->wMc_[i][3]; // translation x,y,z
q[i+3] = rxyz[i]; // Euler rotation x,y,z
}
break;
}
case vpRobot::MIXT_FRAME :
std::cout << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" << std::endl;
}
}
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;
}
}
/*!
Return a column vector with elements of \e v that are sorted.
\sa invSort()
*/
vpColVector
vpColVector::sort(const vpColVector &v)
{
if (v.data==NULL) {
throw(vpException(vpException::fatalError,
"Cannot sort content of column vector: vector empty")) ;
}
vpColVector tab ;
tab = v ;
unsigned int nb_permutation = 1 ;
unsigned int i = 0 ;
while (nb_permutation !=0 )
{
nb_permutation = 0 ;
for (unsigned int j = v.getRows()-1 ; j >= i+1 ; j--)
{
if ((tab[j]<tab[j-1]))
{
double tmp = tab[j] ;
tab[j] = tab[j-1] ;
tab[j-1] = tmp ;
nb_permutation++ ;
}
}
i++ ;
}
return tab ;
}
void vpRobust::psiTukey(double sig, vpColVector &x, vpColVector & weights)
{
unsigned int n_data = x.getRows();
double cst_const = vpCST*4.6851;
for(unsigned int i=0; i<n_data; i++)
{
//if(sig==0 && weights[i]!=0)
if(std::fabs(sig) <= std::numeric_limits<double>::epsilon() && std::fabs(weights[i]) > std::numeric_limits<double>::epsilon())
{
weights[i]=1;
continue;
}
double xi_sig = x[i]/sig;
//if((fabs(xi_sig)<=(cst_const)) && weights[i]!=0)
if((std::fabs(xi_sig)<=(cst_const)) && std::fabs(weights[i]) > std::numeric_limits<double>::epsilon())
{
weights[i] = vpMath::sqr(1-vpMath::sqr(xi_sig/cst_const));
//w[i] = vpMath::sqr(1-vpMath::sqr(x[i]/sig/4.7));
}
else
{
//Outlier - could resize list of points tracked here?
weights[i] = 0;
}
}
}
/*!
Initialize the column vector from a part of an input column vector \e v.
\param v : Input column vector used for initialization.
\param r : row index in \e v that corresponds to the first element of the column vector to contruct.
\param nrows : Number of rows of the constructed column vector.
The sub-vector starting from v[r] element and ending on v[r+nrows-1] element
is used to initialize the contructed column vector.
The following code shows how to use this function:
\code
#include <visp3/core/vpColVector.h>
int main()
{
vpColVector v(4);
int val = 0;
for(size_t i=0; i<v.getRows(); i++) {
v[i] = val++;
}
std::cout << "v: " << v.t() << std::endl;
vpColVector w;
w.init(v, 0, 2);
std::cout << "w: " << w.t() << std::endl;
}
\endcode
It produces the following output:
\code
v: 0 1 2 3
w: 1 2
\endcode
*/
void
vpColVector::init(const vpColVector &v, unsigned int r, unsigned int nrows)
{
unsigned int rnrows = r+nrows ;
if (rnrows > v.getRows())
throw(vpException(vpException::dimensionError,
"Bad row dimension (%d > %d) used to initialize vpColVector",
rnrows, v.getRows()));
resize(nrows);
if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
return; // Noting to do
for (unsigned int i=r ; i < rnrows; i++)
(*this)[i-r] = v[i];
}
// Fill in the plot message for publishing data to be plotted in the /VS_errors topic
void makePlotMSG(std_msgs::Float64MultiArray &plotMsg,
const vpColVector &taskError,
const vpColVector &v)
{
// add the errors
for(unsigned int i=0; i<taskError.getRows(); i++)
{
plotMsg.data.push_back(taskError[i]);
}
// add velocity twist
for(unsigned int i=0; i<v.getRows(); i++)
{
plotMsg.data.push_back(v[i]);
}
}
vpColVector
vpColVector::sort(const vpColVector &v)
{
if (v.data==NULL) {
vpERROR_TRACE("vpColVector a non initialized") ;
throw(vpMatrixException(vpMatrixException::notInitializedError)) ;
}
vpColVector tab ;
tab = v ;
unsigned int nb_permutation = 1 ;
unsigned int i = 0 ;
while (nb_permutation !=0 )
{
nb_permutation = 0 ;
for (unsigned int j = v.getRows()-1 ; j >= i+1 ; j--)
{
if ((tab[j]<tab[j-1]))
{
double tmp = tab[j] ;
tab[j] = tab[j-1] ;
tab[j-1] = tmp ;
nb_permutation++ ;
}
}
i++ ;
}
return tab ;
}
/*
\brief
Function to determine if a set of 4 pairs of matched points give rise
to a degeneracy in the calculation of a homography as needed by RANSAC.
This involves testing whether any 3 of the 4 points in each set is
colinear.
point are coded this way
x1b,y1b, x2b, y2b, ... xnb, ynb
x1a,y1a, x2a, y2a, ... xna, yna
leading to 2*2*n
*/
bool
vpHomography::degenerateConfiguration(vpColVector &x, unsigned int *ind)
{
for (unsigned int i = 1; i < 4 ; i++)
for (unsigned int j = 0 ;j < i ; j++)
if (ind[i] == ind[j]) return true ;
unsigned int n = x.getRows()/4;
double pa[4][3];
double pb[4][3];
unsigned int n2 = 2 * n;
unsigned int ind2;
for(unsigned int i = 0; i < 4 ;i++)
{
ind2 = 2 * ind[i];
pb[i][0] = x[ind2];
pb[i][1] = x[ind2+1];
pb[i][2] = 1;
pa[i][0] = x[n2+ind2] ;
pa[i][1] = x[n2+ind2+1] ;
pa[i][2] = 1;
}
return ( iscolinear(pa[0],pa[1],pa[2]) ||
iscolinear(pa[0],pa[1],pa[3]) ||
iscolinear(pa[0],pa[2],pa[3]) ||
iscolinear(pa[1],pa[2],pa[3]) ||
iscolinear(pb[0],pb[1],pb[2]) ||
iscolinear(pb[0],pb[1],pb[3]) ||
iscolinear(pb[0],pb[2],pb[3]) ||
iscolinear(pb[1],pb[2],pb[3]));
}
