/// hook that is called when traversing occupied nodes of the updated Octree (does nothing here)
void srs_env_model::CPointCloudPlugin::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator& it, const SMapWithParameters & mp)
{
// std::cerr << "PCP: handle occupied" << std::endl;
tPclPoint point;
// Set position
point.x = it.getX();
point.y = it.getY();
point.z = it.getZ();
// Set color
point.r = it->r();
point.g = it->g();
point.b = it->b();
// std::cerr << "Occupied node r " << (int)point.r << ", g " << (int)point.g << ", b " << (int)point.b << std::endl;
/*
// Set color
point.r = 255 - counter % 255;
point.g = counter % 255;
point.b = 128;
*/
m_data->push_back( point );
++counter;
}
/**
* hook that is called when traversing occupied nodes of the updated Octree (does nothing here)
*/
void srs_env_model::COcPatchMaker::handleOccupiedNode(srs_env_model::tButServerOcTree::iterator& it, const SMapWithParameters & mp)
{
// PERROR("OnHandleOccupied");
if( ! m_bCamModelInitialized )
return;
// Test input point
tf::Point pos(it.getX(), it.getY(), it.getZ());
// if( m_bTransformCamera )
pos = m_to_sensorTf(pos);
pos = pos * m_fracMatrix;
if( pos.z() < 0 )
return;
cv::Point2d uv = m_camera_model.project3dToPixel(cv::Point3d( pos.x(), pos.y(), pos.z()));
// ignore point if not in sensor cone
if (!inSensorCone(uv))
return;
// Ok, add it...
// std::cerr << "PCP: handle occupied" << std::endl;
tPclPoint point;
// Set position
point.x = it.getX();
point.y = it.getY();
point.z = it.getZ();
// Set color
point.r = it->r();
point.g = it->g();
point.b = it->b();
// std::cerr << "Occupied node r " << (int)point.r << ", g " << (int)point.g << ", b " << (int)point.b << std::endl;
m_cloud.push_back( point );
}