/** * @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; }