klg日誌文件中提取RGBD圖像數據

main函數
本程序用於從klg日誌文件中提取RGB圖像和Depth圖像信息，klg文件在ElasticFusion中被用於保存數據集。
涉及的相關開源代碼如下：
LogView
mp3guy/Logger1
mp3guy/Logger2
HTLife/png_to_klg

#include "Resolution.h"
#include "RawLogReader.h"
#include <fstream>
#include <iomanip>
#include <string>
#include <iostream>

int find_argument(int argc, char** argv, const char* argument_name)
{
    for(int i = 1; i < argc; ++i)
    {
        if(strcmp(argv[i], argument_name) == 0)
        {
            return i;
        }
    }
    return -1;
}

int parse_argument (int argc, char** argv, const char* str, int &val)
{
    int index = find_argument (argc, argv, str) + 1;

    if (index > 0 && index < argc )
        val = atoi (argv[index]);

    return (index - 1);
}

int parse_argument (int argc, char** argv, const char* str, std::string &val)
{
    int index = find_argument (argc, argv, str) + 1;

    if (index > 0 && index < argc )
        val = argv[index];

    return index - 1;
}

int main(int argc, char * argv[])
{
    int width = 640;
    int height = 480;

    parse_argument(argc, argv, "-w", width);
    parse_argument(argc, argv, "-h", height);

    Resolution::getInstance(width, height);

    Bytef * decompressionBuffer = new Bytef[Resolution::getInstance().numPixels() * 2];     //存儲深度圖的指針
    IplImage * deCompImage = 0;     //存儲rgb圖的指針

    std::string logFile;
    assert(parse_argument(argc, argv, "-l", logFile) > 0 && "Please provide a log file");

    RawLogReader logReader(decompressionBuffer,
                           deCompImage,
                           logFile,
                           find_argument(argc, argv, "-f") != -1);

    cv::Mat1b tmp(height, width);
    cv::Mat3b depthImg(height, width);

    while(logReader.hasMore())
    {
        logReader.getNext();

        cv::Mat3b rgbImg(height, width, (cv::Vec<unsigned char, 3> *)logReader.deCompImage->imageData);

        cv::Mat1w depth(height, width, (unsigned short *)&decompressionBuffer[0]);

        cv::normalize(depth, tmp, 0, 255, cv::NORM_MINMAX, 0);

        cv::cvtColor(tmp, depthImg, CV_GRAY2RGB);

//        cv::imshow("RGB", rgbImg);
        std::cout<<setiosflags(std::ios::fixed)<<logReader.timestamp<<std::endl;
        cv::imwrite("rgb"+std::to_string(logReader.timestamp)+".png",rgbImg);
        cv::imwrite("depth"+std::to_string(logReader.timestamp)+".png",depth);
//        cv::imshow("Depth", depthImg);

        char key = cv::waitKey(1);

        if(key == 'q')
            break;
        else if(key == ' ')
            key = cv::waitKey(0);
        if(key == 'q')
            break;
    }

    delete [] decompressionBuffer;

    if(deCompImage)
    {
        cvReleaseImage(&deCompImage);
    }

    return 0;
}

Resolution.h文件

/*
 * Resolution.h
 *
 *  Created on: 18 Nov 2012
 *      Author: thomas
 */

#ifndef RESOLUTION_H_
#define RESOLUTION_H_

#include <cassert>

class Resolution
{
    public:
        static const Resolution & getInstance(int width = 0, int height = 0)
        {
            static const Resolution instance(width, height);
            return instance;
        }

        const int & width() const
        {
            return imgWidth;
        }

        const int & height() const
        {
            return imgHeight;
        }

        const int & cols() const
        {
            return imgWidth;
        }

        const int & rows() const
        {
            return imgHeight;
        }

        const int & numPixels() const
        {
            return imgNumPixels;
        }

    private:
        Resolution(int width, int height)
         : imgWidth(width),
           imgHeight(height),
           imgNumPixels(width * height)
        {
            assert(width > 0 && height > 0);
        }

        const int imgWidth;
        const int imgHeight;
        const int imgNumPixels;
};

#endif /* RESOLUTION_H_ */

RawLogReader.h文件

/*
 * RawLogReader.h
 *
 *  Created on: 19 Nov 2012
 *      Author: thomas
 */

#ifndef RAWLOGREADER_H_
#define RAWLOGREADER_H_

#include <cassert>
#include "Resolution.h"
#include <zlib.h>
#include <opencv2/opencv.hpp>
#include <stdio.h>
#include <string>
#include <boost/filesystem.hpp>

class RawLogReader
{
    public:
        RawLogReader(Bytef *& decompressionBuffer,
                     IplImage *& deCompImage,
                     std::string file,
                     bool flipColors);

        virtual ~RawLogReader();

        void getNext();

