Example #1
0
SensorData CameraOpenNICV::captureImage(CameraInfo * info)
{
	SensorData data;
	if(_capture.isOpened())
	{
		_capture.grab();
		cv::Mat depth, rgb;
		_capture.retrieve(depth, CV_CAP_OPENNI_DEPTH_MAP );
		_capture.retrieve(rgb, CV_CAP_OPENNI_BGR_IMAGE );

		depth = depth.clone();
		rgb = rgb.clone();

		UASSERT(_depthFocal>0.0f);
		if(!rgb.empty() && !depth.empty())
		{
			CameraModel model(
					_depthFocal, //fx
					_depthFocal, //fy
					float(rgb.cols/2) - 0.5f,  //cx
					float(rgb.rows/2) - 0.5f,  //cy
					this->getLocalTransform(),
					0,
					rgb.size());
			data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
		}
	}
	else
	{
		ULOGGER_WARN("The camera must be initialized before requesting an image.");
	}
	return data;
}
Example #2
0
void BayesFilter::setVirtualPlacePrior(float virtualPlacePrior)
{
	if(virtualPlacePrior < 0)
	{
		ULOGGER_WARN("virtualPlacePrior=%f, must be >=0 and <=1", virtualPlacePrior);
		_virtualPlacePrior = 0;
	}
	else if(virtualPlacePrior > 1)
	{
		ULOGGER_WARN("virtualPlacePrior=%f, must be >=0 and <=1", virtualPlacePrior);
		_virtualPlacePrior = 1;
	}
	else
	{
		_virtualPlacePrior = virtualPlacePrior;
	}
}
Example #3
0
void RtabmapThread::setDataBufferSize(int size)
{
	if(size < 0)
	{
		ULOGGER_WARN("size < 0, then setting it to 0 (inf).");
		_dataBufferMaxSize = 0;
	}
	else
	{
		_dataBufferMaxSize = size;
	}
}
cv::Mat CameraVideo::captureImage()
{
	cv::Mat img;  // Null image
	if(_capture.isOpened())
	{
		if(!_capture.read(img))
		{
			if(_usbDevice)
			{
				UERROR("Camera has been disconnected!");
			}
		}
	}
	else
	{
		ULOGGER_WARN("The camera must be initialized before requesting an image.");
	}

	unsigned int w;
	unsigned int h;
	this->getImageSize(w, h);

	if(!img.empty() &&
	   w &&
	   h &&
	   w != (unsigned int)img.cols &&
	   h != (unsigned int)img.rows)
	{
		cv::Mat resampled;
		cv::resize(img, resampled, cv::Size(w, h));
		return resampled;
	}
	else
	{
		// clone required
		return img.clone();
	}
}
Example #5
0
void RtabmapThread::addData(const SensorData & sensorData)
{
	if(!_paused)
	{
		if(!sensorData.isValid())
		{
			ULOGGER_ERROR("data not valid !?");
			return;
		}

		if(_rate>0.0f)
		{
			if(_frameRateTimer->getElapsedTime() < 1.0f/_rate)
			{
				return;
			}
		}
		_frameRateTimer->start();

		bool notify = true;
		_dataMutex.lock();
		{
			_dataBuffer.push_back(sensorData);
			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > (unsigned int)_dataBufferMaxSize)
			{
				ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
				_dataBuffer.pop_front();
				notify = false;
			}
		}
		_dataMutex.unlock();

		if(notify)
		{
			_dataAdded.release();
		}
	}
}