SortedAggExecStreamGenerator(
uint nRows, uint nKeys, std::vector<int> repeatSeqValues)
{
this->nRows = nRows;
this->nKeys = nKeys;
for (uint i = 0; i < nKeys; i++) {
keyRepeats.push_back(repeatSeqValues[i]);
}
interval = LbmExecStreamTestBase::getTupleInterval(keyRepeats);
sortedToUnsortedMap.reset(new uint[interval]);
for (uint i = 0; i < interval; i++) {
uint value = 0;
uint scale = 1;
// calculate sorted position (backwards)
// value = key0 * scale_1_to_n + key1 * scale_2_to_n + ...
for (int j = nKeys - 1; j >= 0; j--) {
uint key = i % keyRepeats[j];
value += key * scale;
scale *= keyRepeats[j];
}
sortedToUnsortedMap[value] = i;
}
current = -1;
currentRow.reset(new uint[nKeys + 1]);
}
// 0 for received nothing
int recv_msg_async(zmq::socket_t &sock, boost::shared_array<uint8_t> &data)
{
zmq::message_t msgt;
int nbytes;
try{
nbytes = sock.recv(&msgt, ZMQ_DONTWAIT);
}catch(zmq::error_t e){
return -1;
}
if(nbytes == 0){
if(zmq_errno() == EAGAIN)
return 0;
else
return -1;
}
size_t len = msgt.size();
uint8_t *dataptr;
try{
dataptr = new uint8_t[len];
}catch(std::bad_alloc e){
return -1;
}
memcpy(dataptr, msgt.data(), len);
data.reset(dataptr);
return len;
}
/*
* return number of bytes received, negative if error, 0 for received nothing, which should be treated as error
*/
int recv_msg(zmq::socket_t &sock, boost::shared_array<uint8_t> &data)
{
zmq::message_t msgt;
int nbytes;
try{
nbytes = sock.recv(&msgt);
}catch(zmq::error_t e){
//LOG(NOR, stderr, "recv failed: %s\n", e.what());
return -1;
}
if(nbytes == 0) return 0;
size_t len = msgt.size();
uint8_t *dataptr;
try{
dataptr = new uint8_t[len];
}catch(std::bad_alloc e){
//LOG(DBG, stderr, "can not allocate memory!\n");
return -1;
}
memcpy(dataptr, msgt.data(), len);
data.reset(dataptr);
return len;
}
/*
* return number of bytes received, negative if error, 0 for received nothing,
* which should be treated as error
*/
int32_t RecvMsg(zmq::socket_t &sock, boost::shared_array<uint8_t> &data,
bool *_more){
zmq::message_t msgt;
int nbytes;
try{
nbytes = sock.recv(&msgt);
}catch(zmq::error_t &e){
LOG(ERROR) << "RecvMsg error = " << e.what();
return -1;
}
if(nbytes == 0) return 0;
size_t len = msgt.size();
uint8_t *dataptr;
try{
dataptr = new uint8_t[len];
}catch(std::bad_alloc &e){
return -1;
}
memcpy(dataptr, msgt.data(), len);
data.reset(dataptr);
if(_more != NULL){
int more_recv;
size_t s = sizeof(int);
sock.getsockopt(ZMQ_RCVMORE, &more_recv, &s);
*_more = (more_recv == 1) ? true : false;
}
return len;
}
void AsynchIO::createBuffers(uint32_t size) {
// Allocate all the buffer memory at once
bufferMemory.reset(new char[size*BufferCount]);
// Create the Buffer structs in a vector
// And push into the buffer queue
buffers.reserve(BufferCount);
for (uint32_t i = 0; i < BufferCount; i++) {
buffers.push_back(BufferBase(&bufferMemory[i*size], size));
queueReadBuffer(&buffers[i]);
}
}
NormalizerExecStreamGenerator(
uint nRows, uint nKeys, std::vector<int> repeatSeqValues)
{
this->nKeys = nKeys;
this->repeatSeqValues = repeatSeqValues;
interval = LbmExecStreamTestBase::getTupleInterval(repeatSeqValues);
changeIndexes.reset(new uint[interval]);
changeIndexes[0] = getValueCount(nRows, interval, 0);
for (uint i = 1; i < interval; i++) {
changeIndexes[i] =
changeIndexes[i - 1] + getValueCount(nRows, interval, i);
}
current = 0;
lastRow = 0;
}
void region::read_header()
{
header.reset(new char[HEADER_SIZE]);
std::ifstream fp(path.string().c_str(), std::ios::binary);
if (fp.fail()) {
throw std::runtime_error(std::string("file not found (") + path.string() + ")");
}
fp.read(header.get(), HEADER_SIZE);
if (fp.fail()) {
throw std::runtime_error(std::string("cannot read header (") + path.string() + ")");
}
}
void Header::write(const M_string& key_vals, boost::shared_array<uint8_t>& buffer, uint32_t& size)
{
// Calculate the necessary size
size = 0;
{
M_string::const_iterator it = key_vals.begin();
M_string::const_iterator end = key_vals.end();
for (; it != end; ++it)
{
const std::string& key = it->first;
const std::string& value = it->second;
size += key.length();
size += value.length();
size += 1; // = sign
size += 4; // 4-byte length
}
}
if (size == 0)
{
return;
}
buffer.reset(new uint8_t[size]);
char* ptr = (char*)buffer.get();
// Write the data
{
M_string::const_iterator it = key_vals.begin();
M_string::const_iterator end = key_vals.end();
for (; it != end; ++it)
{
const std::string& key = it->first;
const std::string& value = it->second;
uint32_t len = key.length() + value.length() + 1;
SROS_SERIALIZE_PRIMITIVE(ptr, len);
SROS_SERIALIZE_BUFFER(ptr, key.data(), key.length());
static const char equals = '=';
SROS_SERIALIZE_PRIMITIVE(ptr, equals);
SROS_SERIALIZE_BUFFER(ptr, value.data(), value.length());
}
}
ROS_ASSERT(ptr == (char*)buffer.get() + size);
}
void TSSLSocketFactory::cleanupOpenSSL() {
if (!initialized) {
return;
}
initialized = false;
#if (OPENSSL_VERSION_NUMBER < OPENSSL_VERSION_NO_THREAD_ID)
CRYPTO_set_id_callback(NULL);
#endif
CRYPTO_set_locking_callback(NULL);
CRYPTO_set_dynlock_create_callback(NULL);
CRYPTO_set_dynlock_lock_callback(NULL);
CRYPTO_set_dynlock_destroy_callback(NULL);
CRYPTO_cleanup_all_ex_data();
ERR_free_strings();
EVP_cleanup();
ERR_remove_state(0);
mutexes.reset();
}
void kpoBaseApp::image_callback (const boost::shared_ptr<openni_wrapper::Image> &image)
{
unsigned image_width_ = image->getWidth();
unsigned image_height_ = image->getHeight();
static unsigned rgb_array_size = 0;
static boost::shared_array<unsigned char> rgb_array ((unsigned char*)(NULL));
static unsigned char* rgb_buffer = 0;
// here we need exact the size of the point cloud for a one-one correspondence!
if (rgb_array_size < image_width_ * image_height_ * 3)
{
rgb_array_size = image_width_ * image_height_ * 3;
rgb_array.reset (new unsigned char [rgb_array_size]);
rgb_buffer = rgb_array.get ();
}
image->fillRGB (image_width_, image_height_, rgb_buffer, image_width_ * 3);
{
QMutexLocker locker (&mtx_);
openniImage2opencvMat((XnRGB24Pixel*)rgb_buffer, scene_image_, image_height_, image_width_);
if (need_image_cap) {
std::cout << "need_image_cap " << need_image_cap << std::endl;
cv::Mat rgb_image_;
cv::cvtColor(scene_image_, rgb_image_, CV_BGR2RGB);
qint64 timestamp = QDateTime::currentMSecsSinceEpoch();
if (timestamp - last_snapshot_time > 30000) {
last_snapshot_time = timestamp;
QString filename = QString::fromUtf8("/myshare/autonomous_snapshots/");
filename += QString::number(timestamp);
filename += QString::fromUtf8(".png");
cv::imwrite(filename.toStdString().c_str(), rgb_image_);
need_image_cap = 0;
}
}
}
}
void initialise()
{
open_console_wnd();
// Initialise the memory pool
c_cuda_mem_pool& mem_pool = c_cuda_mem_pool::get_instance();
mem_pool.initialise(256*1024*1024, 256*1024);
// Device memory
g_rands = new c_randoms_chunk(g_size);
g_rands->alloc_device_memory();
// Host memory
h_rands.reset(new float[g_size]);
// Init CUDA MT
srand(1337);
c_cuda_rng& rng = c_cuda_rng::get_instance();
bool ret = rng.init(mt_dat_file);
assert(ret);
ret = rng.seed(1337);
assert(ret);
}
// 0 for received nothing
int32_t RecvMsgAsync(zmq::socket_t &sock, boost::shared_array<uint8_t> &data,
bool *_more){
zmq::message_t msgt;
int nbytes;
try{
nbytes = sock.recv(&msgt, ZMQ_DONTWAIT);
}catch(zmq::error_t &e){
return -1;
}
if(nbytes == 0){
if(zmq_errno() == EAGAIN)
return 0;
else
return -1;
}
size_t len = msgt.size();
uint8_t *dataptr;
try{
dataptr = new uint8_t[len];
}catch(std::bad_alloc &e){
return -1;
}
memcpy(dataptr, msgt.data(), len);
data.reset(dataptr);
if(_more != NULL){
int more_recv;
size_t s = sizeof(int);
sock.getsockopt(ZMQ_RCVMORE, &more_recv, &s);
*_more = (more_recv == 1) ? true : false;
}
return len;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr ONIGrabber::convertToXYZRGBPointCloud(const boost::shared_ptr<openni_wrapper::Image> &image,
const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const
{
static unsigned rgb_array_size = 0;
static boost::shared_array<unsigned char> rgb_array(0);
static unsigned char* rgb_buffer = 0;
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > cloud(new pcl::PointCloud<pcl::PointXYZRGB > ());
cloud->header.frame_id = rgb_frame_id_;
cloud->height = depth_height_;
cloud->width = depth_width_;
cloud->is_dense = false;
cloud->points.resize(cloud->height * cloud->width);
float constant = 1.0f / device_->getImageFocalLength(cloud->width);
register int centerX = (cloud->width >> 1);
int centerY = (cloud->height >> 1);
register const XnDepthPixel* depth_map = depth_image->getDepthMetaData().Data();
if (depth_image->getWidth() != depth_width_ || depth_image->getHeight() != depth_height_)
{
static unsigned buffer_size = 0;
static boost::shared_array<unsigned short> depth_buffer(0);
if (buffer_size < depth_width_ * depth_height_)
{
buffer_size = depth_width_ * depth_height_;
depth_buffer.reset(new unsigned short [buffer_size]);
}
depth_image->fillDepthImageRaw(depth_width_, depth_height_, depth_buffer.get());
depth_map = depth_buffer.get();
}
// here we need exact the size of the point cloud for a one-one correspondence!
if (rgb_array_size < image_width_ * image_height_ * 3)
{
rgb_array_size = image_width_ * image_height_ * 3;
rgb_array.reset(new unsigned char [rgb_array_size]);
rgb_buffer = rgb_array.get();
}
image->fillRGB(image_width_, image_height_, rgb_buffer, image_width_ * 3);
// depth_image already has the desired dimensions, but rgb_msg may be higher res.
register int color_idx = 0, depth_idx = 0;
RGBValue color;
color.Alpha = 0;
float bad_point = std::numeric_limits<float>::quiet_NaN();
for (int v = -centerY; v < centerY; ++v)
{
for (register int u = -centerX; u < centerX; ++u, color_idx += 3, ++depth_idx)
{
pcl::PointXYZRGB& pt = cloud->points[depth_idx];
/// @todo Different values for these cases
// Check for invalid measurements
if (depth_map[depth_idx] == 0 ||
depth_map[depth_idx] == depth_image->getNoSampleValue() ||
depth_map[depth_idx] == depth_image->getShadowValue())
{
pt.x = pt.y = pt.z = bad_point;
}
else
{
pt.z = depth_map[depth_idx] * 0.001f;
pt.x = u * pt.z * constant;
pt.y = v * pt.z * constant;
}
// Fill in color
color.Red = rgb_buffer[color_idx];
color.Green = rgb_buffer[color_idx + 1];
color.Blue = rgb_buffer[color_idx + 2];
pt.rgb = color.float_value;
}
}
return (cloud);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr
pcl::OpenNIGrabber::convertToXYZPointCloud (const boost::shared_ptr<openni_wrapper::DepthImage>& depth_image) const
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud <pcl::PointXYZ>);
cloud->height = depth_height_;
cloud->width = depth_width_;
cloud->is_dense = false;
cloud->points.resize (cloud->height * cloud->width);
register float constant = 1.0f / device_->getDepthFocalLength (depth_width_);
if (device_->isDepthRegistered ())
cloud->header.frame_id = rgb_frame_id_;
else
cloud->header.frame_id = depth_frame_id_;
register int centerX = (cloud->width >> 1);
int centerY = (cloud->height >> 1);
float bad_point = std::numeric_limits<float>::quiet_NaN ();
// we have to use Data, since operator[] uses assert -> Debug-mode very slow!
register const unsigned short* depth_map = depth_image->getDepthMetaData ().Data ();
if (depth_image->getWidth() != depth_width_ || depth_image->getHeight () != depth_height_)
{
static unsigned buffer_size = 0;
static boost::shared_array<unsigned short> depth_buffer (0);
if (buffer_size < depth_width_ * depth_height_)
{
buffer_size = depth_width_ * depth_height_;
depth_buffer.reset (new unsigned short [buffer_size]);
}
depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer.get ());
depth_map = depth_buffer.get ();
}
register int depth_idx = 0;
for (int v = -centerY; v < centerY; ++v)
{
for (register int u = -centerX; u < centerX; ++u, ++depth_idx)
{
pcl::PointXYZ& pt = cloud->points[depth_idx];
// Check for invalid measurements
if (depth_map[depth_idx] == 0 ||
depth_map[depth_idx] == depth_image->getNoSampleValue () ||
depth_map[depth_idx] == depth_image->getShadowValue ())
{
// not valid
pt.x = pt.y = pt.z = bad_point;
continue;
}
pt.z = depth_map[depth_idx] * 0.001f;
pt.x = static_cast<float> (u) * pt.z * constant;
pt.y = static_cast<float> (v) * pt.z * constant;
}
}
cloud->sensor_origin_.setZero ();
cloud->sensor_orientation_.w () = 0.0f;
cloud->sensor_orientation_.x () = 1.0f;
cloud->sensor_orientation_.y () = 0.0f;
cloud->sensor_orientation_.z () = 0.0f;
return (cloud);
}
~raw_buffer() {
data.reset();
}
template <typename PointT> typename pcl::PointCloud<PointT>::Ptr
pcl::OpenNIGrabber::convertToXYZRGBPointCloud (const boost::shared_ptr<openni_wrapper::Image> &image,
const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const
{
static unsigned rgb_array_size = 0;
static boost::shared_array<unsigned char> rgb_array (0);
static unsigned char* rgb_buffer = 0;
boost::shared_ptr<pcl::PointCloud<PointT> > cloud (new pcl::PointCloud<PointT>);
cloud->header.frame_id = rgb_frame_id_;
cloud->height = std::max (image_height_, depth_height_);
cloud->width = std::max (image_width_, depth_width_);
cloud->is_dense = false;
cloud->points.resize (cloud->height * cloud->width);
float constant = 1.0f / device_->getImageFocalLength (depth_width_);
register int centerX = (depth_width_ >> 1);
int centerY = (depth_height_ >> 1);
register const XnDepthPixel* depth_map = depth_image->getDepthMetaData ().Data ();
if (depth_image->getWidth () != depth_width_ || depth_image->getHeight() != depth_height_)
{
static unsigned buffer_size = 0;
static boost::shared_array<unsigned short> depth_buffer (0);
if (buffer_size < depth_width_ * depth_height_)
{
buffer_size = depth_width_ * depth_height_;
depth_buffer.reset (new unsigned short [buffer_size]);
}
depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer.get ());
depth_map = depth_buffer.get ();
}
// here we need exact the size of the point cloud for a one-one correspondence!
if (rgb_array_size < image_width_ * image_height_ * 3)
{
rgb_array_size = image_width_ * image_height_ * 3;
rgb_array.reset (new unsigned char [rgb_array_size]);
rgb_buffer = rgb_array.get ();
}
image->fillRGB (image_width_, image_height_, rgb_buffer, image_width_ * 3);
float bad_point = std::numeric_limits<float>::quiet_NaN ();
// set xyz to Nan and rgb to 0 (black)
if (image_width_ != depth_width_)
{
PointT pt;
pt.x = pt.y = pt.z = bad_point;
pt.b = pt.g = pt.r = 0;
pt.a = 255; // point has no color info -> alpha = max => transparent
cloud->points.assign (cloud->points.size (), pt);
}
// fill in XYZ values
unsigned step = cloud->width / depth_width_;
unsigned skip = cloud->width * step - cloud->width;
int value_idx = 0;
int point_idx = 0;
for (int v = -centerY; v < centerY; ++v, point_idx += skip)
{
for (register int u = -centerX; u < centerX; ++u, ++value_idx, point_idx += step)
{
PointT& pt = cloud->points[point_idx];
/// @todo Different values for these cases
// Check for invalid measurements
if (depth_map[value_idx] != 0 &&
depth_map[value_idx] != depth_image->getNoSampleValue () &&
depth_map[value_idx] != depth_image->getShadowValue ())
{
pt.z = depth_map[value_idx] * 0.001f;
pt.x = static_cast<float> (u) * pt.z * constant;
pt.y = static_cast<float> (v) * pt.z * constant;
}
else
{
pt.x = pt.y = pt.z = bad_point;
}
}
}
// fill in the RGB values
step = cloud->width / image_width_;
skip = cloud->width * step - cloud->width;
value_idx = 0;
point_idx = 0;
RGBValue color;
color.Alpha = 0;
for (unsigned yIdx = 0; yIdx < image_height_; ++yIdx, point_idx += skip)
{
for (unsigned xIdx = 0; xIdx < image_width_; ++xIdx, point_idx += step, value_idx += 3)
{
PointT& pt = cloud->points[point_idx];
color.Red = rgb_buffer[value_idx];
color.Green = rgb_buffer[value_idx + 1];
color.Blue = rgb_buffer[value_idx + 2];
pt.rgba = color.long_value;
}
}
cloud->sensor_origin_.setZero ();
cloud->sensor_orientation_.w () = 0.0;
cloud->sensor_orientation_.x () = 1.0;
cloud->sensor_orientation_.y () = 0.0;
cloud->sensor_orientation_.z () = 0.0;
return (cloud);
}
pcl::PointCloud<Eigen::MatrixXf>::Ptr
pcl::OpenNIGrabber::convertToEigenPointCloud (const boost::shared_ptr<openni_wrapper::Image> &image,
const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const
{
static unsigned rgb_array_size = 0;
static boost::shared_array<unsigned char> rgb_array (0);
static unsigned char* rgb_buffer = 0;
boost::shared_ptr<pcl::PointCloud<Eigen::MatrixXf> > cloud (new pcl::PointCloud<Eigen::MatrixXf>);
cloud->properties.acquisition_time = depth_image->getTimeStamp ();
cloud->height = depth_height_;
cloud->width = depth_width_;
cloud->is_dense = false;
// Prepare channels. Default soring order for channels is: rgb x y z
cloud->channels["rgb"].name = "rgb";
cloud->channels["x"].name = "x"; cloud->channels["y"].name = "y"; cloud->channels["z"].name = "z";
cloud->channels["rgb"].offset = 0;
cloud->channels["x"].offset = 4; cloud->channels["y"].offset = 8; cloud->channels["z"].offset = 12;
cloud->channels["x"].size = cloud->channels["y"].size = cloud->channels["z"].size = cloud->channels["rgb"].size = 4;
cloud->channels["rgb"].datatype = 6;
cloud->channels["x"].datatype = cloud->channels["y"].datatype = cloud->channels["z"].datatype = 7;
cloud->channels["x"].count = cloud->channels["y"].count = cloud->channels["z"].count = cloud->channels["rgb"].count = 1;
// Resize the output to width * height * 4Bpp (xyz+rgb)
cloud->points.resize (cloud->height * cloud->width, 4);
float constant = 1.0f / device_->getImageFocalLength (cloud->width);
register int centerX = (cloud->width >> 1);
int centerY = (cloud->height >> 1);
register const XnDepthPixel* depth_map = depth_image->getDepthMetaData ().Data ();
if (depth_image->getWidth () != depth_width_ || depth_image->getHeight() != depth_height_)
{
static unsigned buffer_size = 0;
static boost::shared_array<unsigned short> depth_buffer (0);
if (buffer_size < depth_width_ * depth_height_)
{
buffer_size = depth_width_ * depth_height_;
depth_buffer.reset (new unsigned short [buffer_size]);
}
depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer.get ());
depth_map = depth_buffer.get ();
}
// here we need exact the size of the point cloud for a one-one correspondence!
if (rgb_array_size < image_width_ * image_height_ * 3)
{
rgb_array_size = image_width_ * image_height_ * 3;
rgb_array.reset (new unsigned char [rgb_array_size]);
rgb_buffer = rgb_array.get ();
}
image->fillRGB (image_width_, image_height_, rgb_buffer, image_width_ * 3);
// depth_image already has the desired dimensions, but rgb_msg may be higher res.
register int color_idx = 0, depth_idx = 0;
RGBValue color;
color.Alpha = 0;
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for (int v = -centerY; v < centerY; ++v)
{
for (register int u = -centerX; u < centerX; ++u, color_idx += 3, ++depth_idx)
{
/// @todo Different values for these cases
// Check for invalid measurements
if (depth_map[depth_idx] == 0 ||
depth_map[depth_idx] == depth_image->getNoSampleValue () ||
depth_map[depth_idx] == depth_image->getShadowValue ())
{
cloud->points.row (depth_idx).setConstant (bad_point);
}
else
{
cloud->points (depth_idx, 3) = depth_map[depth_idx] * 0.001f;
cloud->points (depth_idx, 1) = static_cast<float> (u) * cloud->points (depth_idx, 3) * constant;
cloud->points (depth_idx, 2) = static_cast<float> (v) * cloud->points (depth_idx, 3) * constant;
}
// Fill in color
color.Red = rgb_buffer[color_idx];
color.Green = rgb_buffer[color_idx + 1];
color.Blue = rgb_buffer[color_idx + 2];
cloud->points (depth_idx, 0) = color.float_value;
}
}
return (cloud);
}
void MatlabJK::disable() {
Jcell_ptr_.reset();
Kcell_ptr_.reset();
}
pcl::PointCloud<pcl::PointXYZI>::Ptr ONIGrabber::convertToXYZIPointCloud(const boost::shared_ptr<openni_wrapper::IRImage> &ir_image,
const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const
{
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> > cloud(new pcl::PointCloud<pcl::PointXYZI > ());
cloud->header.frame_id = rgb_frame_id_;
cloud->height = depth_height_;
cloud->width = depth_width_;
cloud->is_dense = false;
cloud->points.resize(cloud->height * cloud->width);
float constant = 1.0f / device_->getImageFocalLength(cloud->width);
register int centerX = (cloud->width >> 1);
int centerY = (cloud->height >> 1);
register const XnDepthPixel* depth_map = depth_image->getDepthMetaData().Data();
register const XnIRPixel* ir_map = ir_image->getMetaData().Data();
if (depth_image->getWidth() != depth_width_ || depth_image->getHeight() != depth_height_)
{
static unsigned buffer_size = 0;
static boost::shared_array<unsigned short> depth_buffer(0);
static boost::shared_array<unsigned short> ir_buffer(0);
if (buffer_size < depth_width_ * depth_height_)
{
buffer_size = depth_width_ * depth_height_;
depth_buffer.reset(new unsigned short [buffer_size]);
ir_buffer.reset(new unsigned short [buffer_size]);
}
depth_image->fillDepthImageRaw(depth_width_, depth_height_, depth_buffer.get());
depth_map = depth_buffer.get();
ir_image->fillRaw(depth_width_, depth_height_, ir_buffer.get());
ir_map = ir_buffer.get ();
}
register int depth_idx = 0;
float bad_point = std::numeric_limits<float>::quiet_NaN();
for (int v = -centerY; v < centerY; ++v)
{
for (register int u = -centerX; u < centerX; ++u, ++depth_idx)
{
pcl::PointXYZI& pt = cloud->points[depth_idx];
/// @todo Different values for these cases
// Check for invalid measurements
if (depth_map[depth_idx] == 0 ||
depth_map[depth_idx] == depth_image->getNoSampleValue() ||
depth_map[depth_idx] == depth_image->getShadowValue())
{
pt.x = pt.y = pt.z = bad_point;
}
else
{
pt.z = depth_map[depth_idx] * 0.001f;
pt.x = u * pt.z * constant;
pt.y = v * pt.z * constant;
}
pt.data_c[0] = pt.data_c[1] = pt.data_c[2] = pt.data_c[3] = 0;
pt.intensity = (float) ir_map[depth_idx];
}
}
return (cloud);
}
inline bool GetMsgParam(const std::string& netmsgbus_msgcontent, boost::shared_array<char>& msgparam, uint32_t& param_len)
{
std::string msgparamstr;
if (netmsgbus_msgcontent.length() < SENDER_LEN_BYTES)
return false;
uint8_t msgsender_len = (uint8_t)netmsgbus_msgcontent[0];
if (netmsgbus_msgcontent.length() < SENDER_LEN_BYTES + (std::size_t)msgsender_len + MSGKEY_LEN_BYTES)
return false;
uint8_t msgkey_len = (uint8_t)(netmsgbus_msgcontent[SENDER_LEN_BYTES + msgsender_len]);
uint32_t msgparam_offset = SENDER_LEN_BYTES + msgsender_len + MSGKEY_LEN_BYTES + msgkey_len;
if (netmsgbus_msgcontent.length() < msgparam_offset + MSGVALUE_LEN_BYTES)
return false;
uint32_t netparam_len = *((uint32_t *)&netmsgbus_msgcontent[msgparam_offset]);
param_len = ntohl(netparam_len);
if (netmsgbus_msgcontent.length() < msgparam_offset + MSGVALUE_LEN_BYTES + param_len)
return false;
msgparam.reset(new char[param_len]);
memcpy(msgparam.get(), &netmsgbus_msgcontent[msgparam_offset + MSGVALUE_LEN_BYTES], param_len);
return true;
//if(GetMsgKey(netmsgbus_msgcontent, msgparam_str, msgparamstr))
//{
// assert(msgparamstr.size());
// msgparam.reset(new char[msgparamstr.size()]);
// memcpy(msgparam.get(), msgparamstr.data(), msgparamstr.size());
// param_len = msgparamstr.size();
// return true;
//}
//printf("netmsgbus_msgcontent error, no msgparam found.\n");
//return false;
}