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;
}