        int getNumFrames();

        bool hasMore();

        Bytef *& decompressionBuffer;
        IplImage *& deCompImage;
        unsigned char * depthReadBuffer;
        unsigned char * imageReadBuffer;
        int64_t timestamp;
        int32_t depthSize;
        int32_t imageSize;

    private:
        const std::string file;
        FILE * fp;
        int32_t numFrames;
        int currentFrame;
        bool flipColors;
        bool isCompressed;
};

#endif /* RAWLOGREADER_H_ */

RawLogReader.cpp文件

/*
 * RawLogReader.cpp
 *
 *  Created on: 19 Nov 2012
 *      Author: thomas
 */

#include "RawLogReader.h"

/**
 * @brief
 * @param [in] deCompImage 指向depth圖的指針
 * @param [in] decompressionBuffer 指向RGB圖的指針
 * @param [in] file 輸入的klg文件地址
 * @param [in] flipColors 是否翻轉顏色的bool變量
 * */
RawLogReader::RawLogReader(Bytef *& decompressionBuffer,
                           IplImage *& deCompImage,
                           std::string file,
                           bool flipColors)
 : decompressionBuffer(decompressionBuffer),
   deCompImage(deCompImage),
   file(file),
   flipColors(flipColors), 
   isCompressed(false)
{
    assert(boost::filesystem::exists(file.c_str()));

    fp = fopen(file.c_str(), "rb");     //以binary方式讀入文件，得到文件指針fp

    currentFrame = 0;

    assert(fread(&numFrames, sizeof(int32_t), 1, fp));  //文件的第一行是一個int32_t類型的幀數

    depthReadBuffer = new unsigned char[Resolution::getInstance().numPixels() * 2];     //深度圖是16UC1的，所以一個像素佔用16bit
    imageReadBuffer = new unsigned char[Resolution::getInstance().numPixels() * 3];     //RGB圖是8UC1的，一個像素佔用8bit
}

RawLogReader::~RawLogReader()
{
    delete [] depthReadBuffer;
    delete [] imageReadBuffer;

    fclose(fp);
}

void RawLogReader::getNext()
{
    assert(fread(&timestamp, sizeof(int64_t), 1, fp));

    assert(fread(&depthSize, sizeof(int32_t), 1, fp));
    assert(fread(&imageSize, sizeof(int32_t), 1, fp));

    assert(fread(depthReadBuffer, depthSize, 1, fp));

    if(imageSize > 0)
    {
        assert(fread(imageReadBuffer, imageSize, 1, fp));
    }

    if(deCompImage != 0)
    {
        cvReleaseImage(&deCompImage);
    }

    CvMat tempMat = cvMat(1, imageSize, CV_8UC1, (void *)imageReadBuffer);

    if(imageSize == Resolution::getInstance().numPixels() * 3)
    {
        isCompressed = false;

        deCompImage = cvCreateImage(cvSize(Resolution::getInstance().width(), Resolution::getInstance().height()), IPL_DEPTH_8U, 3);

        memcpy(deCompImage->imageData, imageReadBuffer, Resolution::getInstance().numPixels() * 3);
    }
    else if(imageSize > 0)
    {
        isCompressed = true;

        deCompImage = cvDecodeImage(&tempMat);
    }
    else
    {
        isCompressed = false;

        deCompImage = cvCreateImage(cvSize(Resolution::getInstance().width(), Resolution::getInstance().height()), IPL_DEPTH_8U, 3);

        memset(deCompImage->imageData, 0, Resolution::getInstance().numPixels() * 3);
    }

    if(depthSize == Resolution::getInstance().numPixels() * 2)
    {
        //RGB should not be compressed in this case
        assert(!isCompressed);

        memcpy(&decompressionBuffer[0], depthReadBuffer, Resolution::getInstance().numPixels() * 2);
    }
    else if(depthSize > 0)
    {
        //RGB should also be compressed
        assert(isCompressed);

        unsigned long decompLength = Resolution::getInstance().numPixels() * 2;

        uncompress(&decompressionBuffer[0], (unsigned long *)&decompLength, (const Bytef *)depthReadBuffer, depthSize);
    }
    else
    {
        isCompressed = false;

        memset(&decompressionBuffer[0], 0, Resolution::getInstance().numPixels() * 2);
    }

    if(flipColors)
    {
        cv::Mat3b rgb(Resolution::getInstance().rows(), Resolution::getInstance().cols(), (cv::Vec<unsigned char, 3> *)deCompImage->imageData, Resolution::getInstance().width() * 3);
        cv::cvtColor(rgb, rgb, CV_RGB2BGR);
    }

    currentFrame++;
}

int RawLogReader::getNumFrames()
{
    return numFrames;
}

bool RawLogReader::hasMore()
{
    return currentFrame + 1 < numFrames;
}