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