void updatePointCloudFromRangeSensor()
{
if(!pointSetFromRangeSensor)
return;
SgVertexArray& disPoints = *pointSetFromRangeSensor->getOrCreateVertices();
boost::mutex::scoped_lock lock(mtx);
disPoints.resize(rangeSensorPoints.size());
copy(rangeSensorPoints.begin(), rangeSensorPoints.end(), disPoints.begin());
lock.unlock();
pointSetFromRangeSensor->notifyUpdate();
}
void updatePointCloudFromRangeCamera()
{
if(!pointSetFromRangeCamera)
return;
SgVertexArray& disPoints = *pointSetFromRangeCamera->getOrCreateVertices();
SgColorArray& disColors = *pointSetFromRangeCamera->getOrCreateColors();
boost::mutex::scoped_lock lock(mtx);
disPoints.resize(rangeCameraPoints.size());
disColors.resize(rangeCameraColors.size());
copy(rangeCameraPoints.begin(), rangeCameraPoints.end(), disPoints.begin());
copy(rangeCameraColors.begin(), rangeCameraColors.end(), disColors.begin());
lock.unlock();
pointSetFromRangeCamera->notifyUpdate();
}