float PointCloud2Renderer::getProtoScale(Renderable &renderable) { float factor = 1.0; if(renderable.pointcloud2().has_units()) { factor = 1000.0/renderable.pointcloud2().units(); } return factor; }
int PointCloud2Renderer::fillPointList(Renderable & renderable, std::vector<SbVec3f> &points, std::vector<SbColor> &colors) { const PointCloud2 & pc(renderable.pointcloud2()); int pointNum = pc.height()*pc.width(); points.reserve(pointNum); colors.reserve(pointNum); QByteArray dataArray; char * rawData = const_cast<char *>(pc.data().c_str()); for(int i = 0; i < pointNum; ++i) { dataArray.fromRawData(rawData,4); float x = *reinterpret_cast<float *> (rawData); rawData += 4; float y = *reinterpret_cast<float *> (rawData); rawData += 4; float z = *reinterpret_cast<float *> (rawData); rawData += 8; float blue = *reinterpret_cast<unsigned char *>(rawData)/255.0; rawData += 1; float green = *reinterpret_cast<unsigned char *>(rawData)/255.0; rawData += 1; float red = *reinterpret_cast<unsigned char *>(rawData)/255.0; rawData += 14; if(!std::isnan(x)) { points.push_back(SbVec3f(x, y, z)); colors.push_back(SbColor(red, green, blue)); } } return pointNum; }