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