void
ServerPointCloudProcessor::cloudCallback(const core_msgs::PointCloudConstPtr& input)
{
bool is_filtered = false;
#if 0
// Measure the elapsed time in milliseconds between callbacks
long mtime, seconds, useconds;
gettimeofday(&cloudcallback_end, NULL);
seconds = filter_range_end.tv_sec - filter_range_start.tv_sec;
useconds = filter_range_end.tv_usec - filter_range_start.tv_usec;
mtime = ((seconds) * 1000 + useconds/1000.0) + 0.5;
std::cout << "cloudCallback elapsed time: " << mtime << " ms" << std::endl;
#endif
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
std::stringstream compressed_data(input->data);
decompressPointCloud(compressed_data, cloud);
if (enable_range_)
{
filterRangeDepth(cloud, cloud_filtered, min_distance_, max_distance_);
is_filtered = true;
}
if (enable_plane_)
{
if (filterPlaneModel(cloud_filtered, distance_threshold_) < 0)
std::cerr << "Could not estimate a planar model for the given data" << std::endl;
else
is_filtered = true;
}
// Enqueue point cloud
#if 0
gettimeofday(&cloudcallback_start, NULL);
#endif
}
int main( int argc, char* argv[]) {
if(argc < 6) {
std::cout<< "All File arguments are required!!" << std::endl;
exit(-1);
}
uint32_t total_frames = std::stoi(argv[1], nullptr, 10);
uint32_t key_frame_interval = std::stoi(argv[2], nullptr, 10);
std::string dir_path(argv[3]);
uint32_t search_area = std::stoi(argv[4], nullptr, 10);
int32_t vErrThreshold = std::stoi(argv[5], nullptr, 10);
std::string out_file_path(argv[6], nullptr, 10);
#if 0
std::string dir_path(argv[1]);
uint32_t first_frame_idx = std::stoi(argv[2], nullptr, 10);
uint32_t frame_count = std::stoi(argv[3], nullptr, 10);
uint32_t search_area = std::stoi(argv[4], nullptr, 10);
uint32_t pad_zero = std::stoi(argv[5], nullptr, 10);
int32_t vErrThreshold = std::stoi(argv[6], nullptr, 10);
std::vector<std::string> file_paths;
std::vector< std::unique_ptr<MPTC::DXTImage> > dxt_frames;
std::stringstream ss;
std::fstream outfile;
outfile.open("out.txt", std::ios::out);
//MPTC::DXTImage dxt_img(img_path, true, 0);
ss.str("");
ss << std::setw(pad_zero) << std::setfill('0') << first_frame_idx;
std::string frame_num_str = ss.str();
std::string file_path = dir_path + "/" + frame_num_str+ ".png";
file_paths.push_back(file_path);
MPTC::DXTImage::SetPattern(static_cast<int32_t>(search_area));
std::unique_ptr<MPTC::DXTImage> dxt_img(new MPTC::DXTImage(file_path, true, 0, vErrThreshold));
std::unique_ptr<MPTC::DXTImage> null_dxt(nullptr);
dxt_frames.push_back(std::move(dxt_img));
std::cout << "Frame Number:" << 0 << std::endl;
std::cout << "Before PSNR: " << dxt_frames[0]->PSNR() << std::endl;
dxt_frames[0]->Reencode(null_dxt, 0);
std::cout << "After PSNR: " << dxt_frames[0]->PSNR() << std::endl;
std::cout << "Intra block motion size:" << dxt_frames[0]->_motion_indices.size() << std::endl;
std::vector<uint8_t> palette = std::move(dxt_frames[0]->Get8BitPalette());
std::vector<uint64_t> count_palette(256, 0);
std::vector<uint64_t> count_intra(256, 0);
std::vector<uint64_t> total_counts(256,0);
for(auto a : dxt_frames[0]->_intra_motion_indices) {
count_intra[std::get<0>(a)]++;
count_intra[std::get<1>(a)]++;
total_counts[std::get<0>(a)]++;
total_counts[std::get<1>(a)]++;
}
uint64_t Total = std::accumulate(count_intra.begin(), count_intra.end(), 0U);
double entropy = 0.0;
for( auto e : count_intra ) {
if(e!=0) {
double p = static_cast<double>(e)/static_cast<double>(Total);
entropy += (-1.0 * p * log2(p));
}
}
std::cout << "Total:" << Total << std::endl;
std::cout << "Entropy:" << entropy << std::endl;
//Entropy encode motion indices
//*****************MAX BYTES *******************
uint32_t max_bytes = 180000;
std::vector<uint8_t> compressed_data(max_bytes, 0);
entropy::Arithmetic_Codec ace(max_bytes, compressed_data.data());
entropy::Adaptive_Data_Model model(257);
ace.start_encoder();
for(auto a : dxt_frames[0]->_motion_indices) {
ace.encode(std::get<0>(a), model);
ace.encode(std::get<1>(a), model);
}
ace.stop_encoder();
std::cout << "Compressed motion index bytes:" << ace.get_num_bytes() << std::endl;
// Entropy encode index mask
std::vector<uint8_t> compressed_mask(max_bytes, 0);
entropy::Arithmetic_Codec ace_mask(max_bytes, compressed_mask.data());
entropy::Adaptive_Bit_Model mask_bit_model;
ace_mask.start_encoder();
for(auto a : dxt_frames[0]->_index_mask) {
ace_mask.encode(a, mask_bit_model);
}
ace_mask.stop_encoder();
std::cout << "Compressed Mask bytes:" << ace_mask.get_num_bytes() << std::endl;
#if 0
//Entropy Decode
entropy::Arithmetic_Codec ade(max_bytes, compressed_data.data());
entropy::Adaptive_Data_Model decode_model(257);
ade.start_decoder();
std::vector<std::tuple<uint8_t, uint8_t> > decoded_symbols;
for(int i = 0; i < dxt_frames[0]->_motion_indices.size(); i++) {
uint8_t sym1 = ade.decode(decode_model);
uint8_t sym2 = ade.decode(decode_model);
decoded_symbols.push_back(std::make_tuple(sym1, sym2));
#ifdef NDEBUG
auto a = dxt_frames[0]->_motion_indices[i];
std::cout << sym1 << "-" << std::get<0>(a) << std::endl;
std::cout << sym2 << "-" << std::get<1>(a) << std::endl;
#endif
assert(dxt_frames[0]->_motion_indices[i] == decoded_symbols[i]);
}
ade.stop_decoder();
//Entropy decode mask bits
entropy::Arithmetic_Codec ade_mask(max_bytes,compressed_mask.data());
entropy::Adaptive_Bit_Model decode_mask_bit_model;
ade_mask.start_decoder();
std::vector<uint8_t> decoded_mask;
for(int i = 0; i < dxt_frames[0]->_motion_indices.size(); i++) {
uint8_t sym = ade_mask.decode(decode_mask_bit_model);
decoded_mask.push_back(sym);
#ifndef NDEBUG
auto a = dxt_frames[0]->_index_mask[i];
std::cout << static_cast<int>(sym) << " -- " << static_cast<int>(a) << std::endl;
#endif
assert(sym == dxt_frames[0]->_index_mask[i]);
}
ade_mask.stop_decoder();
#endif
uint32_t total_bits = ace.get_num_bytes() * 8 + dxt_frames[0]->_unique_palette.size() * 32 + ace_mask.get_num_bytes() * 8;
float total_bytes = static_cast<float>(total_bits)/8;
outfile << total_bytes+10 << "\t" << dxt_frames[0]->PSNR() << std::endl;
std::cout << "*****Total bytes****:" << total_bytes << std::endl;
float bpp = static_cast<float>(total_bits)/(dxt_frames[0]->_width * dxt_frames[0]->_height);
std::cout << "BPP:" << bpp << "\t" << dxt_frames[0]->PSNR() << std::endl;
std::unique_ptr<MPTC::RGBAImage> ep1 = std::move(dxt_frames[0]->EndpointOneImage());
std::vector<uint8_t> ep1_vector = std::move(ep1->Pack());
stbi_write_png("ep1.png", ep1->Width(), ep1->Height(),
4, ep1->Pack().data(), 4 * ep1->Width());
std::unique_ptr<MPTC::RGBAImage> ep2 = std::move(dxt_frames[0]->EndpointTwoImage());
std::vector<uint8_t> ep2_vector = std::move(ep2->Pack());
stbi_write_png("ep2.png", ep2->Width(), ep2->Height(),
4, ep2->Pack().data(), 4 * ep2->Width());
std::vector<uint32_t> ep_diff;
int max_diff = std::numeric_limits<int>::min();
int min_diff = std::numeric_limits<int>::max();
std::vector<uint32_t> count_ep(512, 0);
for(size_t ep_idx = 0; ep_idx < ep1_vector.size(); ep_idx++) {
if(ep_idx % 4 == 3) continue;
int diff = static_cast<int>(ep1_vector[ep_idx]) - static_cast<int>(ep2_vector[ep_idx]);
if(diff > max_diff) max_diff = diff;
if(diff < min_diff) min_diff = diff;
ep_diff.push_back(static_cast<uint32_t>(diff + 255));
count_ep[ep_diff[ep_diff.size() - 1]]++;
}
uint64_t Total_ep = std::accumulate(count_ep.begin(), count_ep.end(), 0U);
double entropy_ep = 0.0;
for( auto e : count_ep ) {
if(e!=0) {
double p = static_cast<double>(e)/static_cast<double>(Total_ep);
entropy_ep += (-1.0 * p * log2(p));
}
}
// Entropy encode endpoint
std::vector<uint8_t> compressed_ep(max_bytes, 0);
entropy::Arithmetic_Codec ace_ep(max_bytes, compressed_ep.data());
entropy::Adaptive_Data_Model ep_model;
ace_ep.start_encoder();
for(auto a :ep_diff) {
ace_ep.encode(a, mask_bit_model);
}
ace_ep.stop_encoder();
std::cout << "----EndPoint compressed----:" << ace_ep.get_num_bytes() << std::endl;
std::cout << "Total end point:" << Total_ep << std::endl;
std::cout << "Entropy end point:" << entropy_ep << std::endl;
ReconstructImage(dxt_frames, 0);
std::cout << "PSNR after decompression: " << dxt_frames[0]->PSNR() << std::endl;
for(uint32_t i = first_frame_idx + 1; i <= first_frame_idx + frame_count; i++) {
ss.str("");
ss << std::setw(pad_zero) << std::setfill('0') << i;
std::string frame_num_str = ss.str();
std::string file_path = dir_path + "/" + frame_num_str+ ".png";
std::unique_ptr<MPTC::DXTImage> dxt_img1(new MPTC::DXTImage(file_path, false, search_area, vErrThreshold));
dxt_frames.push_back(std::move(dxt_img1));
file_paths.push_back(file_path);
}
double combined_bpp = bpp;
for(size_t i = 1; i < dxt_frames.size(); i++) {
//*****************MAX BYTES *******************
std::cout << std::endl << std::endl;
std::cout << "Frame Number:" << i << std::endl;
std::cout << "Before PSNR:" << dxt_frames[i]->PSNR() << std::endl;
dxt_frames[i]->Reencode(dxt_frames[i-1], -1);
std::cout << "After PSNR:" << dxt_frames[i]->PSNR() << std::endl;
std::cout << "Total unique indices:" << dxt_frames[i]->_unique_palette.size()<< std::endl;
std::cout << "Intra block motion size:" << dxt_frames[i]->_intra_motion_indices.size()<<std::endl;
std::cout << "Inter block motion size:" << dxt_frames[i]->_inter_block_motion_indices.size() << std::endl;
std::cout << "Inter pixel motion size:" << dxt_frames[i]->_inter_pixel_motion_indices.size() << std::endl;
uint32_t max_bytes_inter = 180000;
std::vector<uint8_t> compressed_data_inter(max_bytes_inter, 0);
entropy::Arithmetic_Codec ace_inter(max_bytes_inter, compressed_data_inter.data());
entropy::Adaptive_Data_Model model_inter(257);
ace_inter.start_encoder();
for(auto a : dxt_frames[i]->_motion_indices) {
ace_inter.encode(std::get<0>(a), model_inter);
ace_inter.encode(std::get<1>(a), model_inter);
}
ace_inter.stop_encoder();
// Entropy encode index mask
std::vector<uint8_t> compressed_mask_inter(max_bytes_inter, 0);
entropy::Arithmetic_Codec ace_mask_inter(max_bytes_inter, compressed_mask_inter.data());
entropy::Adaptive_Bit_Model mask_bit_model_inter;
ace_mask_inter.start_encoder();
for(auto a : dxt_frames[i]->_index_mask) {
ace_mask_inter.encode(a, mask_bit_model_inter);
}
ace_mask_inter.stop_encoder();
//Entropy Decode
entropy::Arithmetic_Codec ade(max_bytes, compressed_data_inter.data());
entropy::Adaptive_Data_Model decode_model(257);
ade.start_decoder();
std::vector<std::tuple<uint8_t, uint8_t> > decoded_symbols;
for(int ii = 0; ii < dxt_frames[i]->_motion_indices.size(); ii++) {
uint8_t sym1 = ade.decode(decode_model);
uint8_t sym2 = ade.decode(decode_model);
decoded_symbols.push_back(std::make_tuple(sym1, sym2));
#if 0
auto a = dxt_frames[]->_motion_indices[i];
std::cout << sym1 << "-" << std::get<0>(a) << std::endl;
std::cout << sym2 << "-" << std::get<1>(a) << std::endl;
#endif
assert(dxt_frames[i]->_motion_indices[ii] == decoded_symbols[ii]);
}
ade.stop_decoder();
//Entropy decode mask bits
entropy::Arithmetic_Codec ade_mask(max_bytes,compressed_mask_inter.data());
entropy::Adaptive_Bit_Model decode_mask_bit_model;
ade_mask.start_decoder();
std::vector<uint8_t> decoded_mask;
for(int ii = 0; ii < dxt_frames[i]->_index_mask.size(); ii++) {
uint8_t sym = ade_mask.decode(decode_mask_bit_model);
decoded_mask.push_back(sym);
#if 0
auto a = dxt_frames[0]->_index_mask[i];
std::cout << static_cast<int>(sym) << " -- " << static_cast<int>(a) << std::endl;
#endif
assert(sym == dxt_frames[i]->_index_mask[ii]);
}
ade_mask.stop_decoder();
std::vector<uint64_t> counts(256,0);
uint8_t max = std::numeric_limits<uint8_t>::min();
uint8_t min = std::numeric_limits<uint8_t>::max();
for(auto a : dxt_frames[i]->_motion_indices) {
counts[std::get<0>(a)]++;
counts[std::get<1>(a)]++;
total_counts[std::get<0>(a)]++;
total_counts[std::get<1>(a)]++;
max = std::max(std::max(max, std::get<0>(a)), std::get<1>(a));
min = std::min(std::min(min, std::get<0>(a)), std::get<1>(a));
}
Total = std::accumulate(counts.begin(), counts.end(), 0U);
entropy = 0.0;
for( auto e : counts ) {
if(e!=0) {
double p = static_cast<double>(e)/static_cast<double>(Total);
entropy += (-1.0 * p * log2(p));
}
}
std::cout << "Total:" << Total << std::endl;
std::cout << "Entropy:" << entropy << std::endl;
total_bits = ace_inter.get_num_bytes() * 8 + 1000 + dxt_frames[0]->_unique_palette.size() * 32 + ace_mask_inter.get_num_bytes() * 8;
total_bytes = static_cast<float>(total_bits)/8;
std::cout << "Compressed motion index bytes:" << ace_inter.get_num_bytes() << std::endl;
std::cout << "Compressed Mask bytes:" << ace_mask_inter.get_num_bytes() << std::endl;
std::cout << "Total bytes:" << total_bytes << std::endl;
bpp = static_cast<float>(total_bits)/(dxt_frames[0]->_width * dxt_frames[0]->_height);
std::cout << "BPP:" << bpp << std::endl;
combined_bpp += bpp;
}
std::cout << std::endl << std::endl;
std::cout << "Combined BPP:" << combined_bpp << std::endl;
#endif
return 0;
}
compressed_data retrieve (data_type, chunk_coordinates xyz) override
{ return compressed_data(); }