void virtual_bidirectional_iteratorTest::testUpCast()
{
{
std::vector<int> cont;
virtual_random_access_iterator<int> random_access(cont.begin());
virtual_bidirectional_iterator<int> me(random_access);
CPPUNIT_ASSERT(me == static_cast<virtual_bidirectional_iterator<int> >(random_access));
}
}
int main() {
DEBUG_PRINT("creating QUEUE");
UIntQueue Q;
DEBUG_PRINT("QUEUE created!");
for (uint32_t rounds = 0; rounds < ROUNDS; ++rounds) {
enque(Q);
random_access(Q);
deque(Q);
}
return 0;
}
int read_MPEG_video (media_entry *me, uint8 *data, uint32 *data_size, double *mtime, int *recallme, uint8 *marker) /* reads MPEG-1,2 Video */
{
int ret;
uint32 num_bytes;
char *vsh1_1;
#ifdef MPEG2VSHE
char *vsh2_1;
#endif
uint32 count,flag=0,seq_head_pres=0;
unsigned char *data_tmp;
static_MPEG_video *s=NULL;
uint32 wasSeeking=0;
if (!(me->flags & ME_FD)) {
if ( (ret=mediaopen(me)) < 0 )
return ret;
s = (static_MPEG_video *) calloc (1, sizeof(static_MPEG_video));
me->stat = (void *) s;
s->final_byte=0x00;
s->std=TO_PROBE;
s->fragmented=0;
} else
s = (static_MPEG_video *) me->stat;
num_bytes = ((me->description).byte_per_pckt>261)?(me->description).byte_per_pckt:DEFAULT_BYTE_X_PKT;
#if HAVE_ALLOCA
data_tmp=(unsigned char *)alloca(65000);
#else
data_tmp=(unsigned char *)calloc(1,65000);
#endif
if (data_tmp==NULL)
return ERR_ALLOC;
if (s->std == MPEG_2) {/*the first time is TO_PROBE*/
#ifdef MPEG2VSHE
*data_size = 8;
#else
*data_size = 4;
#endif /* bytes for the video specific header */
} else {
*data_size = 4;
}
if (s->std == TO_PROBE) {
probe_standard(me,data_tmp,data_size,me->fd,&s->std);
flag=1;
seq_head_pres=1;
}
/*---- Random Access ----*/
if((me->description).msource!=live && me->play_offset!=me->prev_mstart_offset){
if(!me->description.bitrate){
//Bit Rate unavaible in sequence header, so i calculate it from scratch
me->description.bitrate=(int)(s->data_total * 8 / (*mtime))*1000;
}
else{
s->fragmented=0;
wasSeeking=1;
if(!flag){
lseek(me->fd,0,SEEK_SET);
probe_standard(me,data_tmp,data_size,me->fd,&s->std);
seq_head_pres=1;
}
}
count=random_access(me);
lseek(me->fd,count,SEEK_SET);
me->prev_mstart_offset=me->play_offset;
}
/*---- end Random Access ----*/
#if 0
if(num_bytes==0){ /*TODO:case 1 slice for pkt*/
do{
if(!flag){
if ( next_start_code(data_tmp,data_size,me->fd) == -1) {
close(me->fd);
#if !HAVE_ALLOCA
free(data_tmp);
#endif
return ERR_EOF;
}
read(me->fd,&s->final_byte,1);
data_tmp[*data_size]=s->final_byte;
*data_size+=1;
}else{
s->final_byte=data_tmp[*data_size-1];
}
if (s->final_byte == 0xb3) {
read_seq_head(me,data_tmp,data_size,me->fd,&s->final_byte,s->std);
seq_head_pres=1;
}
if (s->final_byte == 0xb8) {
read_gop_head(data_tmp,data_size,me->fd,&s->final_byte,&s->hours,&s->minutes,&s->seconds,&s->picture,s->std);
}
if (s->final_byte == 0x00) {
read_picture_head(data_tmp,data_size,me->fd,&s->final_byte,&s->temp_ref,&s->vsh1,s->std);
if (s->std == MPEG_2) {
read_picture_coding_ext(data_tmp,data_size,me->fd,&s->final_byte,&s->vsh2);
}
}
}while(s->final_byte>0xAF);
read_slice(data_tmp,data_size,me->fd,&s->final_byte);
s->vsh1.b=1;
}/*end if(num_bytes==0)*/
else
#endif
if(!s->fragmented)/*num_bytes !=0 and slice was not fragmented*/{
char buf_aux[3];
int i;
if(flag && wasSeeking==0)
s->final_byte=data_tmp[*data_size-1];
else{
data_tmp[*data_size]=0x00;
*data_size+=1;
data_tmp[*data_size]=0x00;
*data_size+=1;
data_tmp[*data_size]=0x01;
*data_size+=1;
data_tmp[*data_size]=s->final_byte;
*data_size+=1;
}
if(s->final_byte==0xb7){
if (next_start_code(data_tmp,data_size,me->fd)==-1){ /* If there aren't 3 more bytes we are at EOF */
#if !HAVE_ALLOCA
free(data_tmp);
#endif
return ERR_EOF;
}
if(read(me->fd,&s->final_byte,1)<1) {
#if !HAVE_ALLOCA
free(data_tmp);
#endif
return ERR_EOF;
}
data_tmp[*data_size]=s->final_byte;
*data_size+=1;
}
*recallme=1;
while((s->final_byte > 0xAF || s->final_byte==0x00) && *recallme){
if (s->final_byte == 0xb3) {
read_seq_head(me, data_tmp, data_size, me->fd, &s->final_byte, s->std);
seq_head_pres=1;
}
if (s->final_byte == 0xb8) {
read_gop_head(data_tmp, data_size, me->fd, &s->final_byte, &s->hours, &s->minutes, &s->seconds, &s->picture, s->std);
}
if (s->final_byte == 0x00) {
read_picture_head(data_tmp, data_size, me->fd, &s->final_byte, &s->temp_ref, &s->vsh1, s->std);
if (s->std == MPEG_2 && *data_size<num_bytes) {
read_picture_coding_ext(data_tmp, data_size, me->fd, &s->final_byte, &s->vsh2);
}
}
if (s->final_byte == 0xb7) {/*sequence end code*/
*recallme=0;
s->fragmented=0;
}
while(s->final_byte==0xb2||s->final_byte==0xb5){
if(next_start_code(data_tmp,data_size,me->fd)<0) {
#if !HAVE_ALLOCA
free(data_tmp);
#endif
return ERR_EOF;
}
if(read(me->fd,&s->final_byte,1)<1) {
#if !HAVE_ALLOCA
free(data_tmp);
#endif
return ERR_EOF;
}
data_tmp[*data_size]=s->final_byte;
*data_size+=1;
}
}
// *data_size-=4;
while(num_bytes > *data_size && *recallme){
// *data_size+=4;
if ( read(me->fd,&buf_aux,3) <3){ /* If there aren't 3 more bytes we are at EOF */
// close(me->fd);
#if !HAVE_ALLOCA
free(data_tmp);
#endif
return ERR_EOF;
}
while ( !((buf_aux[0] == 0x00) && (buf_aux[1]==0x00) && (buf_aux[2]==0x01)) && *data_size <num_bytes) {
data_tmp[*data_size]=buf_aux[0];
*data_size+=1;
buf_aux[0]=buf_aux[1];
buf_aux[1]=buf_aux[2];
if ( read(me->fd,&buf_aux[2],1) <1){
// close(me->fd);
#if !HAVE_ALLOCA
free(data_tmp);
#endif
return ERR_EOF;
}
}
for (i=0;i<3;i++) {
data_tmp[*data_size]=buf_aux[i];
*data_size+=1;
}
if(read(me->fd,&s->final_byte,1)<1) {
#if !HAVE_ALLOCA
free(data_tmp);
#endif
return ERR_EOF;
}
data_tmp[*data_size]=s->final_byte;
*data_size+=1;
if ( ((buf_aux[0] == 0x00) && (buf_aux[1]==0x00) && (buf_aux[2]==0x01)) ) {
if(((s->final_byte > 0xAF)||(s->final_byte==0x00)) ){/*TODO: 0xb7*/
*recallme = 0;
// *data_size-=4;
}
else
*recallme=1;
s->fragmented=0;
}
else{
*recallme=1;
s->fragmented=1;
}
}/*end while *data_size < num_bytes && *recallme*/
s->vsh1.b=1;
if ( ((buf_aux[0] == 0x00) && (buf_aux[1]==0x00) && (buf_aux[2]==0x01)) )
*data_size-=4;
}/*end else num_bytes!=0 and slice was not fragmented*/
else{/*num_bytes!=0 and slice was fragmented*/
char buf_aux[3];
int i;
if ( read(me->fd,&buf_aux,3) <3){ /* If there aren't 3 more bytes we are at EOF */
// close(me->fd);
#if !HAVE_ALLOCA
free(data_tmp);
#endif
return ERR_EOF;
}
while ( !((buf_aux[0] == 0x00) && (buf_aux[1]==0x00) && (buf_aux[2]==0x01)) && *data_size <num_bytes) {
data_tmp[*data_size]=buf_aux[0];
*data_size+=1;
buf_aux[0]=buf_aux[1];
buf_aux[1]=buf_aux[2];
if ( read(me->fd,&buf_aux[2],1) <1){
// close(me->fd);
#if !HAVE_ALLOCA
free(data_tmp);
#endif
return ERR_EOF;
}
}
for (i=0;i<3;i++) {
data_tmp[*data_size]=buf_aux[i];
*data_size+=1;
}
if(read(me->fd,&s->final_byte,1)<1) {
#if !HAVE_ALLOCA
free(data_tmp);
#endif
return ERR_EOF;
}
data_tmp[*data_size]=s->final_byte;
*data_size+=1;
if ( ((buf_aux[0] == 0x00) && (buf_aux[1]==0x00) && (buf_aux[2]==0x01)) ) {
if(((s->final_byte > 0xAF)||(s->final_byte==0x00)) ){/*TODO: 0xb7*/
*recallme = 0;
}
else
*recallme=1;
s->fragmented=0;
*data_size-=4;
}
else{
*recallme=1;
s->fragmented=1;
}
s->vsh1.b=0;
}
if ( me->description.msource != live )
*mtime = (s->hours * 3.6e6) + (s->minutes * 6e4) + (s->seconds * 1000) + (s->temp_ref*40) + (s->picture*40);
s->data_total+=*data_size;
if (s->std==MPEG_2 && !flag) {
#ifdef MPEG2VSHE
s->data_total-=8;
s->vsh1.t=1;
#else
s->data_total-=4;
s->vsh1.t=0;
#endif
} else {
s->data_total-=4;
s->vsh1.t=0;
}
s->vsh1.mbz=0;
s->vsh1.an=0;
s->vsh1.n=0;
if (seq_head_pres) {
s->vsh1.s=1;
} else {
s->vsh1.s=0;
}
// s->vsh1.b=1;
if (!s->fragmented) {
s->vsh1.e=1;
} else {
s->vsh1.e=0;
}
vsh1_1 = (char *)(&s->vsh1); /* to see the struct as a set of bytes */
data_tmp[0] = vsh1_1[3];
data_tmp[1] = vsh1_1[2];
data_tmp[2] = vsh1_1[1];
data_tmp[3] = vsh1_1[0];
#ifdef MPEG2VSHE
if (s->std == MPEG_2 && !flag) {
vsh2_1 = (char *)(&s->vsh2);
data_tmp[4] = vsh2_1[3];
data_tmp[5] = vsh2_1[2];
data_tmp[6] = vsh2_1[1];
data_tmp[7] = vsh2_1[0];
}
#endif
flag=0; /*so it is just probed*/
memcpy(data, data_tmp, *data_size);
#if !HAVE_ALLOCA
free(data_tmp);
#endif
*marker=!(*recallme);
return ERR_NOERROR;
}
template<typename PointT, typename NormalT> void
pcl::NormalSpaceSampling<PointT, NormalT>::applyFilter (PointCloud &output)
{
// If sample size is 0 or if the sample size is greater then input cloud size
// then return entire copy of cloud
if (sample_ >= input_->size ())
{
output = *input_;
return;
}
// Resize output cloud to sample size
output.points.resize (sample_);
output.width = sample_;
output.height = 1;
// allocate memory for the histogram of normals.. Normals will then be sampled from each bin..
unsigned int n_bins = binsx_ * binsy_ * binsz_;
// list<int> holds the indices of points in that bin.. Using list to avoid repeated resizing of vectors.. Helps when the point cloud is
// large..
std::vector< std::list <int> > normals_hg;
normals_hg.reserve (n_bins);
for (unsigned int i = 0; i < n_bins; i++)
normals_hg.push_back (std::list<int> ());
for (unsigned int i = 0; i < input_normals_->points.size (); i++)
{
unsigned int bin_number = findBin (input_normals_->points[i].normal, n_bins);
normals_hg[bin_number].push_back (i);
}
// Setting up random access for the list created above.. Maintaining the iterators to individual elements of the list in a vector.. Using
// vector now as the size of the list is known..
std::vector< std::vector <std::list<int>::iterator> > random_access (normals_hg.size ());
for (unsigned int i = 0; i < normals_hg.size (); i++)
{
random_access.push_back (std::vector< std::list<int>::iterator> ());
random_access[i].resize (normals_hg[i].size ());
unsigned int j=0;
for (std::list<int>::iterator itr = normals_hg[i].begin (); itr != normals_hg[i].end (); itr++, j++)
{
random_access[i][j] = itr;
}
}
unsigned int* start_index = new unsigned int[normals_hg.size ()];
start_index[0] = 0;
unsigned int prev_index = start_index[0];
for (unsigned int i = 1; i < normals_hg.size (); i++)
{
start_index[i] = prev_index + normals_hg[i-1].size ();
prev_index = start_index[i];
}
// maintaining flags to check if a point is sampled
boost::dynamic_bitset<> is_sampled_flag (input_normals_->points.size ());
// maintaining flags to check if all points in the bin are sampled
boost::dynamic_bitset<> bin_empty_flag (normals_hg.size ());
unsigned int i = 0;
while (i < sample_)
{
// iterating through every bin and picking one point at random, until the required number of points are sampled..
for (unsigned int j = 0; j < normals_hg.size (); j++)
{
unsigned int M = normals_hg[j].size ();
if (M == 0 || bin_empty_flag.test (j))// bin_empty_flag(i) is set if all points in that bin are sampled..
{
continue;
}
unsigned int pos = 0;
unsigned int random_index = 0;
//picking up a sample at random from jth bin
do
{
random_index = std::rand () % M;
pos = start_index[j] + random_index;
} while (is_sampled_flag.test (pos));
is_sampled_flag.flip (start_index[j] + random_index);
// checking if all points in bin j are sampled..
if (isEntireBinSampled (is_sampled_flag, start_index[j], normals_hg[j].size ()))
{
bin_empty_flag.flip (j);
}
unsigned int index = *(random_access[j][random_index]);
output.points[i] = input_->points[index];
i++;
if (i == sample_)
{
break;
}
}
}
delete[] start_index;
}
template<typename PointT, typename NormalT> void
pcl::NormalSpaceSampling<PointT, NormalT>::applyFilter (std::vector<int> &indices)
{
if (!initCompute ())
{
indices = *indices_;
return;
}
unsigned int max_values = (std::min) (sample_, static_cast<unsigned int> (input_normals_->size ()));
// Resize output indices to sample size
indices.resize (max_values);
removed_indices_->resize (max_values);
// Allocate memory for the histogram of normals. Normals will then be sampled from each bin.
unsigned int n_bins = binsx_ * binsy_ * binsz_;
// list<int> holds the indices of points in that bin. Using list to avoid repeated resizing of vectors.
// Helps when the point cloud is large.
std::vector<std::list <int> > normals_hg;
normals_hg.reserve (n_bins);
for (unsigned int i = 0; i < n_bins; i++)
normals_hg.emplace_back();
for (std::vector<int>::const_iterator it = indices_->begin (); it != indices_->end (); ++it)
{
unsigned int bin_number = findBin (input_normals_->points[*it].normal, n_bins);
normals_hg[bin_number].push_back (*it);
}
// Setting up random access for the list created above. Maintaining the iterators to individual elements of the list
// in a vector. Using vector now as the size of the list is known.
std::vector<std::vector<std::list<int>::iterator> > random_access (normals_hg.size ());
for (size_t i = 0; i < normals_hg.size (); i++)
{
random_access.emplace_back();
random_access[i].resize (normals_hg[i].size ());
size_t j = 0;
for (std::list<int>::iterator itr = normals_hg[i].begin (); itr != normals_hg[i].end (); itr++, j++)
random_access[i][j] = itr;
}
std::vector<size_t> start_index (normals_hg.size ());
start_index[0] = 0;
size_t prev_index = 0;
for (size_t i = 1; i < normals_hg.size (); i++)
{
start_index[i] = prev_index + normals_hg[i-1].size ();
prev_index = start_index[i];
}
// Maintaining flags to check if a point is sampled
boost::dynamic_bitset<> is_sampled_flag (input_normals_->points.size ());
// Maintaining flags to check if all points in the bin are sampled
boost::dynamic_bitset<> bin_empty_flag (normals_hg.size ());
unsigned int i = 0;
while (i < sample_)
{
// Iterating through every bin and picking one point at random, until the required number of points are sampled.
for (size_t j = 0; j < normals_hg.size (); j++)
{
unsigned int M = static_cast<unsigned int> (normals_hg[j].size ());
if (M == 0 || bin_empty_flag.test (j)) // bin_empty_flag(i) is set if all points in that bin are sampled..
continue;
unsigned int pos = 0;
unsigned int random_index = 0;
// Picking up a sample at random from jth bin
do
{
random_index = static_cast<unsigned int> ((*rng_uniform_distribution_) () % M);
pos = start_index[j] + random_index;
} while (is_sampled_flag.test (pos));
is_sampled_flag.flip (start_index[j] + random_index);
// Checking if all points in bin j are sampled.
if (isEntireBinSampled (is_sampled_flag, start_index[j], static_cast<unsigned int> (normals_hg[j].size ())))
bin_empty_flag.flip (j);
unsigned int index = *(random_access[j][random_index]);
indices[i] = index;
i++;
if (i == sample_)
break;
}
}
// If we need to return the indices that we haven't sampled
if (extract_removed_indices_)
{
std::vector<int> indices_temp = indices;
std::sort (indices_temp.begin (), indices_temp.end ());
std::vector<int> all_indices_temp = *indices_;
std::sort (all_indices_temp.begin (), all_indices_temp.end ());
set_difference (all_indices_temp.begin (), all_indices_temp.end (),
indices_temp.begin (), indices_temp.end (),
inserter (*removed_indices_, removed_indices_->begin ()));
}
}
