高精度地图一个重要的矢量数据就是交通标志,比如地面的字符、箭头，路边的限速牌等。在我理解看来，交通标志与车道线是同等重要的，因为它是无人车初始定位的重要匹配依据。写到这就扯远了，我今天要讨论的是，如何用交互的方式从点云中提取规则多边形，这篇文章以地面字符箭头为例，也就是矩形的提取。首先，算法输入是已经分类好的初始点云，如下图所示：
这里所说的半自动化是需要用户在点云上点一下，以生成一个种子点，作为区域增长的起点。步骤如下：
1.捕捉点云，生成一个种子点Seed
2.查找Seed周围的临近点（这里会用kdtree加快速度），然后以临界点作为种子点再次查找，一直找到没有新的点加入为止
3.经过步骤2得到一个聚类簇的集合，因其是三维点，不利于计算轮廓(字符箭头显然是二维面)。需要求得其法线方向（参见我另一篇博客：利用PCA求点云的法线），然后根据法线方向将其投影到一个二维面内
4.经过第三步，三维点云被投影到二维面，这时的轮廓就很好计算了（可采用geos中的getEnvelope方法,这一步会跟第5步同时做)
5.计算出轮廓是不规则多边形的,要进行规则化,这里规则化为矩形,当然也可以规则化为其他图形.将不规则多边形规则化为矩形,有一个重要方法是,选择多边形的每一条边作为x轴,并选该边的一个端点作为原点建立直角坐标系,再求其包围盒.所有包围盒里最小的那个就是所要求得的矩形轮廓.
6.求得了矩形轮廓,下一步就是参照第四和第五步的变换矩阵,再逆变换回去,矩形轮廓就出来了,如下图所示:
代码如下:
class ExtractSymbol
{
public:
ExtractSymbol(const CloudComparePointType& seed, const pcl::PointCloud<CloudComparePointType>::Ptr& cloud)
: _cloud(cloud), octree(5.0), _seed(seed.x, seed.y, seed.z)
{
octree.setInputCloud(cloud);
octree.addPointsFromInputCloud();
geom = createPoint(osg::Vec2d(seed.x, seed.y));
search(seed);
}
geos::geom::Geometry* createCube(const osg::Vec2d& v1, const osg::Vec2d& v2, const osg::Vec2d& v3, const osg::Vec2d& v4)
{
CoordinateSequence *pts = new CoordinateArraySequence();
pts->add(Coordinate(v1.x(), v1.y()));
pts->add(Coordinate(v2.x(), v2.y()));
pts->add(Coordinate(v3.x(), v3.y()));
pts->add(Coordinate(v4.x(), v4.y()));
pts->add(Coordinate(v1.x(), v1.y()));
GeometryFactory *fact = new GeometryFactory();
geos::geom::Polygon* poly = fact->createPolygon(fact->createLinearRing(pts), NULL);
return poly;
}
geos::geom::Geometry* createPoint(const osg::Vec2d& v)
{
GeometryFactory *fact = new GeometryFactory();
geos::geom::Point* point = fact->createPoint(Coordinate(v.x(), v.y()));
return point;
}
void search(const CloudComparePointType& seed)
{
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
bool needSearch = false;
if(octree.radiusSearch(seed, 0.5, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
{
for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
{
CloudComparePointType p = _cloud->points[pointIdxRadiusSearch[i]];
geos::geom::Geometry* pt = createPoint(osg::Vec2d(p.x, p.y));
if (!geom->covers(pt))
{
geom = geom->Union(pt)->convexHull();
needSearch = true;
}
}
}
if (!needSearch) return;
const std::vector<Coordinate>* coordinates = geom->getCoordinates()->toVector();
for (std::vector<Coordinate>::const_iterator itr = coordinates->begin(); itr != coordinates->end(); itr++)
{
CloudComparePointType p;
p.x = itr->x;
p.y = itr->y;
p.z = seed.z;
search(p);
}
}
bool getBound(ville3d::Vertex& center, ville3d::Vector& heading, double& length, double& width)
{
const std::vector<Coordinate>* coordinates = geom->getCoordinates()->toVector();
osg::ref_ptr<osg::Vec3dArray> res_envelop = getMinEnvelop(coordinates);
if (res_envelop.valid())
{
getCenteroid(res_envelop, center);
getRectangle(res_envelop, heading, length, width);
return true;
}
return false;
}
void getCenteroid(osg::Vec3dArray* rectangle, ville3d::Vertex& center)
{
osg::Vec3d centeroid;
for (osg::Vec3dArray::iterator itr = rectangle->begin(); itr != rectangle->end(); itr++)
{
centeroid += *itr;
}
centeroid = centeroid / rectangle->size();
center = ville3d::Vertex(centeroid.x(), centeroid.y(), _seed.z());
}
void getRectangle(osg::Vec3dArray* rectangle, ville3d::Vector& heading, double& length, double& width)
{
osg::Vec3d c0 = rectangle->at(0);
osg::Vec3d c1 = rectangle->at(1);
osg::Vec3d c2 = rectangle->at(2);
double length0 = (c2 - c1).length();
double length1 = (c1 - c0).length();
if (length0 > length1)
{
length = length0;
width = length1;
heading = ville3d::Vector(c2.x() - c1.x(), c2.y() - c1.y(), 0);
}
else
{
length = length1;
width = length0;
heading = ville3d::Vector(c1.x() - c0.x(), c1.y() - c0.y(), 0);
}
}
osg::Vec3dArray* getMinEnvelop(const std::vector<Coordinate>*& coordinates)
{
if (coordinates->size() < 2) return NULL;
osg::Matrixd res_matrix;
double min_area = DBL_MAX;
geos::geom::Geometry* res_envelop = NULL;
osg::ref_ptr<osg::Vec3dArray> result = new osg::Vec3dArray;
for (int i = 0; i < coordinates->size() - 1; i++)
{
Coordinate c1 = coordinates->at(i);
Coordinate c2 = coordinates->at(i + 1);
osg::Vec3d u = osg::Vec3d(c2.x - c1.x, c2.y - c1.y, 0);
u.normalize();
osg::Vec3d n = osg::Z_AXIS;
osg::Vec3d v = n ^ u;
osg::Vec3d dd;
dd.x() = osg::Vec3d(c1.x, c1.y, 0.0) * osg::Vec3d(u.x(), u.y(), u.z());
dd.y() = osg::Vec3d(c1.x, c1.y, 0.0) * osg::Vec3d(v.x(), v.y(), v.z());
dd.z() = osg::Vec3d(c1.x, c1.y, 0.0) * osg::Vec3d(n.x(), n.y(), n.z());
double m[4][4];
m[0][0] = u.x(); m[0][1] = u.y(); m[0][2] = u.z(); m[0][3] = -dd.x();
m[1][0] = v.x(); m[1][1] = v.y(); m[1][2] = v.z(); m[1][3] = -dd.y();
m[2][0] = n.x(); m[2][1] = n.y(); m[2][2] = n.z(); m[2][3] = -dd.z();
m[3][0] = 0.0; m[3][1] = 0.0; m[3][2] = 0.0; m[3][3] = 1.0;
osg::Matrixd matrix = osg::Matrixd(m[0][0], m[0][1], m[0][2], m[0][3],
m[1][0], m[1][1], m[1][2], m[1][3],
m[2][0], m[2][1], m[2][2], m[2][3],
m[3][0], m[3][1], m[3][2], m[3][3]);
geos::geom::Geometry* pointset;
for (std::vector<Coordinate>::const_iterator itr = coordinates->begin(); itr != coordinates->end(); itr++)
{
osg::Vec3d v = matrix.postMult(osg::Vec3d(itr->x, itr->y, 0.0));
if (itr == coordinates->begin())
{
pointset = createPoint(osg::Vec2d(v.x(), v.y()));
}
else
{
pointset = pointset->Union(createPoint(osg::Vec2d(v.x(), v.y())));
}
}
geos::geom::Geometry* envelop = pointset->getEnvelope();
double area = envelop->getArea();
if (area < min_area)
{
min_area = area;
res_matrix = matrix;
res_envelop = envelop;
}
}
const std::vector<Coordinate>* envelopcoords = res_envelop->getCoordinates()->toVector();
assert(envelopcoords->size() == 5);
for (std::vector<Coordinate>::size_type i = 0; i < 4; i++)
{
Coordinate c = envelopcoords->at(i);
osg::Vec3d v = osg::Matrix::inverse(res_matrix).postMult(osg::Vec3d(c.x, c.y, 0.0));
result->push_back(v);
}
return result.release();
}
private:
pcl::PointCloud<CloudComparePointType>::Ptr _cloud;
pcl::octree::OctreePointCloudSearch<CloudComparePointType, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerPointIndices> octree;
osg::Vec3d _seed;
geos::geom::Geometry* geom;
};
6082
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
