Eigen::Vector3i extractC3HLACSignature117(pcl::VoxelGrid<PointT> grid, pcl::PointCloud<PointT> cloud, std::vector< std::vector<float> > &feature, int color_threshold_r, int color_threshold_g, int color_threshold_b, const float voxel_size, const int subdivision_size, const int offset_x , const int offset_y, const int offset_z ){
feature.resize( 0 );
pcl::PointCloud<pcl::C3HLACSignature117> c3_hlac_signature;
pcl::C3HLAC117Estimation<PointT, pcl::C3HLACSignature117> c3_hlac_;
c3_hlac_.setRadiusSearch (0.000000001); // not used actually.
c3_hlac_.setSearchMethod ( boost::make_shared<pcl::KdTreeFLANN<PointT> > () ); // not used actually.
c3_hlac_.setColorThreshold( color_threshold_r, color_threshold_g, color_threshold_b );
if( c3_hlac_.setVoxelFilter (grid, subdivision_size, offset_x, offset_y, offset_z, voxel_size) ){
c3_hlac_.setInputCloud ( cloud.makeShared() );
t1 = my_clock();
c3_hlac_.compute( c3_hlac_signature );
t2 = my_clock();
#ifndef QUIET
ROS_INFO (" %d c3_hlac estimated. (%f sec)", (int)c3_hlac_signature.points.size (), t2-t1);
#endif
const int hist_num = c3_hlac_signature.points.size();
feature.resize( hist_num );
for( int h=0; h<hist_num; h++ ){
feature[ h ].resize( DIM_C3HLAC_117_1_3_ALL );
for( int i=0; i<DIM_C3HLAC_117_1_3_ALL; i++)
feature[ h ][ i ] = c3_hlac_signature.points[ h ].histogram[ i ];
}
}
return c3_hlac_.getSubdivNum();
}
static mps_addr_t make(void)
{
size_t length = rnd() % (avLEN * 2);
size_t size = (length+2) * sizeof(mps_word_t);
mps_addr_t p;
mps_res_t res;
alloc_bytes += size;
for(;;) {
mps_bool_t commit_res;
double t1, t2;
t1 = my_clock();
MPS_RESERVE_BLOCK(res, p, ap, size);
t1 = time_since(t1); /* reserve time */
if(res)
die(res, "MPS_RESERVE_BLOCK");
res = dylan_init(p, size, exactRoots, exactRootsCOUNT);
if(res)
die(res, "dylan_init");
t2 = my_clock();
commit_res = mps_commit(ap, p, size);
t2 = time_since(t2); /* commit time */
t1 += t2; /* total MPS time for this allocation */
alloc_time += t1;
if (t1 > max_alloc_time)
max_alloc_time = t1;
if (commit_res)
break;
else
++ commit_failures;
}
return p;
}
int main(int argc, char *argv[])
{
#ifndef NOCATCH
try
{
#endif
TestCase testCase( argc, argv );
#ifdef RELOCATE
std::vector<Geometry::byte> altbuffer;
#endif
#ifdef RELOCATE
altbuffer.resize(testCase.geom->size());
testCase.geom->relocate( &(altbuffer[0]) );
std::memcpy( &(altbuffer[0]), testCase.geom->getBuffer(), testCase.geom->size() );
std::memset( testCase.geom->getBuffer(), 0, testCase.geom->size() );
#endif
#ifdef RELOCATE
testCase.geom.reset();
G4VPhysicalVolume *geomRoot =
(G4VPhysicalVolume*)&(altbuffer[0]);
#else
G4VPhysicalVolume *geomRoot =
(G4VPhysicalVolume*)testCase.geom->getBuffer();
#endif
const my_clock_t t0 = my_clock();
BEGIN_ENERGY_MEASUREMENT;
cpuexec(
// number of rounds multiplies problem size, no separate rounds
testCase.getSize() * testCase.getRounds(),
&(testCase.input[0]),
geomRoot,
&(testCase.output[0]),
testCase.phys_step );
const my_clock_t t1 = my_clock();
END_ENERGY_MEASUREMENT;
std::cerr << "Elapsed: " << tdiffms(t0,t1) << " ms\n";
testCase.outputData( "imgcpu.txt" );
return EXIT_SUCCESS;
#ifndef NOCATCH
}
catch ( const std::runtime_error &e )
{
std::cerr << e.what() << std::endl;
return EXIT_FAILURE;
}
#endif
}
static void set_clock_timing(void)
{
long i;
double t1, t2, t3;
t2 = 0.0;
t3 = my_clock();
i = 0;
do {
t1 = my_clock();
/* do nothing here */
t2 += my_clock()-t1;
++i;
} while (t1 < t3 + CLOCK_TIME_SET);
clock_time = t2/i;
total_clock_time += my_clock() - t3 + clock_time;
}
static double time_since(double t)
{
t = my_clock() - t;
total_clock_time += clock_time + clock_time;
if (t < clock_time)
return 0.0;
else
return (t - clock_time);
}
void
EGPlanner::startPlanner()
{
if ( getState() != READY ) {
DBGA("Planner not ready to start!");
return;
}
if (!mMultiThread) {
mHand->showVirtualContacts(false);
mIdleSensor = new SoIdleSensor(sensorCB, this);
mIdleSensor->schedule();
}
PROF_RESET_ALL;
PROF_START_TIMER(EG_PLANNER);
getrusage(RUSAGE_SELF, &r_usage);
mStartTime = my_clock(r_usage);
setState( RUNNING );
}
double
EGPlanner::getRunningTime()
{
if (isActive())
{
return my_clock(r_usage) - mStartTime + mRunningTime;
/* double total_clock_time = mRunningTime + clock_time;
if (total_clock_time < mRunningTime ){
mRunningTime += INT_MAX/CLOCKS_PER_SEC;
total_clock_time = mRunningTime + (double)(clock_time - mStartTime) / CLOCKS_PER_SEC;
}
mRunningTime = clock_time;
return clock_time;
*/
}
else return mRunningTime;
}
static void test_step(mps_arena_t arena, double multiplier)
{
mps_bool_t res;
double t1 = my_clock();
res = mps_arena_step(arena, 0.1, multiplier);
cdie(ArenaGlobals(arena)->clamped, "arena was unclamped");
t1 = time_since(t1);
if (res) {
if (t1 > max_step_time)
max_step_time = t1;
step_time += t1;
++ steps;
} else {
if (t1 > max_no_step_time)
max_no_step_time = t1;
no_step_time += t1;
++ no_steps;
}
}
void vad_cb(const sensor_msgs::PointCloud2ConstPtr& cloud) {
if ((cloud->width * cloud->height) == 0)
return;
pcl::fromROSMsg (*cloud, cloud_xyzrgb_);
t0 = my_clock();
if( limitPoint( cloud_xyzrgb_, cloud_xyzrgb, distance_th ) > 10 ){
std::cout << "compute normals and voxelize...." << std::endl;
#ifdef CCHLAC_TEST
getVoxelGrid( grid, cloud_xyzrgb, cloud_downsampled, voxel_size );
#else
//****************************************
//* compute normals
computeNormal( cloud_xyzrgb, cloud_normal );
t0_2 = my_clock();
//* voxelize
getVoxelGrid( grid, cloud_normal, cloud_downsampled, voxel_size );
#endif
std::cout << " ...done.." << std::endl;
const int pnum = cloud_downsampled.points.size();
float x_min = 10000000, y_min = 10000000, z_min = 10000000;
float x_max = -10000000, y_max = -10000000, z_max = -10000000;
for( int p=0; p<pnum; p++ ){
if( cloud_downsampled.points[ p ].x < x_min ) x_min = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y < y_min ) y_min = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z < z_min ) z_min = cloud_downsampled.points[ p ].z;
if( cloud_downsampled.points[ p ].x > x_max ) x_max = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y > y_max ) y_max = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z > z_max ) z_max = cloud_downsampled.points[ p ].z;
}
//std::cout << x_min << " " << y_min << " " << z_min << std::endl;
//std::cout << x_max << " " << y_max << " " << z_max << std::endl;
//std::cout << grid.getMinBoxCoordinates() << std::endl;
std::cout << "search start..." << std::endl;
//****************************************
//* object detection start
t1 = my_clock();
search_obj.cleanData();
#ifdef CCHLAC_TEST
search_obj.setC3HLAC( dim, color_threshold_r, color_threshold_g, color_threshold_b, grid, cloud_downsampled, voxel_size, box_size );
#else
search_obj.setVOSCH( dim, color_threshold_r, color_threshold_g, color_threshold_b, grid, cloud_normal, cloud_downsampled, voxel_size, box_size );
#endif
t1_2 = my_clock();
if( ( search_obj.XYnum() != 0 ) && ( search_obj.Znum() != 0 ) )
#ifdef CCHLAC_TEST
search_obj.search();
#else
search_obj.searchWithoutRotation();
#endif
search_obj.removeOverlap();
t2 = my_clock();
//* object detection end
//****************************************
std::cout << " ...search done." << std::endl;
tAll += t2 - t0;
process_count++;
#ifdef CCHLAC_TEST
std::cout << "voxelize :"<< t1 - t0 << " sec" << std::endl;
#else
std::cout << "normal estimation :"<< t0_2 - t0 << " sec" << std::endl;
std::cout << "voxelize :"<< t1 - t0_2 << " sec" << std::endl;
#endif
std::cout << "feature extraction : "<< t1_2 - t1 << " sec" <<std::endl;
std::cout << "search : "<< t2 - t1_2 << " sec" <<std::endl;
std::cout << "all processes : "<< t2 - t0 << " sec" << std::endl;
std::cout << "AVERAGE : "<< tAll / process_count << " sec" << std::endl;
marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker_range", 1);
marker_array_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100);
visualization_msgs::MarkerArray marker_array_msg_;
#if 0
//* show the limited space
marker_pub_.publish( setMarker( -1, (x_max+x_min)/2, (y_max+y_min)/2, (z_max+z_min)/2, x_max-x_min, y_max-y_min, z_max-z_min, 0.1, 1.0, 0.0, 0.0 ) );
#endif
for( int m=0; m<model_num; m++ ){
for( int q=0; q<rank_num; q++ ){
//if( search_obj.maxDot( m, q ) < detect_th ) break;
std::cout << search_obj.maxX( m, q ) << " " << search_obj.maxY( m, q ) << " " << search_obj.maxZ( m, q ) << std::endl;
std::cout << "dot " << search_obj.maxDot( m, q ) << std::endl;
//if( (search_obj.maxX( m, q )!=0)||(search_obj.maxY( m, q )!=0)||(search_obj.maxZ( m, q )!=0) ){
//* publish markers for detected regions
if( search_obj.maxDot( m, q ) < detect_th )
marker_array_msg_.markers.push_back( setMarker( m, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ) );
else{
marker_array_msg_.markers.push_back( setMarker( m, search_obj.maxX( m, q ) * region_size + sliding_box_size_x/2 + x_min, search_obj.maxY( m, q ) * region_size + sliding_box_size_y/2 + y_min, search_obj.maxZ( m, q ) * region_size + sliding_box_size_z/2 + z_min, sliding_box_size_x, sliding_box_size_y, sliding_box_size_z, 0.5, marker_color_r[ m ], marker_color_g[ m ], marker_color_b[ m ] ) );
}
}
}
//std::cerr << "MARKER ARRAY published with size: " << marker_array_msg_.markers.size() << std::endl;
marker_array_pub_.publish(marker_array_msg_);
}
std::cout << "Waiting msg..." << std::endl;
}
void *ENC_TH_Loop_Thread(session_t *sp)
{
struct sockaddr_in SendAddr[3],ServAddr;
/* IPパケット用バッファ */
// buffer_t ipbuf;
char buffer[MAXBUFSIZE];
u_int16_t seq = 0;
u_int32_t ts = 0;
gw_hdr gwhdr;
int e_sendsock[MAX_CLIENTS]; /* socket for sending packet */
int recvsize = 0; /* recieve size */
int i, cnt;
struct ip_mreq multicastRequest;
int sock_buflen = SOCK_BUFLEN;
char flag = 0;
int timestamp = 0;
SA *your_addr[MAX_CLIENTS];
socklen_t your_adlen;
struct addrinfo hints;
struct ifreq ifr;
struct sockaddr_ll sll;
static int enc_cnt = 1, enc_avg_sum = 0;
for(cnt = 0; cnt < sp->session_num; cnt++){
e_sendsock[cnt] = send_sock_create(sp->ESendAddr[cnt].addr,
sp->ESendAddr[cnt].port,
(void **)&your_addr[cnt], &your_adlen);
// setsockopt(e_sendsock[cnt], SOL_SOCKET, SO_SNDBUF, &sock_buflen, sizeof(sock_buflen));
if(sp->send_group_e)
{
if(setsockopt(e_sendsock[cnt], IPPROTO_IP, IP_MULTICAST_LOOP,
&flag, sizeof(flag)) < 0 ){
perror("SET NON LOOP failed");
}
}
}
/* init value */
// for(i = 0;i < MAXPKT;i++)
// {
// memset(inbuf.bf[i].buf, 0, MAXBUFSIZE);
// memset(outbuf.bf[i].buf, 0, MAXBUFSIZE);
// }
// memset(ipbuf.buf, 0, MAXBUFSIZE);
#if TH
pthread_mutex_init(&dummy_enc_th, NULL);
#endif
/* main loop */
/* 受信してエンコードバッファに入れる */
while(1)
{
pthread_mutex_lock(&dummy_enc_th);
while(enc_th_buf->tail == enc_th_buf->head){
pthread_cond_wait(&data_recv_enc_th, &dummy_enc_th);
}
pthread_mutex_unlock(&dummy_enc_th);
// memcpy(inbuf.bf[0].buf, enc_th_buf->bf[enc_th_buf->head].buf, enc_th_buf->bf[enc_th_buf->head].size);
/* 受信パケットを送信用バッファへコピー */
memcpy(buffer+sizeof(gw_hdr), enc_th_buf->bf[enc_th_buf->head].buf, recvsize);
// memcpy(ipbuf.buf, enc_th_buf->bf[enc_th_buf->head].buf, enc_th_buf->bf[enc_th_buf->head].size);
recvsize = enc_th_buf->bf[enc_th_buf->head].size;
timestamp = enc_th_buf->bf[enc_th_buf->head].timestamp;
enc_th_buf->head = ++enc_th_buf->head % MAX_RECVBUF_NUM;
/* TCP */
/* ゲートウェイヘッダ付加 */
set_gwhdr(&gwhdr, seq, 34, ts);
/* GWhdrを送信用バッファにコピー */
memcpy(buffer, &gwhdr, sizeof(gw_hdr));
/* 転送用IPパケットを送信用バッファにコピー */
// memcpy(buffer+sizeof(gw_hdr), inbuf.bf[0].buf, recvsize);
// memcpy(buffer+sizeof(gw_hdr), ipbuf.buf, recvsize);
// printf("send: %d %d %d %d %d size: %d\n", gwhdr.version, gwhdr.x, gwhdr.loss, gwhdr.pt, gwhdr.seq, sizeof(gw_hdr));
//printf("\tsend:%d\n",recvsize);
/* マルチポイントに送信 */
for(cnt = 0; cnt < sp->session_num; cnt++){
// if(sendto(e_sendsock[cnt], inbuf.bf[0].buf,
// if(loss_gen(lossrate)){
if(wsendto(e_sendsock[cnt], buffer,
recvsize + sizeof(gw_hdr), 0, your_addr[cnt], your_adlen, sp->lossrate) == -1)
{
perror("send() failed unspec");
exit(0);
}
// e_point(&DIV[1]);
gettimeofday(&DIV[1], NULL);
t4 = my_clock();
enc_avg_sum += t4 - timestamp;
// printf("enc %d %d [us] avg %d [us]\n", enc_cnt, t4-t3, enc_avg_sum/enc_cnt);
l_st->enc_avg_time = enc_avg_sum/enc_cnt;
// e_time("dec",&DIV[2], &DIV[3]);
enc_cnt++;
// e_time("enc",&DIV[0], &DIV[1]);
// printf("%d\n",recvsize);
// }
// printf("%d %d %d %d\n",recvsize+gwhdr_size+fechdr_size,recvsize, gwhdr_size, fechdr_size);
l_st->enc_send_pkt++;
}
if(seq == MAX_SEQ){
seq = 0;
} else {
seq++;
}
}
}
int main(int argc, char *argv[])
{
int r = EXIT_OK;
int i, ii;
int m;
time_t t_total;
#if defined(__EMX__)
_response(&argc,&argv);
_wildcard(&argc,&argv);
#endif
if (argv[0])
argv0 = argv[0];
align_mem();
(void) my_clock();
printf("\nLZO real-time data compression library (v%s, %s).\n",
LZO_VERSION_STRING, LZO_VERSION_DATE);
printf("Copyright (C) 1996-2002 Markus Franz Xaver Johannes Oberhumer\n\n");
if (lzo_init() != LZO_E_OK)
{
printf("lzo_init() failed !!!\n");
exit(EXIT_LZO_INIT);
}
if (argc < 2)
usage(argv0,-1,0);
i = get_options(argc,argv);
if (methods_n == 0)
add_method(default_method);
if (methods_n > 1 && opt_read_from_stdin)
{
printf("%s: cannot use multiple methods and '-@'\n", argv0);
exit(EXIT_USAGE);
}
if (opt_block_size < 16)
opt_block_size = 16;
if (opt_block_size > MAX_BLOCK_SIZE)
opt_block_size = MAX_BLOCK_SIZE;
dict_len = 0;
#ifndef USE_DICT
opt_dict = 0;
#else
if (opt_dict)
{
opt_optimize_compressed_data = 0;
if (opt_dictionary_file)
{
read_dict(opt_dictionary_file);
if (opt_max_dict_len > 0 && dict_len > (lzo_uint) opt_max_dict_len)
dict_len = opt_max_dict_len;
if (dict_len > 0)
printf("Using dictionary '%s', %ld bytes, ID 0x%08lx.\n",
opt_dictionary_file,
(long) dict_len, (long) dict_adler32);
}
if (dict_len <= 0)
{
init_default_dict();
if (opt_max_dict_len > 0 && dict_len > (lzo_uint) opt_max_dict_len)
dict_len = opt_max_dict_len;
printf("Using default dictionary, %ld bytes, ID 0x%08lx.\n",
(long) dict_len, (long) dict_adler32);
}
if (opt_max_dict_len == -1)
printf("Dictionary size will be adjusted to file size.\n");
else if (opt_max_dict_len <= 0)
printf("Dictionary size will be adjusted to file size.\n");
}
#endif
t_total = time(NULL);
(void) my_clock();
ii = i;
for (m = 0; m < methods_n && r == EXIT_OK; m++)
{
int method = methods[m];
i = ii;
if (i >= argc && opt_calgary_corpus_path == NULL && !opt_read_from_stdin)
usage(argv0,-1,0);
if (m == 0 && opt_verbose >= 1)
printf("%lu block-size\n\n", (long) opt_block_size);
if (!info(method,NULL))
info(method,stdout);
#ifdef USE_CORPUS
if (opt_calgary_corpus_path != NULL)
r = do_corpus(calgary_corpus,method,opt_calgary_corpus_path,
opt_c_loops,opt_d_loops);
else
#endif
{
for ( ; i < argc && r == EXIT_OK; i++)
{
r = do_file(method,argv[i],opt_c_loops,opt_d_loops,NULL,NULL);
if (r == EXIT_FILE) /* ignore file errors */
r = EXIT_OK;
}
if (opt_read_from_stdin)
{
char buf[512], *p;
while (r == EXIT_OK && fgets(buf,sizeof(buf)-1,stdin) != NULL)
{
buf[sizeof(buf)-1] = 0;
p = buf + strlen(buf);
while (p > buf && is_space(p[-1]))
*--p = 0;
p = buf;
while (*p && is_space(*p))
p++;
if (*p)
r = do_file(method,p,opt_c_loops,opt_d_loops,NULL,NULL);
if (r == EXIT_FILE) /* ignore file errors */
r = EXIT_OK;
}
opt_read_from_stdin = 0;
}
}
}
t_total = time(NULL) - t_total;
if (opt_totals)
print_totals();
if (opt_execution_time || (methods_n > 1 && opt_verbose >= 1))
printf("\n%s: execution time: %lu seconds\n", argv0, (long) t_total);
if (r != EXIT_OK)
printf("\n%s: exit code: %d\n", argv0, r);
return r;
}
static
int process_file ( const compress_t *c, lzo_decompress_t decompress,
const char *method_name,
const char *file_name, lzo_uint l,
int t_loops, int c_loops, int d_loops )
{
int i;
unsigned blocks = 0;
unsigned long compressed_len = 0;
my_clock_t t_time = 0, c_time = 0, d_time = 0;
my_clock_t t_start, c_start, d_start;
#ifdef USE_DUMP
FILE *dump = NULL;
if (opt_dump_compressed_data)
dump = fopen(opt_dump_compressed_data,"wb");
#endif
/* process the file */
t_start = my_clock();
for (i = 0; i < t_loops; i++)
{
lzo_uint len, c_len, c_len_max, d_len = 0;
lzo_byte *d = data;
len = l;
c_len = 0;
blocks = 0;
/* process blocks */
if (len > 0 || opt_try_to_compress_0_bytes) do
{
int j;
int r;
const lzo_uint bl = len > opt_block_size ? opt_block_size : len;
lzo_uint bl_overwrite = bl;
#if defined(__LZO_CHECKER)
lzo_byte *dd = NULL;
lzo_byte *b1 = NULL;
lzo_byte *b2 = NULL;
const lzo_uint b1_len = bl + bl / 64 + 16 + 3;
#else
lzo_byte * const dd = d;
lzo_byte * const b1 = block1;
lzo_byte * const b2 = block2;
const lzo_uint b1_len = sizeof(_block1) - 256;
unsigned char random_byte;
random_byte = (unsigned char) my_clock();
#endif
blocks++;
/* may overwrite 3 bytes past the end of the decompressed block */
if (opt_use_asm_fast_decompressor)
bl_overwrite += (lzo_uint) sizeof(int) - 1;
#if defined(__LZO_CHECKER)
/* malloc a block of the exact size to detect any overrun */
dd = malloc(bl_overwrite > 0 ? bl_overwrite : 1);
b1 = malloc(b1_len);
b2 = dd;
if (dd == NULL || b1 == NULL)
{
perror("malloc");
return EXIT_MEM;
}
if (bl > 0)
memcpy(dd,d,bl);
#endif
/* compress the block */
c_len = c_len_max = 0;
c_start = my_clock();
for (j = r = 0; r == 0 && j < c_loops; j++)
{
c_len = b1_len;
r = do_compress(c,dd,bl,b1,&c_len);
if (r == 0 && c_len > c_len_max)
c_len_max = c_len;
}
c_time += my_clock() - c_start;
if (r != 0)
{
printf(" compression failed in block %d (%d) (%lu %lu)\n",
blocks, r, (long)c_len, (long)bl);
return EXIT_LZO_ERROR;
}
/* optimize the block */
if (opt_optimize_compressed_data)
{
d_len = bl;
r = do_optimize(c,b1,c_len,b2,&d_len);
if (r != 0 || d_len != bl)
{
printf(" optimization failed in block %d (%d) "
"(%lu %lu %lu)\n", blocks, r,
(long)c_len, (long)d_len, (long)bl);
return EXIT_LZO_ERROR;
}
}
#ifdef USE_DUMP
/* dump compressed data to disk */
if (dump)
{
lzo_fwrite(dump,b1,c_len);
fflush(dump);
}
#endif
/* decompress the block and verify */
#if defined(__LZO_CHECKER)
lzo_memset(b2,0,bl_overwrite);
#else
init_mem_checker(b2,_block2,bl_overwrite,random_byte);
#endif
d_start = my_clock();
for (j = r = 0; r == 0 && j < d_loops; j++)
{
d_len = bl;
r = do_decompress(c,decompress,b1,c_len,b2,&d_len);
if (d_len != bl)
break;
}
d_time += my_clock() - d_start;
if (r != 0)
{
printf(" decompression failed in block %d (%d) "
"(%lu %lu %lu)\n", blocks, r,
(long)c_len, (long)d_len, (long)bl);
return EXIT_LZO_ERROR;
}
if (d_len != bl)
{
printf(" decompression size error in block %d (%lu %lu %lu)\n",
blocks, (long)c_len, (long)d_len, (long)bl);
return EXIT_LZO_ERROR;
}
if (is_compressor(c))
{
if (lzo_memcmp(d,b2,bl) != 0)
{
lzo_uint x = 0;
while (x < bl && b2[x] == d[x])
x++;
printf(" decompression data error in block %d at offset "
"%lu (%lu %lu)\n", blocks, (long)x,
(long)c_len, (long)d_len);
if (opt_compute_adler32)
printf(" checksum: 0x%08lx 0x%08lx\n",
(long)adler_in, (long)adler_out);
#if 0
printf("Orig: ");
r = (x >= 10) ? -10 : 0 - (int) x;
for (j = r; j <= 10 && x + j < bl; j++)
printf(" %02x", (int)d[x+j]);
printf("\nDecomp:");
for (j = r; j <= 10 && x + j < bl; j++)
printf(" %02x", (int)b2[x+j]);
printf("\n");
#endif
return EXIT_LZO_ERROR;
}
if ((opt_compute_adler32 && adler_in != adler_out) ||
(opt_compute_crc32 && crc_in != crc_out))
{
printf(" checksum error in block %d (%lu %lu)\n",
blocks, (long)c_len, (long)d_len);
printf(" adler32: 0x%08lx 0x%08lx\n",
(long)adler_in, (long)adler_out);
printf(" crc32: 0x%08lx 0x%08lx\n",
(long)crc_in, (long)crc_out);
return EXIT_LZO_ERROR;
}
}
#if defined(__LZO_CHECKER)
/* free in reverse order of allocations */
free(b1);
free(dd);
#else
if (check_mem(b2,_block2,bl_overwrite,random_byte) != 0)
{
printf(" decompression overwrite error in block %d "
"(%lu %lu %lu)\n",
blocks, (long)c_len, (long)d_len, (long)bl);
return EXIT_LZO_ERROR;
}
#endif
d += bl;
len -= bl;
compressed_len += c_len_max;
}
while (len > 0);
}
t_time += my_clock() - t_start;
#ifdef USE_DUMP
if (dump)
fclose(dump);
opt_dump_compressed_data = NULL; /* only dump the first file */
#endif
print_stats(method_name, file_name,
t_loops, c_loops, d_loops,
t_time, c_time, d_time,
compressed_len, l, blocks);
return EXIT_OK;
}
void vad_cb(const sensor_msgs::PointCloud2ConstPtr& cloud) {
if ((cloud->width * cloud->height) == 0)
return;
//ROS_INFO ("Received %d data points in frame %s with the following fields: %s", (int)cloud->width * cloud->height, cloud->header.frame_id.c_str (), pcl::getFieldsList (*cloud).c_str ());
//std::cout << "fromROSMsg?" << std::endl;
pcl::fromROSMsg (*cloud, cloud_xyz_);
//std::cout << " fromROSMsg done." << std::endl;
t0 = my_clock();
if( limitPoint( cloud_xyz_, cloud_xyz, distance_th ) > 10 ){
//std::cout << " limit done." << std::endl;
std::cout << "compute normals and voxelize...." << std::endl;
//****************************************
//* compute normals
n3d.setInputCloud (cloud_xyz.makeShared());
n3d.setRadiusSearch (normals_radius_search);
normals_tree = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
n3d.setSearchMethod (normals_tree);
n3d.compute (cloud_normal);
pcl::concatenateFields (cloud_xyz, cloud_normal, cloud_xyz_normal);
t0_2 = my_clock();
//* voxelize
getVoxelGrid( grid, cloud_xyz_normal, cloud_downsampled, voxel_size );
std::cout << " ...done.." << std::endl;
const int pnum = cloud_downsampled.points.size();
float x_min = 10000000, y_min = 10000000, z_min = 10000000;
float x_max = -10000000, y_max = -10000000, z_max = -10000000;
for( int p=0; p<pnum; p++ ){
if( cloud_downsampled.points[ p ].x < x_min ) x_min = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y < y_min ) y_min = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z < z_min ) z_min = cloud_downsampled.points[ p ].z;
if( cloud_downsampled.points[ p ].x > x_max ) x_max = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y > y_max ) y_max = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z > z_max ) z_max = cloud_downsampled.points[ p ].z;
}
//std::cout << x_min << " " << y_min << " " << z_min << std::endl;
//std::cout << x_max << " " << y_max << " " << z_max << std::endl;
//std::cout << grid.getMinBoxCoordinates() << std::endl;
std::cout << "search start..." << std::endl;
//****************************************
//* object detection start
t1 = my_clock();
search_obj.cleanData();
search_obj.setGRSD( dim, grid, cloud_xyz_normal, cloud_downsampled, voxel_size, box_size );
t1_2 = my_clock();
if( ( search_obj.XYnum() != 0 ) && ( search_obj.Znum() != 0 ) )
search_obj.searchWithoutRotation();
t2 = my_clock();
//* object detection end
//****************************************
std::cout << " ...search done." << std::endl;
tAll += t2 - t0;
process_count++;
std::cout << "normal estimation :"<< t0_2 - t0 << " sec" << std::endl;
std::cout << "voxelize :"<< t1 - t0_2 << " sec" << std::endl;
std::cout << "feature extraction : "<< t1_2 - t1 << " sec" <<std::endl;
std::cout << "search : "<< t2 - t1_2 << " sec" <<std::endl;
std::cout << "all processes : "<< t2 - t0 << " sec" << std::endl;
std::cout << "AVERAGE : "<< tAll / process_count << " sec" << std::endl;
marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker_range", 1);
marker_array_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100);
visualization_msgs::MarkerArray marker_array_msg_;
//* show the limited space
visualization_msgs::Marker marker_;
marker_.header.frame_id = "openni_rgb_optical_frame";
marker_.header.stamp = ros::Time::now();
marker_.ns = "BoxEstimation";
marker_.id = -1;
marker_.type = visualization_msgs::Marker::CUBE;
marker_.action = visualization_msgs::Marker::ADD;
marker_.pose.position.x = (x_max+x_min)/2;
marker_.pose.position.y = (y_max+y_min)/2;
marker_.pose.position.z = (z_max+z_min)/2;
marker_.pose.orientation.x = 0;
marker_.pose.orientation.y = 0;
marker_.pose.orientation.z = 0;
marker_.pose.orientation.w = 1;
marker_.scale.x = x_max-x_min;
marker_.scale.y = x_max-x_min;
marker_.scale.z = x_max-x_min;
marker_.color.a = 0.1;
marker_.color.r = 1.0;
marker_.color.g = 0.0;
marker_.color.b = 0.0;
marker_.lifetime = ros::Duration();
marker_pub_.publish(marker_);
for( int q=0; q<rank_num; q++ ){
if( search_obj.maxDot( q ) < detect_th ) break;
std::cout << search_obj.maxX( q ) << " " << search_obj.maxY( q ) << " " << search_obj.maxZ( q ) << std::endl;
std::cout << "dot " << search_obj.maxDot( q ) << std::endl;
//if( (search_obj.maxX( q )!=0)||(search_obj.maxY( q )!=0)||(search_obj.maxZ( q )!=0) ){
//* publish marker
visualization_msgs::Marker marker_;
//marker_.header.frame_id = "base_link";
marker_.header.frame_id = "openni_rgb_optical_frame";
marker_.header.stamp = ros::Time::now();
marker_.ns = "BoxEstimation";
marker_.id = q;
marker_.type = visualization_msgs::Marker::CUBE;
marker_.action = visualization_msgs::Marker::ADD;
marker_.pose.position.x = search_obj.maxX( q ) * region_size + sliding_box_size_x/2 + x_min;
marker_.pose.position.y = search_obj.maxY( q ) * region_size + sliding_box_size_y/2 + y_min;
marker_.pose.position.z = search_obj.maxZ( q ) * region_size + sliding_box_size_z/2 + z_min;
marker_.pose.orientation.x = 0;
marker_.pose.orientation.y = 0;
marker_.pose.orientation.z = 0;
marker_.pose.orientation.w = 1;
marker_.scale.x = sliding_box_size_x;
marker_.scale.y = sliding_box_size_y;
marker_.scale.z = sliding_box_size_z;
marker_.color.a = 0.5;
marker_.color.r = 0.0;
marker_.color.g = 1.0;
marker_.color.b = 0.0;
marker_.lifetime = ros::Duration();
// std::cerr << "BOX MARKER COMPUTED, WITH FRAME " << marker_.header.frame_id << " POSITION: "
// << marker_.pose.position.x << " " << marker_.pose.position.y << " "
// << marker_.pose.position.z << std::endl;
// marker_pub_.publish (marker_);
// }
marker_array_msg_.markers.push_back(marker_);
}
//std::cerr << "MARKER ARRAY published with size: " << marker_array_msg_.markers.size() << std::endl;
marker_array_pub_.publish(marker_array_msg_);
}
std::cout << "Waiting msg..." << std::endl;
}
static void *test(void *arg, size_t s)
{
mps_arena_t arena;
mps_fmt_t format;
mps_chain_t chain;
mps_root_t exactRoot, ambigRoot;
unsigned long objs;
size_t i;
mps_message_t message;
size_t live, condemned, not_condemned;
size_t messages;
mps_word_t collections, old_collections;
double total_mps_time, total_time;
double t1;
arena = (mps_arena_t)arg;
(void)s; /* unused */
die(dylan_fmt(&format, arena), "fmt_create");
die(mps_chain_create(&chain, arena, genCOUNT, testChain), "chain_create");
die(mps_pool_create(&pool, arena, mps_class_amc(), format, chain),
"pool_create(amc)");
die(mps_ap_create(&ap, pool, mps_rank_exact()), "BufferCreate");
for(i = 0; i < exactRootsCOUNT; ++i)
exactRoots[i] = objNULL;
for(i = 0; i < ambigRootsCOUNT; ++i)
ambigRoots[i] = rnd_addr();
die(mps_root_create_table_masked(&exactRoot, arena,
mps_rank_exact(), (mps_rm_t)0,
&exactRoots[0], exactRootsCOUNT,
(mps_word_t)1),
"root_create_table(exact)");
die(mps_root_create_table(&ambigRoot, arena,
mps_rank_ambig(), (mps_rm_t)0,
&ambigRoots[0], ambigRootsCOUNT),
"root_create_table(ambig)");
printf("Stepping every %lu allocations.\n",
(unsigned long)step_frequencies[test_number]);
mps_message_type_enable(arena, mps_message_type_gc());
/* zero all our counters and timers. */
objs = 0;
clock_reads = 0;
steps = no_steps = 0;
alloc_bytes = 0;
commit_failures = 0;
alloc_time = step_time = no_step_time = 0.0;
max_alloc_time = max_step_time = max_no_step_time = 0.0;
total_clock_time = 0.0;
collections = old_collections = 0;
t1 = my_clock();
while(objs < objCOUNT) {
size_t r;
r = (size_t)rnd();
if(r & 1) {
i = (r >> 1) % exactRootsCOUNT;
if(exactRoots[i] != objNULL)
cdie(dylan_check(exactRoots[i]), "dying root check");
exactRoots[i] = make();
if(exactRoots[(exactRootsCOUNT-1) - i] != objNULL)
dylan_write(exactRoots[(exactRootsCOUNT-1) - i],
exactRoots, exactRootsCOUNT);
} else {
void vad_cb(const sensor_msgs::PointCloud2ConstPtr& cloud) {
if ((cloud->width * cloud->height) == 0)
return;
pcl::fromROSMsg (*cloud, cloud_xyzrgb_);
t0 = my_clock();
if( limitPoint( cloud_xyzrgb_, cloud_xyzrgb, distance_th ) > 10 ){
std::cout << "voxelize...." << std::endl;
getVoxelGrid( grid, cloud_xyzrgb, cloud_downsampled, voxel_size );
std::cout << " ...done.." << std::endl;
const int pnum = cloud_downsampled.points.size();
float x_min = 10000000, y_min = 10000000, z_min = 10000000;
float x_max = -10000000, y_max = -10000000, z_max = -10000000;
for( int p=0; p<pnum; p++ ){
if( cloud_downsampled.points[ p ].x < x_min ) x_min = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y < y_min ) y_min = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z < z_min ) z_min = cloud_downsampled.points[ p ].z;
if( cloud_downsampled.points[ p ].x > x_max ) x_max = cloud_downsampled.points[ p ].x;
if( cloud_downsampled.points[ p ].y > y_max ) y_max = cloud_downsampled.points[ p ].y;
if( cloud_downsampled.points[ p ].z > z_max ) z_max = cloud_downsampled.points[ p ].z;
}
//std::cout << x_min << " " << y_min << " " << z_min << std::endl;
//std::cout << x_max << " " << y_max << " " << z_max << std::endl;
//std::cout << grid.getMinBoxCoordinates() << std::endl;
std::cout << "search start..." << std::endl;
//****************************************
//* object detection start
t1 = my_clock();
search_obj.cleanData();
search_obj.setC3HLAC( dim, color_threshold_r, color_threshold_g, color_threshold_b, grid, cloud_downsampled, voxel_size, box_size );
t1_2 = my_clock();
if( ( search_obj.XYnum() != 0 ) && ( search_obj.Znum() != 0 ) )
search_obj.search();
t2 = my_clock();
//* object detection end
//****************************************
std::cout << " ...search done." << std::endl;
//* show the processing time
tAll += t2 - t0;
process_count++;
std::cout << "voxelize :"<< t1 - t0 << " sec" << std::endl;
std::cout << "feature extraction : "<< t1_2 - t1 << " sec" <<std::endl;
std::cout << "search : "<< t2 - t1_2 << " sec" <<std::endl;
std::cout << "all processes : "<< t2 - t0 << " sec" << std::endl;
std::cout << "AVERAGE : "<< tAll / process_count << " sec" << std::endl;
marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker_range", 1);
marker_array_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100);
visualization_msgs::MarkerArray marker_array_msg_;
//* show the limited space
//setMarker( id_, pos_x, pos_y, pos_z, scale_x, scale_y, scale_z, ca, cr, cg, cb )
marker_pub_.publish( setMarker( -1, (x_max+x_min)/2, (y_max+y_min)/2, (z_max+z_min)/2, x_max-x_min, y_max-y_min, z_max-z_min, 0.1, 1.0, 0.0, 0.0 ) );
//* publish markers for detected regions
for( int q=0; q<rank_num; q++ ){
if( search_obj.maxDot( q ) < detect_th )
marker_array_msg_.markers.push_back( setMarker( q, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ) );
else{
std::cout << search_obj.maxX( q ) << " " << search_obj.maxY( q ) << " " << search_obj.maxZ( q ) << std::endl;
std::cout << "dot " << search_obj.maxDot( q ) << std::endl;
marker_array_msg_.markers.push_back( setMarker( q, search_obj.maxX( q ) * region_size + sliding_box_size_x/2 + x_min, search_obj.maxY( q ) * region_size + sliding_box_size_y/2 + y_min, search_obj.maxZ( q ) * region_size + sliding_box_size_z/2 + z_min, sliding_box_size_x, sliding_box_size_y, sliding_box_size_z, 0.5, 0.0, 1.0, 0.0 ) );
}
}
//std::cerr << "MARKER ARRAY published with size: " << marker_array_msg_.markers.size() << std::endl;
marker_array_pub_.publish(marker_array_msg_);
}
std::cout << "Waiting msg..." << std::endl;
}