C++ QVec类代码示例

/**
 * \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();
}

bool SpecificWorker::hayCamino()
{

  
  QVec t = inner->transform("robot", ctarget.target, "world");
  float alpha =atan2(t.x(), t.z() );
  float d = t.norm2();
  float x, z;
 //int i = 50;

    for ( uint i = 0; i<ldata.size(); i++ ) {
        if ( ldata[i].angle <= alpha ) {
            if ( ldata[i].dist < d ) {
	        qDebug() <<"NO hay camino";
                return false;
            } else {
                ctarget.activeSub=false;
                qDebug() <<"hay camino";
                return true;
            }
        }
    }
    
    qDebug() <<"NO ve la marca";
    state = State::TURN;
    return false;
}

int SpecificWorker::getMinTag(int flag){
	
	int index = -1;
	QVec targetRobot;
	float min_distance = 10000;

	if (flag == 0){ //If looking for a box
		for(unsigned int i = 0; i < listaMarcas.size(); i++){
			targetRobot = innermodel->transform("world",QVec::vec3(listaMarcas[i].tx, 0, listaMarcas[i].tz) , "rgbd");

			if (targetRobot.norm2() < min_distance && listaMarcas[i].id > 9 && std::find(cajasRecogidas.begin(), cajasRecogidas.end(), listaMarcas[i].id) == cajasRecogidas.end()){
				min_distance = targetRobot.norm2();
				index = i;
				currentBox = listaMarcas[i].id;
			}
		}
	}
	else{ //If looking for a wall tag		
		for(unsigned int i = 0; i < listaMarcas.size(); i++){
			targetRobot = innermodel->transform("world",QVec::vec3(listaMarcas[i].tx, 0, listaMarcas[i].tz) , "rgbd");

			if (targetRobot.norm2() < min_distance && listaMarcas[i].id < 9){
				min_distance = targetRobot.norm2();
				index = i;
			}
		}
		
	}
	
	return index;
}

void SpecificWorker::irASubTarget()
{
 qDebug() <<  __FUNCTION__<<"ir a subTarget";
    QVec t = inner->transform ( "robot", ctarget.subTarget, "world" );
    float alpha =atan2 ( t.x(), t.z() );
    float r= 0.4*alpha;
    float d = t.norm2();

    if ( d<100 ) {
        ctarget.activeSub=false;
        differentialrobot_proxy->setSpeedBase ( 0,0 );
        sleep ( 1 );

    } else {
        if ( fabs ( r ) > 0.2 ) {
            d = 0;
        }
        if ( d>300 ) {
            d=300;
        }
        try {
            differentialrobot_proxy->setSpeedBase ( d,r );
        } catch ( const Ice::Exception &ex ) {
            std::cout << ex << std::endl;
        }
    }
}

/**
 * \brief This method stored the motors's names, the initial position of the motors and the goal
 * position of them. When all is prepared, it raises up the flag READY.
 * @param newAnglesOfMotors an structure with the name of the motors and the value os them.
 */ 
void SpecificWorker::setJointPosition(const MotorAngleList &newAnglesOfMotors)
{
	qDebug()<<"YEAH";
	QMutexLocker ml(mutex);
	if(INITIALIZED == true)
	{
		// 1) SACAMOS VALORES ACTUALES DE LOS MOTORES: (aprovechamos y sacamos motores disponibles)
		QVec firstAngles;
		for(auto motor : newAnglesOfMotors)
		{
			float angle = innerModel->getJoint(QString::fromStdString(motor.name))->getAngle();
			firstAngles.push_back(angle);
			selectedMotors<<QString::fromStdString(motor.name);
		}
		//  2) SACAMOS VALORES FINALES DE LOS MOTORES
		QVec finalAngles;
		for(auto motor : newAnglesOfMotors)
		{
			float angle = motor.angle;
			finalAngles.push_back(angle);
		}
		jointValues.append(firstAngles);
		jointValues.append(finalAngles);
		
		COMPUTE_READY = true;
		
		qDebug()<<"||------------------------------------------------";
		qDebug()<<"|| setJointPosition: jointValues-->"<<jointValues;
		qDebug()<<"||------------------------------------------------";
	}
}

