// Get entropy of histogram
float GetEntropy(const VecF& hist) {
double h = 0.0;
for (int i = 0; i < hist.size(); i++) {
if (hist[i] > 0) {
h += hist[i] * log(hist[i]);
}
}
return h;
}
void wrCameras(char* file, const VecF<3>& position, const VecF<3>& target, const std::vector<timestampedValue<VecF<3> > >& rolls, const std::vector<timestampedValue<VecF<9> > >& positions, const std::vector<timestampedValue<VecF<9> > >& targets)
{
float fov = 0.785398f;
float farclip = 27.7778f;
float nearclip = 0.222222f;
wr(file,0x1f0,0x0000FFFF);
wr(file,0x200,0xFFFFFFFF);
wr(file,0x204,fov);
wr(file,0x208,farclip);
wr(file,0x20c,nearclip);
wr(file,0x210,0xFFFF0001);
wr(file,0x214,0x00000001);
wr(file,0x218,0x00000270);
wr(file,0x21c,0x00000001);
wr(file,0x220,0x00000278);
wr(file,0x224,position);
wr(file,0x230,0xFFFF0001);
wr(file,0x234,0x00000001);
wr(file,0x238,0x00000280);
wr(file,0x23c,0x00000001);
wr(file,0x240,0x00000288);
wr(file,0x244,target);
wr(file,0x250,0xFFFF0002);
wr(file,0x254,0x00000001);
wr(file,0x258,0x00000290);
wr(file,0x25c,0x00000001);
wr(file,0x260,0x00000298);
uint32_t pos_timestamps = 0x2a0;
uint32_t pos_data = pos_timestamps + positions.size() * sizeof(uint32_t);
uint32_t tar_timestamps = pos_data + positions.size() * sizeof(float)*9;
uint32_t tar_data = tar_timestamps + targets.size() * sizeof(uint32_t);
uint32_t roll_timestamps = tar_data + targets.size() * sizeof(float)*9;
uint32_t roll_data = roll_timestamps + rolls.size() * sizeof(uint32_t);
wr(file,0x270,positions.size());
wr(file,0x274,pos_timestamps);
wr(file,0x278,positions.size());
wr(file,0x27c,pos_data);
wr(file,0x280,targets.size());
wr(file,0x284,tar_timestamps);
wr(file,0x288,targets.size());
wr(file,0x28c,tar_data);
wr(file,0x290,rolls.size());
wr(file,0x294,roll_timestamps);
wr(file,0x298,rolls.size());
wr(file,0x29c,roll_data);
for(int i=0;i<positions.size();++i)
{
wr(file,pos_timestamps+sizeof(uint32_t)*i,positions[i].stamp);
wr(file,pos_data+sizeof(float)*9*i,positions[i].data);
}
for(int i=0;i<targets.size();++i)
{
wr(file,tar_timestamps+sizeof(uint32_t)*i,targets[i].stamp);
wr(file,tar_data+sizeof(float)*9*i,targets[i].data);
}
for(int i=0;i<rolls.size();++i)
{
wr(file,roll_timestamps+sizeof(uint32_t)*i,rolls[i].stamp);
wr(file,roll_data+sizeof(float)*3*i,rolls[i].data);
}
}