Exemplo n.º 1
0
/// 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 );
}