void SpecificWorker::action_ChangeRoom(bool newAction)
{
	static float lastX = std::numeric_limits<float>::quiet_NaN();
	static float lastZ = std::numeric_limits<float>::quiet_NaN();

	auto symbols = worldModel->getSymbolsMap(params, "r2", "robot");


	try
	{
		printf("trying to get _in_ edge from %d to %d\n", symbols["robot"]->identifier, symbols["r2"]->identifier);
		AGMModelEdge e = worldModel->getEdge(symbols["robot"], symbols["r2"], "in");
		printf("STOP! WE ALREADY GOT THERE!\n");
		stop();
		return;
	}
	catch(...)
	{
	}

	int32_t roomId = symbols["r2"]->identifier;
	printf("room symbol: %d\n",  roomId);
	std::string imName = symbols["r2"]->getAttribute("imName");
	printf("imName: <%s>\n", imName.c_str());

	const float refX = str2float(symbols["r2"]->getAttribute("x"));
	const float refZ = str2float(symbols["r2"]->getAttribute("z"));

	QVec roomPose = innerModel->transformS("world", QVec::vec3(refX, 0, refZ), imName);
	roomPose.print("goal pose");
	// 	AGMModelSymbol::SPtr goalRoom = worldModel->getSymbol(str2int(params["r2"].value));
	// 	const float x = str2float(goalRoom->getAttribute("tx"));
	// 	const float z = str2float(goalRoom->getAttribute("tz"));
	const float x = roomPose(0);
	const float z = roomPose(2);

	bool proceed = true;
	if ( (planningState.state=="PLANNING" or planningState.state=="EXECUTING") )
	{
		if (abs(lastX-x)<10 and abs(lastZ-z)<10)
			proceed = false;
		else
			printf("proceed because the coordinates differ (%f, %f), (%f, %f)\n", x, z, lastX, lastZ);
	}
	else
	{
		printf("proceed because it's stoped\n");
	}

	if (proceed)
	{
		lastX = x;
		lastZ = z;
		printf("changeroom to %d\n", symbols["r2"]->identifier);
		go(x, z, 0, false, 0, 0, 1200);
	}
	else
	{
	}
}

void SpecificWorker::compute()
{
 try {
        differentialrobot_proxy->getBaseState ( bState );
        ldata = laser_proxy->getLaserData();
        inner->updateTransformValues ( "robot", bState.x, 0, bState.z, 0, bState.alpha, 0 );

        float alpha;
        QVec t;

        switch ( state ) {
        case State::INIT:
            state = State::IDLE;
            break;

        case State::IDLE:
            break;

        case State::WORKING:
            if ( heLlegado() ) {
                qDebug() << __FUNCTION__<< "Arrived to target" << ctarget.target;
                stopRobot();
                state = State::FINISH;
            } else if ( hayCamino() ) {
                irATarget();
            }

            break;

        case State::TURN:
	    qDebug() << "Buscando punto" << ctarget.target;
            t = inner->transform ( "robot", ctarget.target, "world" );
            alpha =atan2 ( t.x(), t.z() );
            if ( alpha <= ldata.front().angle and alpha >= ldata. back().angle ) {
                stopRobot();
                state = State::WORKING;
            } else
                try {
                    differentialrobot_proxy->setSpeedBase ( 0, 0.4 );
                } catch ( Ice::Exception &ex ) {
                    std::cout<<ex.what() <<std::endl;
                };
            break;

        case State::FINISH:
            sleep ( 2 );
            undrawTarget ( "target" );
            state = State::IDLE;
            break;
        }
    } catch ( const Ice::Exception &e ) {
        std::cout << "Error reading from Camera" << e << std::endl;
    }

    //histogram();
    innerViewer->update();
    osgView->autoResize();
    osgView->frame();
}

/**
 * \brief Computes the coefficients of the implicit line equation: y = mx + n passing by two points
 * from two points (x1,y1) e (x2,y2) satisfying  (y-y1)/(y2-y1) = (x-x1)/(x2-x1),
 * that solving for y gives:  y = x( (y2-y1)/(x2-x1) ) - x1( (y2-y1)/(x2-x1) ) + y1
 * @param p1 containing x1,y1
 * @param p2 containing x2,y2
 * @return a QVec vector of 2 dimensions with m and n
 */
QVec RMat::QVec::line2DImplicitCoefsFrom2Points(const QVec & p1, const QVec & p2) 
{
	Q_ASSERT( p1.size() == 2 and p2.size() == 2 and p2(0)-p1(0) != 0 );
	QVec res(2);
	res(0) = (p2(1)-p1(1))/(p2(0)-p1(0)) ;
	res(1) =   p1(1) - p1(0) * ((p2(1)-p1(1))/(p2(0)-p1(0))) ;
	return res;
}

