Math::Vector< double, 2 > operator*( const Math::Matrix< double, 3, 3 > mat, const Math::Vector< double, 2 > vec )
{
Math::Vector< double, 3 > hom( vec(0), vec(1), 1.0 );
Math::Vector< double, 2 > tmp;
Math::Vector< double, 2 > res;
tmp = boost::numeric::ublas::prod( mat, hom );
double w = tmp[1];
tmp = tmp / w;
res[0] = tmp[0];
res[1] = tmp[1];
return res;
};
Math::Vector< double, 3 > operator*( const Math::Matrix< double, 4, 4 > mat, const Math::Vector< double, 3 > vec )
{
Math::Vector< double, 4 > hom( vec[0], vec[1], vec[2], 1.0 );
Math::Vector< double, 4 > tmp;
Math::Vector< double, 3 > res;
tmp = boost::numeric::ublas::prod( mat, hom );
double w = tmp[3];
tmp = tmp / w;
res[0] = tmp[0];
res[1] = tmp[1];
res[2] = tmp[2];
return res;
};
int main(void)
{
/* Create the linear system: */
Math::Matrix a(12,12,0.0);
{
/* Open the calibration data file: */
Misc::FileCharacterSource dataSource("CalibrationData.csv");
Misc::CSVSource data(dataSource);
unsigned int numEntries=0;
while(!data.eof())
{
/* Read a calibration entry from the data file: */
double x=data.readField<double>();
double y=data.readField<double>();
double z=data.readField<double>();
double s=data.readField<double>()/640.0;
double t=data.readField<double>()/480.0;
/* Insert the entry's two linear equations into the linear system: */
double eq[2][12];
eq[0][0]=x;
eq[0][1]=y;
eq[0][2]=z;
eq[0][3]=1.0;
eq[0][4]=0.0;
eq[0][5]=0.0;
eq[0][6]=0.0;
eq[0][7]=0.0;
eq[0][8]=-s*x;
eq[0][9]=-s*y;
eq[0][10]=-s*z;
eq[0][11]=-s;
eq[1][0]=0.0;
eq[1][1]=0.0;
eq[1][2]=0.0;
eq[1][3]=0.0;
eq[1][4]=x;
eq[1][5]=y;
eq[1][6]=z;
eq[1][7]=1.0;
eq[1][8]=-t*x;
eq[1][9]=-t*y;
eq[1][10]=-t*z;
eq[1][11]=-t;
for(int row=0;row<2;++row)
{
for(unsigned int i=0;i<12;++i)
for(unsigned int j=0;j<12;++j)
a.set(i,j,a(i,j)+eq[row][i]*eq[row][j]);
}
++numEntries;
}
std::cout<<numEntries<<" calibration data entries read from file"<<std::endl;
}
/* Find the linear system's smallest eigenvalue: */
std::pair<Math::Matrix,Math::Matrix> qe=a.jacobiIteration();
unsigned int minEIndex=0;
double minE=Math::abs(qe.second(0,0));
for(unsigned int i=1;i<12;++i)
{
if(minE>Math::abs(qe.second(i,0)))
{
minEIndex=i;
minE=Math::abs(qe.second(i,0));
}
}
/* Create the normalized homography: */
Math::Matrix hom(3,4);
double scale=qe.first(11,minEIndex);
for(int i=0;i<3;++i)
for(int j=0;j<4;++j)
hom.set(i,j,qe.first(i*4+j,minEIndex)/scale);
{
/* Open the calibration data file: */
Misc::FileCharacterSource dataSource("CalibrationData.csv");
Misc::CSVSource data(dataSource);
/* Test the homography on all calibration data entries: */
while(!data.eof())
{
/* Read a calibration entry from the data file: */
Math::Matrix world(4,1);
for(unsigned int i=0;i<3;++i)
world.set(i,data.readField<double>());
world.set(3,1.0);
double s=data.readField<double>();
double t=data.readField<double>();
/* Apply the homography: */
Math::Matrix str=hom*world;
std::cout<<"Result: s = "<<str(0)/str(2)<<", t = "<<str(1)/str(2)<<std::endl;
}
}
/* Open the calibration file: */
Misc::File matrixFile("CameraCalibrationMatrices.dat","wb",Misc::File::LittleEndian);
/* Create the depth projection matrix: */
Math::Matrix depthProjection(4,4,0.0);
double cameraFov=560.0;
depthProjection(0,0)=1.0/cameraFov;
depthProjection(0,3)=-320.0/cameraFov;
depthProjection(1,1)=1.0/cameraFov;
depthProjection(1,3)=-240.0/cameraFov;
depthProjection(2,3)=-1.0;
depthProjection(3,2)=-1.0/34400.0;
depthProjection(3,3)=1090.0/34400.0;
/* Save the depth projection matrix: */
for(unsigned int i=0;i<4;++i)
for(unsigned int j=0;j<4;++j)
matrixFile.write<double>(depthProjection(i,j));
/* Create the color projection matrix by extending the homography: */
Math::Matrix colorProjection(4,4);
for(unsigned int i=0;i<2;++i)
for(unsigned int j=0;j<4;++j)
colorProjection(i,j)=hom(i,j);
for(unsigned int j=0;j<4;++j)
colorProjection(2,j)=j==2?1.0:0.0;
for(unsigned int j=0;j<4;++j)
colorProjection(3,j)=hom(2,j);
/* Modify the color projection matrix by the depth projection matrix: */
colorProjection*=depthProjection;
/* Save the color projection matrix: */
for(unsigned int i=0;i<4;++i)
for(unsigned int j=0;j<4;++j)
matrixFile.write<double>(colorProjection(i,j));
return 0;
}
static int encode_init(AVCodecContext *avctx)
{
DCAContext *c = avctx->priv_data;
uint64_t layout = avctx->channel_layout;
int i, min_frame_bits;
c->fullband_channels = c->channels = avctx->channels;
c->lfe_channel = (avctx->channels == 3 || avctx->channels == 6);
c->band_interpolation = band_interpolation[1];
c->band_spectrum = band_spectrum[1];
c->worst_quantization_noise = -2047;
c->worst_noise_ever = -2047;
if (!layout) {
av_log(avctx, AV_LOG_WARNING, "No channel layout specified. The "
"encoder will guess the layout, but it "
"might be incorrect.\n");
layout = av_get_default_channel_layout(avctx->channels);
}
switch (layout) {
case AV_CH_LAYOUT_MONO: c->channel_config = 0; break;
case AV_CH_LAYOUT_STEREO: c->channel_config = 2; break;
case AV_CH_LAYOUT_2_2: c->channel_config = 8; break;
case AV_CH_LAYOUT_5POINT0: c->channel_config = 9; break;
case AV_CH_LAYOUT_5POINT1: c->channel_config = 9; break;
default:
av_log(avctx, AV_LOG_ERROR, "Unsupported channel layout!\n");
return AVERROR_PATCHWELCOME;
}
if (c->lfe_channel)
c->fullband_channels--;
for (i = 0; i < 9; i++) {
if (sample_rates[i] == avctx->sample_rate)
break;
}
if (i == 9)
return AVERROR(EINVAL);
c->samplerate_index = i;
if (avctx->bit_rate < 32000 || avctx->bit_rate > 3840000) {
av_log(avctx, AV_LOG_ERROR, "Bit rate %i not supported.", avctx->bit_rate);
return AVERROR(EINVAL);
}
for (i = 0; dca_bit_rates[i] < avctx->bit_rate; i++)
;
c->bitrate_index = i;
avctx->bit_rate = dca_bit_rates[i];
c->frame_bits = FFALIGN((avctx->bit_rate * 512 + avctx->sample_rate - 1) / avctx->sample_rate, 32);
min_frame_bits = 132 + (493 + 28 * 32) * c->fullband_channels + c->lfe_channel * 72;
if (c->frame_bits < min_frame_bits || c->frame_bits > (DCA_MAX_FRAME_SIZE << 3))
return AVERROR(EINVAL);
c->frame_size = (c->frame_bits + 7) / 8;
avctx->frame_size = 32 * SUBBAND_SAMPLES;
if (!cos_table[0]) {
int j, k;
for (i = 0; i < 2048; i++) {
cos_table[i] = (int32_t)(0x7fffffff * cos(M_PI * i / 1024));
cb_to_level[i] = (int32_t)(0x7fffffff * pow(10, -0.005 * i));
}
/* FIXME: probably incorrect */
for (i = 0; i < 256; i++) {
lfe_fir_64i[i] = (int32_t)(0x01ffffff * lfe_fir_64[i]);
lfe_fir_64i[511 - i] = (int32_t)(0x01ffffff * lfe_fir_64[i]);
}
for (i = 0; i < 512; i++) {
band_interpolation[0][i] = (int32_t)(ULLN(0x1000000000) * fir_32bands_perfect[i]);
band_interpolation[1][i] = (int32_t)(ULLN(0x1000000000) * fir_32bands_nonperfect[i]);
}
for (i = 0; i < 9; i++) {
for (j = 0; j < AUBANDS; j++) {
for (k = 0; k < 256; k++) {
double freq = sample_rates[i] * (k + 0.5) / 512;
auf[i][j][k] = (int32_t)(10 * (hom(freq) + gammafilter(j, freq)));
}
}
}
for (i = 0; i < 256; i++) {
double add = 1 + pow(10, -0.01 * i);
cb_to_add[i] = (int32_t)(100 * log10(add));
}
for (j = 0; j < 8; j++) {
double accum = 0;
for (i = 0; i < 512; i++) {
double reconst = fir_32bands_perfect[i] * ((i & 64) ? (-1) : 1);
accum += reconst * cos(2 * M_PI * (i + 0.5 - 256) * (j + 0.5) / 512);
}
band_spectrum[0][j] = (int32_t)(200 * log10(accum));
}
for (j = 0; j < 8; j++) {
double accum = 0;
for (i = 0; i < 512; i++) {
double reconst = fir_32bands_nonperfect[i] * ((i & 64) ? (-1) : 1);
accum += reconst * cos(2 * M_PI * (i + 0.5 - 256) * (j + 0.5) / 512);
}
band_spectrum[1][j] = (int32_t)(200 * log10(accum));
}
}
return 0;
}
void TranslationRotation3D::getF(double *F_out) const {
Eigen::Map<Eigen::Matrix<double, 4, 4, Eigen::RowMajor> > hom(F_out);
hom << eigenRotation(), eigenTranslation(), 0.0, 0.0, 0.0, 1.0;
}
static int encode_init(AVCodecContext *avctx)
{
DCAEncContext *c = avctx->priv_data;
uint64_t layout = avctx->channel_layout;
int i, j, min_frame_bits;
c->fullband_channels = c->channels = avctx->channels;
c->lfe_channel = (avctx->channels == 3 || avctx->channels == 6);
c->band_interpolation = band_interpolation[1];
c->band_spectrum = band_spectrum[1];
c->worst_quantization_noise = -2047;
c->worst_noise_ever = -2047;
if (!layout) {
av_log(avctx, AV_LOG_WARNING, "No channel layout specified. The "
"encoder will guess the layout, but it "
"might be incorrect.\n");
layout = av_get_default_channel_layout(avctx->channels);
}
switch (layout) {
case AV_CH_LAYOUT_MONO: c->channel_config = 0; break;
case AV_CH_LAYOUT_STEREO: c->channel_config = 2; break;
case AV_CH_LAYOUT_2_2: c->channel_config = 8; break;
case AV_CH_LAYOUT_5POINT0: c->channel_config = 9; break;
case AV_CH_LAYOUT_5POINT1: c->channel_config = 9; break;
default:
av_log(avctx, AV_LOG_ERROR, "Unsupported channel layout!\n");
return AVERROR_PATCHWELCOME;
}
if (c->lfe_channel) {
c->fullband_channels--;
c->channel_order_tab = channel_reorder_lfe[c->channel_config];
} else {
c->channel_order_tab = channel_reorder_nolfe[c->channel_config];
}
for (i = 0; i < MAX_CHANNELS; i++) {
for (j = 0; j < DCA_CODE_BOOKS; j++) {
c->quant_index_sel[i][j] = ff_dca_quant_index_group_size[j];
}
/* 6 - no Huffman */
c->bit_allocation_sel[i] = 6;
}
for (i = 0; i < 9; i++) {
if (sample_rates[i] == avctx->sample_rate)
break;
}
if (i == 9)
return AVERROR(EINVAL);
c->samplerate_index = i;
if (avctx->bit_rate < 32000 || avctx->bit_rate > 3840000) {
av_log(avctx, AV_LOG_ERROR, "Bit rate %"PRId64" not supported.", (int64_t)avctx->bit_rate);
return AVERROR(EINVAL);
}
for (i = 0; ff_dca_bit_rates[i] < avctx->bit_rate; i++)
;
c->bitrate_index = i;
c->frame_bits = FFALIGN((avctx->bit_rate * 512 + avctx->sample_rate - 1) / avctx->sample_rate, 32);
min_frame_bits = 132 + (493 + 28 * 32) * c->fullband_channels + c->lfe_channel * 72;
if (c->frame_bits < min_frame_bits || c->frame_bits > (DCA_MAX_FRAME_SIZE << 3))
return AVERROR(EINVAL);
c->frame_size = (c->frame_bits + 7) / 8;
avctx->frame_size = 32 * SUBBAND_SAMPLES;
if (!cos_table[0]) {
int j, k;
cos_table[0] = 0x7fffffff;
cos_table[512] = 0;
cos_table[1024] = -cos_table[0];
for (i = 1; i < 512; i++) {
cos_table[i] = (int32_t)(0x7fffffff * cos(M_PI * i / 1024));
cos_table[1024-i] = -cos_table[i];
cos_table[1024+i] = -cos_table[i];
cos_table[2048-i] = cos_table[i];
}
for (i = 0; i < 2048; i++) {
cb_to_level[i] = (int32_t)(0x7fffffff * ff_exp10(-0.005 * i));
}
for (k = 0; k < 32; k++) {
for (j = 0; j < 8; j++) {
lfe_fir_64i[64 * j + k] = (int32_t)(0xffffff800000ULL * ff_dca_lfe_fir_64[8 * k + j]);
lfe_fir_64i[64 * (7-j) + (63 - k)] = (int32_t)(0xffffff800000ULL * ff_dca_lfe_fir_64[8 * k + j]);
}
}
for (i = 0; i < 512; i++) {
band_interpolation[0][i] = (int32_t)(0x1000000000ULL * ff_dca_fir_32bands_perfect[i]);
band_interpolation[1][i] = (int32_t)(0x1000000000ULL * ff_dca_fir_32bands_nonperfect[i]);
}
for (i = 0; i < 9; i++) {
for (j = 0; j < AUBANDS; j++) {
for (k = 0; k < 256; k++) {
double freq = sample_rates[i] * (k + 0.5) / 512;
auf[i][j][k] = (int32_t)(10 * (hom(freq) + gammafilter(j, freq)));
}
}
}
for (i = 0; i < 256; i++) {
double add = 1 + ff_exp10(-0.01 * i);
cb_to_add[i] = (int32_t)(100 * log10(add));
}
for (j = 0; j < 8; j++) {
double accum = 0;
for (i = 0; i < 512; i++) {
double reconst = ff_dca_fir_32bands_perfect[i] * ((i & 64) ? (-1) : 1);
accum += reconst * cos(2 * M_PI * (i + 0.5 - 256) * (j + 0.5) / 512);
}
band_spectrum[0][j] = (int32_t)(200 * log10(accum));
}
for (j = 0; j < 8; j++) {
double accum = 0;
for (i = 0; i < 512; i++) {
double reconst = ff_dca_fir_32bands_nonperfect[i] * ((i & 64) ? (-1) : 1);
accum += reconst * cos(2 * M_PI * (i + 0.5 - 256) * (j + 0.5) / 512);
}
band_spectrum[1][j] = (int32_t)(200 * log10(accum));
}
}
return 0;
}
