/**
* @brief A point of the road is visible if it is between the robot and the laser beam running through it, and if the previous point was visible
* All points in the road are updated
* @param road ...
* @param laserData ...
* @return bool
*/
bool ElasticBand::checkVisiblePoints(InnerModel *innermodel, WayPoints &road, const RoboCompLaser::TLaserData &laserData)
{
//Simplify laser polyline using Ramer-Douglas-Peucker algorithm
std::vector<Point> points, res;
QVec wd;
for (auto &ld : laserData)
{
//wd = innermodel->laserTo("world", "laser", ld.dist, ld.angle); //OPTIMIZE THIS FOR ALL CLASS METHODS
wd = innermodel->getNode<InnerModelLaser>("laser")->laserTo("world", ld.dist, ld.angle);
points.push_back(Point(wd.x(), wd.z()));
}
res = simPath.simplifyWithRDP(points, 70);
//qDebug() << __FUNCTION__ << "laser polygon after simp" << res.size();
// Create a QPolygon so we can check if robot outline falls inside
QPolygonF polygon;
for (auto &p: res)
polygon << QPointF(p.x, p.y);
// Move the robot along the road
int robot = road.getIndexOfNextPoint();
QVec memo = innermodel->transform6D("world", "robot");
for(int it = robot; it<road.size(); ++it)
{
road[it].isVisible = true;
innermodel->updateTransformValues("robot", road[it].pos.x(), road[it].pos.y(), road[it].pos.z(), 0, road[it].rot.y(), 0);
//get Robot transformation matrix
QMat m = innermodel->getTransformationMatrix("world", "robot");
// Transform all points at one to world RS
//m.print("m");
//pointsMat.print("pointsMat");
QMat newPoints = m * pointsMat;
//Check if they are inside the laser polygon
for (int i = 0; i < newPoints.nCols(); i++)
{
// qDebug() << __FUNCTION__ << "----------------------------------";
// qDebug() << __FUNCTION__ << QPointF(newPoints(0, i), newPoints(2, i));
// qDebug() << __FUNCTION__ << polygon;
if (polygon.containsPoint(QPointF(newPoints(0, i), newPoints(2, i)),Qt::OddEvenFill) == false)
{
road[it].isVisible = false;
//qFatal("fary");
break;
}
}
// if( road[it].isVisible == false)
// {
// for (int k = it; k < road.size(); ++k)
// road[k].isVisible = false;
// break;
// }
}
// Set the robot back to its original state
innermodel->updateTransformValues("robot", memo.x(), memo.y(), memo.z(), 0, memo.ry(), 0);
//road.print();
return true;
}
void RMat::QMat::SVD(QMat & U, QMat & D, QMat & V)
{
#ifdef COMPILE_IPP
// double ippA[rows*cols];
double ippU[rows*rows], ippD[rows*cols], ippV[cols*cols];
int i;
U = QMat(rows, rows);
D = QMat(rows, cols);
V = QMat(cols, cols);
// for(i=0; i<rows*cols;i++)
// ippA[i] = toData()[i];
for(i=0; i<rows*rows;i++)
U.toData()[i] = ippU[i];
for(i=0; i<cols;i+=1)
D.toData()[i*(cols+1)] = ippD[i];
for(i=0; i<cols*cols;i++)
V.toData()[i] = ippV[i];
#else
qFatal("QMat does not support eigen svd");
#endif
}
/**
* \brief Matrix to matrix product operator; \f$ C = this * A \f$
*
* IPP coge (columnas, filas) en las llamadas
* @param A matrix factor for operation
* @return QMat New matrix result
*/
QMat QMat::operator * ( const QMat & A ) const
{
// printf("Operator *: (%d,%d) x (%d,%d)\n", rows, cols, A.rows, A.cols);
QMat C=zeros( rows, A.nCols() );
if ( cols != A.nRows())
{
QString ex= "QMat::operator* - a.cols!=b.rows";
throw ex;
}
else
{
#ifdef COMPILE_IPP
ippmMul_mm_32f ( toDataConst(), cols*sizeof ( T ), sizeof ( T ), cols, rows, A.toDataConst(), A.nCols() *sizeof ( T ), sizeof ( T ), A.nCols(), A.nRows(), C.toData(), C.nCols() *sizeof ( T ), sizeof ( T ) );
#else
for(int i=0;i<rows;i++)
{
for(int j=0;j<A.cols;j++)
{
C(i,j)=0;
for(int k=0;k<cols;k++)
{
C(i,j) += operator()(i,k)*A(k,j);
}
// printf("%f ", C(i,j));
}
// printf("\n");
}
#endif
}
return C;
}
/**
* \brief Matrix to vector product operator; \f$ C = this * vector \f$
* @param A vector factor for operation
* @return QVec New vector result
*/
QVec RMat::QMat::operator *(const QVec & vector) const
{
Q_ASSERT ( cols == vector.size());
QMat aux = vector.toColumnMatrix();
QMat result = operator*(aux);
return result.toVector();
}
QMat RMat::QMat::sqrt( )
{
int i;
QMat R ( rows, cols );
for ( i=0;i< rows*cols;i++ )
R.getWriteData()[i] = ::sqrt ( fabs ( getReadData()[i]) );
return R;
}
/**
* \brief Copy function
*
* Return a copy of this matrix
* @return QMat matrix copied
*/
QMat QMat::copy()
{
QMat A (rows, cols);
#ifdef COMPILE_IPP
ippmCopy_ma_32f_SS ( toData(), rows*cols*sizeof ( T ), cols*sizeof ( T ), sizeof ( T ), A.toData(),rows*cols*sizeof ( T ), cols*sizeof ( T ), sizeof ( T ), cols, rows, 1 );
#else
memcpy(A.toData(), toData(), rows*cols*sizeof(T));
#endif
return A;
}
/**
* \brief Change the order of the indexes in the matrix \f$ C = this^t \f$
* @return QMat new matrix with indexes reordered
*/
QMat QMat::transpose( ) const
{
QMat C ( cols, rows );
#ifdef COMPILE_IPP
ippmTranspose_m_32f ( toDataConst(), cols*sizeof ( T ), sizeof ( T ), cols, rows, C.toData(), rows*sizeof ( T ), sizeof ( T ) );
#else
for (int fila=0; fila<rows; fila++ )
for ( int columna=0; columna<cols; columna++ )
C(columna,fila) = operator()(fila,columna);
#endif
return C;
}
/**
* @brief Moves a virtual copy of the robot along the road checking for enough free space around it
*
* @param innermodel ...
* @param road ...
* @param laserData ...
* @param robotRadius ...
* @return bool
*/
bool ElasticBand::checkCollisionAlongRoad(InnerModel *innermodel, const RoboCompLaser::TLaserData& laserData, WayPoints &road, WayPoints::const_iterator robot,
WayPoints::const_iterator target, float robotRadius)
{
//Simplify laser polyline using Ramer-Douglas-Peucker algorithm
std::vector<Point> points, res;
QVec wd;
for( auto &ld : laserData)
{
wd = innermodel->laserTo("world", "laser", ld.dist, ld.angle); //OPTIMIZE THIS FOR ALL CLASS METHODS
points.push_back(Point(wd.x(), wd.z()));
}
res = simPath.simplifyWithRDP(points, 70);
qDebug() << __FUNCTION__ << "laser polygon after simp" << res.size();
// Create a QPolygon so we can check if robot outline falls inside
QPolygonF polygon;
for (auto &p: res)
polygon << QPointF(p.x, p.y);
// Move the robot along the road
QVec memo = innermodel->transform6D("world","robot");
bool free = false;
for( WayPoints::const_iterator it = robot; it != target; ++it)
{
if( it->isVisible == false)
break;
// compute orientation of the robot at the point
innermodel->updateTransformValues("robot", it->pos.x(), it->pos.y(), it->pos.z(), 0, it->rot.y(), 0);
//get Robot transformation matrix
QMat m = innermodel->getTransformationMatrix("world", "robot");
// Transform all points at one
qDebug() << __FUNCTION__ << "hello2";
m.print("m");
pointsMat.print("pointsMat");
QMat newPoints = m * pointsMat;
qDebug() << __FUNCTION__ << "hello3";
//Check if they are inside the laser polygon
for( int i=0; i<newPoints.nRows(); i++)
if( polygon.containsPoint(QPointF(pointsMat(i,0)/pointsMat(i,3), pointsMat(i,2)/pointsMat(i,3)), Qt::OddEvenFill ) == false)
{
free = false;
break;
}
free = true;
}
qDebug() << __FUNCTION__ << "hello";
// Set the robot back to its original state
innermodel->updateTransformValues("robot", memo.x(), memo.y(), memo.z(), 0, memo.ry(), 0);
return free ? true : false;
}
/**
* \brief Matrix Subtraction operator: \f$ C = this - A \f$
* @param A Matrix subtrahend
* @return QMat Operation result
*/
QMat QMat::operator- ( const QMat & A ) const
{
Q_ASSERT ( equalSize ( *this, A ));
QMat C ( rows,cols );
#ifdef COMPILE_IPP
ippmSub_mm_32f ( toDataConst(), cols*sizeof ( T ), sizeof ( T ), A.toDataConst(), cols*sizeof ( T ), sizeof ( T ), C.toData(), cols*sizeof ( T ), sizeof ( T ), cols, rows );
#else
for(int r=0; r<A.rows; r++)
for(int c=0; c<A.cols; c++)
C(r, c) = A(r,c) - operator()(r, c);
#endif
return C;
}
/**
* \brief Construct a diagonal matrix
*
* Use diagonal values to create a new diagonal matrix
* @param d Matrix to get diagonal values
* @return QMat new diagonal matrix
*/
QMat QMat::diagonal ( const QMat & d )
{
QMat R ( d.nRows(),d.nRows() );
for (int i=0; i<d.nRows(); i++)
{
for (int j=0; j<d.nCols(); j++)
{
R(i,j) = 0;
}
}
for ( int i=0; i < d.nRows(); i++ )
R ( i,i ) = d ( i );
return R;
}
QMat RMat::QMat::makeDefPos()
{
QVec vals ( rows,0. );
QMat V = eigenValsVectors ( vals );
//V.print("autovectores originales");
for ( int i=0; i< rows; i++ )
{
if ( vals ( i ) <= 0. )
vals ( i ) = 0.0000001;
}
QMat DD = QMat::makeDiagonal ( vals );
QMat R = ( V * ( DD * V.transpose() ) );
return R;
}
QMat QMat::invert( ) const
{
Q_ASSERT( isSquare() );
QMat R ( rows, cols );
#ifdef COMPILE_IPP
T pBuffer[cols*cols+cols];
IppStatus status = ippmInvert_m_32f ( getReadData(), cols*sizeof ( T ), sizeof ( T ), pBuffer, R.toData(), cols*sizeof ( T ), sizeof ( T ), cols );
#else
int status =1;
if(determinant()!=0)
{
status=0;
gsl_matrix *u = *this;
gsl_vector *s = gsl_vector_alloc (cols);
gsl_matrix *v = gsl_matrix_alloc (cols,cols);
gsl_vector *work = gsl_vector_alloc (cols);
gsl_matrix *X = gsl_matrix_alloc (cols,cols);
gsl_linalg_SV_decomp_mod(u, X, v, s, work);
gsl_matrix_free(X);
// pasamos de vuelta:
QMat U = u;
QMat V = v;
gsl_matrix_free(u);
gsl_matrix_free(v);
gsl_vector_free(work);
QMat S = QMat::zeros(rows,cols);
for (int i = 0; i < cols; i++)
S(i,i) = 1 / gsl_vector_get(s, i);
gsl_vector_free(s);
return V * S * U.transpose();
}
#endif
if ( status == 0 )
return R;
else
{
QString ex= "QMat::invert() - Singular matrix";
throw ex;
}
}
QMat RMat::QMat::matSqrt( )
{
Q_ASSERT( isSquare() );
//QMat T = makeDefPos();
QVec vals ( rows );
QMat V = eigenValsVectors ( vals );
//V.print("autovectores");
QMat VI = V.invert();
//VI.print("VI");
QMat D = VI * ( ( *this ) * V );
for ( int i=0; i<rows;i++ )
for ( int j=0;j<cols;j++ )
if ( i != j ) D ( i,j ) =0.;
else D ( i,j ) = ::sqrt ( fabs ( D ( i,j ) ) );
//D.print("D");
QMat R = ( V * ( D * VI ) );
return R;
}
QMat RMat::QMat::eigenValsVectors ( QVec & vals )
{
if ( isSquare() and ( vals.size() ==cols ) )
{
QMat R ( rows,cols,0.);
#ifdef COMPILE_IPP
T valsIm[cols];
int SizeBytes;
ippmEigenValuesVectorsGetBufSize_32f(cols, &SizeBytes);
Ipp8u pBuffer[SizeBytes];
IppStatus status = ippmEigenValuesVectorsRight_m_32f (toData(), cols*sizeof(T), sizeof(T), R.toData(), cols*sizeof(T), sizeof(T), vals.getWriteData(),valsIm, cols, pBuffer);
if ( status == ippStsNoErr )
return R;
else
{
printf ( "EigenValsVectors error: %d, %s\n", status, ippGetStatusString ( status ) );
( *this ).print ( "this" );
QString s="QMat::eigenVectors() - Error returned by IPP"; throw s;
}
#else
// qFatal("eigen values using Eigen has not been properly tested yet");
double data[this->getDataSize()];
const T *dataBuffer = this->getReadData();
for(int i = 0; i < this->getDataSize(); i++)
data[i] = (double)dataBuffer[i];
gsl_matrix_view m = gsl_matrix_view_array (data, rows,rows);
gsl_vector *eval = gsl_vector_alloc (rows);
gsl_matrix *evec = gsl_matrix_alloc (rows, rows);
gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (rows);
gsl_eigen_symmv (&m.matrix, eval, evec, w);
gsl_eigen_symmv_free (w);
gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_DESC);
for(int i=0;i<rows;i++)
vals(i) = gsl_vector_get(eval,i);
QMat P(evec);
gsl_vector_free (eval);
gsl_matrix_free (evec);
return P;
#endif
}
QString s="QMat::EigenValsVectors() - Not Square Matrix"; throw s;
return QMat();
}
/**
* \brief Multiplies a n-column vector times a m-row vector to generate a nxm matrix
* @param vector vector to play the rol of row vector
* @return resulting QMat matrix
*/
QMat QVec::externProduct(const QVec & vector) const
{
Q_ASSERT(size() == vector.size());
const int s = size();
QMat C ( size(), vector.size() );
#ifdef COMPILE_IPP
ippmMul_mm_32f ( getReadData(), s * sizeof ( T ), sizeof ( T ), s , s , vector.getReadData(), vector.size() * sizeof ( T ), sizeof ( T ), vector.size() , vector.size() , C.getWriteData(), vector.size() * sizeof ( T ), sizeof ( T ) );
#else
for(int r=0;r<s;r++)
for(int c=0;c<vector.size();c++)
C(r,c) = this->operator()(r) * vector(c);
#endif
return C;
}
void DMRGCat::SuperBlock::getReNormU(QMat& U, BlockQBase& UBase)const{
QMat waveMat;
GsWave.wave2QMat(PToS->QSpace, PToM->QSpace, PToN->QSpace, PToE->QSpace, waveMat);
waveMat.getReNormUAndBase(Para->getD(), U, UBase);
}
int QMat::minDim ( const QMat &A )
{
return qMin ( A.nRows(), A.nCols() );
}
int QMat::maxDim ( const QMat &A )
{
return qMax ( A.nRows(), A.nCols() );
}
bool QMat::isSquare ( const QMat &A ) const
{
return ( A.nRows() == A.nCols() );
}
bool RMat::QMat::is3ColumnVector(const QMat & A) const
{
return (A.nRows() == 3 and A.nCols() == 1);
}
QMat RMat::QMat::zeros(const int m, const int n)
{
QMat R ( m, n );
R.set(0.);
return R;
}
QMat QMat::ones ( int m, int n )
{
QMat R ( m, n );
R.ones();
return R;
}
QMat QMat::identity(int m)
{
QMat R (m, m);
R.makeIdentity();
return R;
}