/**
 * \brief Construct a diagonal matrix 
 *
 * Use vector values to create a new diagonal matrix
 * @param v Vector to get diagonal values
 * @return QMat new diagonal matrix
 */
QMat RMat::QMat::makeDiagonal ( const QVec &v )
{
	QMat R ( v.size(),v.size(), (T)0 );
	int f = v.size();
	for ( int i=0; i<f; i++ )
		R ( i,i ) = v ( i );
	return R;
}

bool Sampler::checkRobotValidDirectionToTargetOneShot(const QVec & origin , const QVec & target) const
{
	//qDebug() << __FUNCTION__ << "Checking between: " << origin << "and " << target;
	
	const float MAX_LENGTH_ALONG_RAY = (target-origin).norm2();
	QVec finalPoint;
	float wRob=420, hRob=1600;  //GET FROM INNERMODEL!!! 
	
// 	if( MAX_LENGTH_ALONG_RAY < 50)   //COMMENT THIS FOR NOW ::::::::::::::::::::...
// 	{
// 		qDebug() << __FUNCTION__ << "target y origin too close";
// 		return false;
// 	}
		
	//Compute angle between origin-target line and world Zaxis
	float alfa1 = QLine2D(target,origin).getAngleWithZAxis();
	//qDebug() << "Angle with Z axis" << origin << target << alfa1;
	
	// Update robot's position and align it with alfa1 so it looks at the TARGET point 	
	innerModelSampler->updateTransformValues("robot", origin.x(), origin.y(), origin.z(), 0., alfa1, 0.);
	
	// Compute rotation matrix between robot and world. Should be the same as alfa
	QMat r1q = innerModelSampler->getRotationMatrixTo("world", "robot");
	
	//qDebug()<< "alfa1" << alfa1 << r1q.extractAnglesR_min().y() << "robot" << innerModelSampler->transform("world","robot"); 
	
	// Create a tall box for robot body with center at zero and sides:
	boost::shared_ptr<fcl::Box> robotBox(new fcl::Box(wRob, hRob, wRob));
	
	// Create a collision object
	fcl::CollisionObject robotBoxCol(robotBox);
	
	//Create and fcl rotation matrix to orient the box with the robot
	const fcl::Matrix3f R1( r1q(0,0), r1q(0,1), r1q(0,2), r1q(1,0), r1q(1,1), r1q(1,2), r1q(2,0), r1q(2,1), r1q(2,2) );
		
	//Check collision at maximum distance
	float hitDistance = MAX_LENGTH_ALONG_RAY;
	
	//Resize big box to enlarge it along the ray direction
	robotBox->side = fcl::Vec3f(wRob, hRob, hitDistance);
	
	//Compute the coord of the tip of a "nose" going away from the robot (Z dir) up to hitDistance/2
	const QVec boxBack = innerModelSampler->transform("world", QVec::vec3(0, hRob/2, hitDistance/2), "robot");
	
	//move the big box so it is aligned with the robot and placed along the nose
	robotBoxCol.setTransform(R1, fcl::Vec3f(boxBack(0), boxBack(1), boxBack(2)));
		
	//Check collision of the box with the world
	for ( auto &it : restNodes)
	{
		if ( innerModelSampler->collide(it, &robotBoxCol))
		{
			//qDebug() << __FUNCTION__ << ": Robot collides with " << it;
			return false;
		}
	}
	return true;
}

QVec RMat::QMat::extractAnglesR_min() const
{
	QVec r = extractAnglesR();
	QVec v1 = r.subVector(0,2);
	QVec v2 = r.subVector(3,5);
	if (v1.norm2() < v2.norm2())
		return v1;
	return v2;	
}

/**
 * \brief Vector to matrix constructor
 *
 * Creates a row or column matrix from vector
 * @param vector Vector to convert to matrix type
 * @param rowVector if true, creates a row matrix from the vector, else creates a column matrix from it
*/
QMat::QMat(const QVec &vector, const bool rowVector)
{
	cols = rowVector?vector.size():1;
	rows = rowVector?1:vector.size();
	
	data = new DataBuffer(cols*rows);
	
	if (rowVector)
		setRow(0, vector);
	else
		setCol(0, vector);
}

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

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

bool SpecificWorker::heLlegado()
{
  QVec t = inner->transform("robot", ctarget.target, "world");
 // qDebug()<< ctarget.target;
  float d = t.norm2();
  qDebug()<< "distancia: "<<d;
  if ( d < 100 )
  {
     return true;
  }else
  {
     return false;
  }

}