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; }