/**
* @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;
}
void ElasticBand::initialize( const RoboCompCommonBehavior::ParameterList& params )
{
ROBOT_WIDTH = std::stof(params.at("RobotXWidth").value);
ROBOT_LENGTH = std::stoi(params.at("RobotZLong").value);
qDebug() << __FUNCTION__ << "Robot dimensions in ElasticBand: " << ROBOT_WIDTH << ROBOT_LENGTH;
////////////////////////////
/// points if the frontal edge of the robot used to test robot's hypothetical positions in the laser field
////////////////////////////
pointsMat = QMat::zeros(3,4);
//front edge
pointsMat(0,0) = -ROBOT_WIDTH/2;
pointsMat(0,2) = ROBOT_LENGTH/2;
pointsMat(0,3) = 1.f;
pointsMat(1,0) = ROBOT_WIDTH/2;
pointsMat(1,2) = ROBOT_LENGTH/2;
pointsMat(1,3) = 1.f;
pointsMat(2,0) = 0;
pointsMat(2,2) = ROBOT_LENGTH/2;
pointsMat(2,3) = 1.f;
//lower
// pointsMat(3,0) = -xs/2;
// pointsMat(3,2) = -zs/2;
// pointsMat(3,3) = 1.f;
// pointsMat(4,0) = xs/2;
// pointsMat(4,2) = -zs/2;
// pointsMat(4,3) = 1.f;
// pointsMat(5,0) = 0;
// pointsMat(5,2) = -zs/2;
// pointsMat(5,3) = 1.f;
//middle
// pointsMat(6,0) = -xs/2;
// pointsMat(6,2) = 0;
// pointsMat(6,3) = 1.f;
// pointsMat(7,0) = xs/2;
// pointsMat(7,2) = 0;
// pointsMat(7,3) = 1.f;
pointsMat = pointsMat.transpose();
}
//在points中找到离anchor最近的点
bool findNearestPoint(const vector<Point2f>& points, const Point2f& anchor, Point2f& nearestPt)
{
int sz = points.size();
if (sz == 0)
{
// cout << "no color similar points." << endl;
nearestPt = Point2f(-1.0, -1.0);
return false;
}
//超过10000个点使用KDTree进行查找
if (sz > 10000)
{
Mat pointsMat(sz,2,CV_32FC1);
for (int i = 0; i < sz; ++i)
{
float * data = pointsMat.ptr<float>(i);
data[0] = points[i].x;
data[1] = points[i].y;
}
Mat anchorPt(1,2,CV_32FC1);
anchorPt.at<float>(0,0) = anchor.x;
anchorPt.at<float>(0,1) = anchor.y;
//使用Flann库
cv::flann::Index flann_Idx(pointsMat, cv::flann::KDTreeIndexParams(4),cvflann::FLANN_DIST_EUCLIDEAN);
//cout << "flann based KDTree have been done." << endl;
//查找最近邻
Mat idx, dist;
flann_Idx.knnSearch(anchorPt, idx, dist, 1, cv::flann::SearchParams());
nearestPt = points[idx.at<int>(0,0)];
return true;
}
int minIdx = 0;
float minDis = sqrt( pow((points[0].x - anchor.x), 2) + pow((points[0].y - anchor.y), 2) );
for (int i = 1; i < sz; ++i)
{
float dis = sqrt( pow((points[i].x - anchor.x), 2) + pow((points[i].y - anchor.y), 2) );
if (dis < minDis)
{
minDis = dis;
minIdx = i;
}
}
nearestPt = points[minIdx];
return true;
}
vector<ofVec3f> findRectangle(const PCPtr& cloud, const pcl::ModelCoefficients& coefficients)
{
vector<Eigen::Vector3f> corners;
//saveCloudAsFile("toproject.pcd",*cloud);
pcl::ModelCoefficients::Ptr coeff (new pcl::ModelCoefficients(coefficients));
// Project points onto the table plane
PC projectedCloud = *cloud;
//saveCloudAsFile("projected.pcd",projectedCloud);
PCXYZ pto = cloud->at(1);
float val = coefficients.values[0] * pto.x +
coefficients.values[1] * pto.y +
coefficients.values[2] * pto.z +
coefficients.values[3];
if(abs(val) < 0.009)
;;
// store the table top plane parameters
Eigen::Vector3f planeNormal;
planeNormal.x() = coeff->values[0];
planeNormal.y() = coeff->values[1];
planeNormal.z() = coeff->values[2];
// compute an orthogonal normal to the plane normal
Eigen::Vector3f v = planeNormal.unitOrthogonal();
// take the cross product of the two normals to get
// a thirds normal, on the plane
Eigen::Vector3f u = planeNormal.cross(v);
// project the 3D point onto a 2D plane
std::vector<cv::Point2f> points;
// choose a point on the plane
Eigen::Vector3f p0(projectedCloud.points[0].x,
projectedCloud.points[0].y,
projectedCloud.points[0].z);
for(unsigned int ii=0; ii<projectedCloud.points.size(); ii++)
{
Eigen::Vector3f p3d(projectedCloud.points[ii].x,
projectedCloud.points[ii].y,
projectedCloud.points[ii].z);
// subtract all 3D points with a point in the plane
// this will move the origin of the 3D coordinate system
// onto the plane
p3d = p3d - p0;
cv::Point2f p2d;
p2d.x = p3d.dot(u);
p2d.y = p3d.dot(v);
points.push_back(p2d);
}
cv::Mat pointsMat(points);
cv::RotatedRect rrect = cv::minAreaRect(pointsMat);
cv::Point2f rrPts[4];
rrect.points(rrPts);
//store the table top bounding points in a vector
for(unsigned int ii=0; ii<4; ii++)
{
Eigen::Vector3f pbbx(rrPts[ii].x*u + rrPts[ii].y*v + p0);
corners.push_back(pbbx);
}
/*Eigen::Vector3f center(rrect.center.x*u + rrect.center.y*v + p0);
corners.push_back(center); */
//Ver si se puede eliminar esto.
vector<ofVec3f> vecCorners = projectPointsInPlane(corners,coefficients);
//saveCloudAsFile("projectedrectangle.pcd",vecCorners);
return vecCorners;
}
