DepthImageView::Ptr getLatest() {
// copy data
bot_core::image_t img;
Eigen::Isometry3d pose;
int format;
{
std::unique_lock<std::mutex> lock(mDataMutex);
img = mCurrentImage;
pose = mCurrentPose;
format = mCurrentFormat;
}
// check whether data is valid
if (img.utime == 0) return NULL;
// get data bytes
std::vector<uint8_t> bytes;
if (format == bot_core::images_t::DEPTH_MM_ZIPPED) {
unsigned long uncompressedSize = img.width*img.height*2;
bytes.resize(uncompressedSize);
uncompress(bytes.data(), &uncompressedSize,
img.data.data(), img.data.size());
}
else {
bytes = img.data;
}
// create depth image
DepthImage depthImage;
depthImage.setSize(img.width, img.height);
depthImage.setOrthographic(false);
depthImage.setPose(pose.cast<float>());
depthImage.setCalib(mCalibMatrix.cast<float>());
// convert from uint16 to float and set data in depth image
std::vector<float> depths(img.width*img.height);
uint16_t* raw = (uint16_t*)bytes.data();
for (int i = 0; i < (int)depths.size(); ++i) {
depths[i] = (raw[i] == 0) ?
depthImage.getInvalidValue(DepthImage::TypeDepth) : raw[i]/1e3f;
}
depthImage.setData(depths, DepthImage::TypeDepth);
// create and return wrapper view
DepthImageView::Ptr view(new DepthImageView());
view->set(depthImage);
view->setUpdateTime(img.utime);
return view;
}
