Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
read_cov_L(const Eigen::Array<T, Eigen::Dynamic, 1>& CPCs,
const Eigen::Array<T, Eigen::Dynamic, 1>& sds,
T& log_prob) {
size_t K = sds.rows();
// adjust due to transformation from correlations to covariances
log_prob += (sds.log().sum() + LOG_2) * K;
return sds.matrix().asDiagonal() * read_corr_L(CPCs, K, log_prob);
}
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
read_corr_L(const Eigen::Array<T, Eigen::Dynamic, 1>& CPCs,
size_t K,
T& log_prob) {
Eigen::Matrix<T, Eigen::Dynamic, 1> values(CPCs.rows() - 1);
size_t pos = 0;
// no need to abs() because this Jacobian determinant
// is strictly positive (and triangular)
// see inverse of Jacobian in equation 11 of LKJ paper
for (size_t k = 1; k <= (K - 2); k++)
for (size_t i = k + 1; i <= K; i++) {
values(pos) = (K - k - 1) * log1m(square(CPCs(pos)));
pos++;
}
log_prob += 0.5 * sum(values);
return read_corr_L(CPCs, K);
}
bool SavePng(std::string filename, const DepthImage& img)
{
// Open up a file for writing
FILE* output = fopen(filename.c_str(), "wb");
if (output == NULL)
{
printf("Failed to open PNG file for writing (errno = %i)\n", errno);
return false;
}
// Create a png pointer with the callbacks above
png_structp png_ptr = png_create_write_struct(
PNG_LIBPNG_VER_STRING, NULL, on_png_error, on_png_warn);
if (png_ptr == NULL)
{
fprintf(stderr, "Failed to allocate png write_struct\n");
fclose(output);
return false;
}
// Create an info pointer
png_infop info_ptr = png_create_info_struct(png_ptr);
if (info_ptr == NULL)
{
fprintf(stderr, "Failed to create png info_struct");
png_destroy_write_struct(&png_ptr, &info_ptr);
fclose(output);
return false;
}
// Set physical vars
png_set_IHDR(png_ptr, info_ptr, img.cols(), img.rows(), 16,
PNG_COLOR_TYPE_GRAY, PNG_INTERLACE_NONE,
PNG_COMPRESSION_TYPE_BASE, PNG_FILTER_TYPE_BASE);
png_init_io(png_ptr, output);
const float zmax = img.maxCoeff();
const float zmin = (img == -std::numeric_limits<float>::infinity())
.select(DepthImage::Constant(img.rows(), img.cols(), zmax),
img)
.minCoeff();
auto scaled = (zmax == zmin)
? DepthImage((img - zmin) + 65535)
: DepthImage((img - zmin) * 65534 / (zmax - zmin) + 1);
Eigen::Array<uint16_t, Eigen::Dynamic, Eigen::Dynamic>
pixels = scaled.cast<uint16_t>().transpose();
std::vector<uint16_t*> rows;
for (int i=pixels.cols() - 1; i >= 0; --i)
{
rows.push_back(pixels.data() + i * pixels.rows());
}
png_set_rows(png_ptr, info_ptr, reinterpret_cast<png_bytepp>(&rows[0]));
png_write_png(png_ptr, info_ptr, PNG_TRANSFORM_SWAP_ENDIAN, NULL);
fclose(output);
png_destroy_write_struct(&png_ptr, &info_ptr);
return true;
}
