source: pacpussensors/trunk/Vislab/20150310_lib3dv-1.2.0/src/file.cc@ 136

/* 3dv-client/file.cc
 *
 * Copyright (C) 2013 VisLab
 *
 * This file is part of 3dv-client; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program; if not, see <http://www.gnu.org/licenses/>.
 */  

#include "file.h"

#include <iostream>

#ifdef ARCHIVE

#include <boost/date_time/posix_time/posix_time_io.hpp>
#include <boost/iostreams/stream_buffer.hpp>
#include <boost/iostreams/stream.hpp>
#include <boost/iostreams/device/back_inserter.hpp>

#include <archive.h>
#include <archive_entry.h>

archive* open_archive(const boost::filesystem::path& path, const std::string& archive_name, archive_file_format::types format, device_params params,uint8_t log_level)
{
    if(!boost::filesystem::exists(path))
        boost::filesystem::create_directories(path);

    archive* archive = archive_write_new();
    
    std::string filename = archive_name;
    
    if(format == archive_file_format::ZIP)
    {
        filename += ".zip";
        archive_write_set_format_zip(archive);
        archive_write_zip_set_compression_store(archive);
    }
    else if(format == archive_file_format::TAR)
    {
        filename += ".tar";
        archive_write_set_format_pax_restricted(archive);
    }
    
    boost::filesystem::path path_filename = path / filename;
    path_filename.make_preferred();
 
    std::cout << "[II] 3dv-client: writing to archive " << path_filename << std::endl;
    
    archive_write_open_filename(archive, path_filename.string().c_str());
    
    std::string buffer;

    boost::iostreams::stream<boost::iostreams::back_insert_device<std::string> > file(boost::iostreams::back_inserter(buffer));
    
    file <<"calibration.u0="<<params.intrinsics.m_u0<<std::endl;
    file <<"calibration.v0="<<params.intrinsics.m_v0<<std::endl;
    file <<"calibration.ku="<<params.intrinsics.m_ku<<std::endl;
    file <<"calibration.kv="<<params.intrinsics.m_kv<<std::endl;
    file <<"calibration.x="<<params.position.m_x<<std::endl;
    file <<"calibration.y="<<params.position.m_y<<std::endl; 
    file <<"calibration.z="<<params.position.m_z<<std::endl;
    file <<"calibration.yaw="<<params.orientation.m_yaw<<std::endl;
    file <<"calibration.pitch="<<params.orientation.m_pitch<<std::endl; 
    file <<"calibration.roll="<<params.orientation.m_roll<<std::endl;
    file <<"calibration.baseline="<<params.baseline<<std::endl;
    file <<"depth mapping.downsample ratio="<<params.downsample<<std::endl;
    file <<"advanced.detection.area.step="<<params.area_step<<std::endl;
            
    std::stringstream filename_ini;
    
    filename_ini << "params.ini";
    

    size_t file_size = buffer.size();
        
    struct archive_entry* entry;
    
    entry = archive_entry_new();
    archive_entry_set_pathname(entry, filename_ini.str().c_str());
    archive_entry_set_ctime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
    archive_entry_set_atime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
    archive_entry_set_mtime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
    archive_entry_set_size(entry, file_size);
    archive_entry_set_filetype(entry, AE_IFREG);
    archive_entry_set_perm(entry, 0644);
    archive_write_header(archive, entry);
    archive_entry_free(entry);
          
    std::cout << "[II] 3dv-client: writing INI file " << filename_ini << std::endl;
    archive_write_data(archive, buffer.data(), buffer.size());
    

    return archive;
}

bool close_archive(archive* archive, uint8_t log_level)
{
    archive_write_close(archive);
    archive_write_free(archive);

        return true;
}

bool write_image_archive(boost::shared_ptr<const lib3dv::image> image, uint32_t guid, uint8_t guid_type, archive* archive, bool autonumber, uint8_t log_level)
{    
    if(log_level == 0)
    {
        std::cout << '*';
        std::cout.flush();
    }

    bool is_mono = image->m_format == lib3dv::image::format::MONO8 ||
                  image->m_format == lib3dv::image::format::MONO16;

    bool is_rgb = image->m_format == lib3dv::image::format::RGB24 ||
                  image->m_format == lib3dv::image::format::RGB48;

    bool is_bayer = image->m_format == lib3dv::image::format::BAYER8_BGGR ||
                    image->m_format == lib3dv::image::format::BAYER8_GBRG ||
                    image->m_format == lib3dv::image::format::BAYER8_GRBG ||
                    image->m_format == lib3dv::image::format::BAYER8_RGGB ||
                    image->m_format == lib3dv::image::format::BAYER16_BGGR ||
                    image->m_format == lib3dv::image::format::BAYER16_GBRG ||
                    image->m_format == lib3dv::image::format::BAYER16_GRBG ||
                    image->m_format == lib3dv::image::format::BAYER16_RGGB;

    bool is_16bpp = (image->m_bpp / image->m_channels) == 16;

    std::stringstream filename;

    switch(image->m_type)
    {
        case lib3dv::image::type::LEFT_RECTIFIED :
        {
            filename << "left_rectified";
            break;
        }

        case lib3dv::image::type::RIGHT_RECTIFIED :
        {
            filename << "right_rectified";
            break;
        }

        case lib3dv::image::type::LEFT_RAW :
        {
            filename << "left_raw";
            break;
        }

        case lib3dv::image::type::RIGHT_RAW:
        {
            filename << "right_raw";
            break;
        }

        case lib3dv::image::type::DSI :
        {
            filename << "dsi";
            break;
        }
    }

    boost::posix_time::ptime timestamp = image->m_timestamp;

    if(autonumber)
        filename << "-" << std::setfill('0') << std::setw(6) << guid / guid_type;

    size_t file_size;
    
    if(is_bayer)
    {
        filename << ".pgm";
        file_size = is_16bpp ? pgm_file_size(image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint16_t>((1 << (image->m_bpp / image->m_channels)) - 1)) :
                               pgm_file_size(image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint8_t>((1 << (image->m_bpp / image->m_channels)) - 1));
    }
    else if(is_mono)
    {
        filename << ".pgm";
        file_size = is_16bpp ? pgm_file_size(image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint16_t>((1 << image->m_bpp) - 1)) :
                               pgm_file_size(image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint8_t>((1 << image->m_bpp) - 1));
    }
    else if(is_rgb)
    {
        filename << ".ppm";
        file_size = is_16bpp ? ppm_file_size(image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint16_t>((1 << image->m_bpp) - 1)) : 
                               ppm_file_size(image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint8_t>((1 << image->m_bpp) - 1));
    }

    if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << filename.str() << " (" << image->m_buffer.size() / 1024 << " KB)" << std::endl;
    
    bool success = false;

    struct archive_entry* entry;
    
    entry = archive_entry_new();
    archive_entry_set_pathname(entry, filename.str().c_str());
    archive_entry_set_ctime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
    archive_entry_set_atime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
    archive_entry_set_mtime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
    archive_entry_set_size(entry, file_size);
    archive_entry_set_filetype(entry, AE_IFREG);
    archive_entry_set_perm(entry, 0644);
    archive_write_header(archive, entry);
    archive_entry_free(entry);
    
    if(is_bayer){
        success = is_16bpp ? write_pgm(archive, reinterpret_cast<const uint16_t*>(image->m_buffer.data()), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint16_t>((1 << image->m_bpp/image->m_channels) - 1))
                           : write_pgm(archive, image->m_buffer.data(), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint8_t>((1 << image->m_bpp/image->m_channels) - 1));
    } else if(is_mono){
        success = is_16bpp ? write_pgm(archive, reinterpret_cast<const uint16_t*>(image->m_buffer.data()), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint16_t>((1 << image->m_bpp) - 1))
                           : write_pgm(archive, image->m_buffer.data(), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint8_t>((1 << image->m_bpp) - 1));
    }
    else if(is_rgb){
        success = is_16bpp ? write_ppm(archive, reinterpret_cast<const uint16_t*>(image->m_buffer.data()), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint16_t>((1 << image->m_bpp) - 1))
                           : write_ppm(archive, image->m_buffer.data(), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint8_t>((1 << image->m_bpp) - 1));
    }

    if(!success)
        std::cerr << "[EE] client::write_file(): unsupported image type" << std::endl;

    return success;
}

bool write_terrain_archive(boost::shared_ptr<const lib3dv::terrain> terrain, uint32_t guid, uint8_t guid_type, archive* archive, bool autonumber, data_file_format::types format, uint8_t log_level)
{
   if(log_level == 0)
    {
        std::cout << '.';
        std::cout.flush();
    }

    const std::vector<lib3dv::point3>& points = terrain->m_data;
            
    std::string buffer;

    boost::iostreams::stream<boost::iostreams::back_insert_device<std::string> > ostream(boost::iostreams::back_inserter(buffer));

    switch(format)
    {
        case data_file_format::BINARY :
        {
            boost::archive::binary_oarchive archive(ostream);
            archive << *terrain;
            break;
        }

        case data_file_format::TEXT :
        {
            boost::archive::text_oarchive archive(ostream);
            archive << *terrain;
            break;
        }
        
        case data_file_format::GNUPLOT :
        {
            for(unsigned int i = 0; i < points.size(); ++i)
                ostream << points[i].m_x << ' ' << points[i].m_y << ' ' << points[i].m_z << std::endl;
            break;
        }

        case data_file_format::RAW :
        default:
        {
            ostream.write(reinterpret_cast<const char*>(points.data()), points.size() * sizeof(float));
        }
    }
        
    std::stringstream filename;
    
    filename << "terrain";
    
    if(autonumber)
        filename << '-' << std::setfill('0') << std::setw(6) << guid / guid_type;
    
    filename << ((format == data_file_format::TEXT || format == data_file_format::GNUPLOT) ? ".txt" : (format == data_file_format::RAW ? ".raw" : ".bin"));
    
    size_t file_size = buffer.size();
    
    struct archive_entry* entry;
    
    entry = archive_entry_new();
    archive_entry_set_pathname(entry, filename.str().c_str());
    archive_entry_set_ctime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
    archive_entry_set_atime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
    archive_entry_set_mtime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
    archive_entry_set_size(entry, file_size);
    archive_entry_set_filetype(entry, AE_IFREG);
    archive_entry_set_perm(entry, 0644);
    archive_write_header(archive, entry);
    archive_entry_free(entry);
    
    archive_write_data(archive, buffer.data(), buffer.size());

    if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << filename << " (" << file_size / 1024 << " KB)" << std::endl;

    return true;
}

bool write_obstacles_archive(boost::shared_ptr<const std::vector<lib3dv::obstacle> > obstacles, uint32_t guid, uint8_t guid_type, archive* archive, bool autonumber, data_file_format::types format, uint8_t log_level)
{
    if(log_level == 0)
    {
        std::cout << '.';
        std::cout.flush();
    }
    
    std::string buffer;

    boost::iostreams::stream<boost::iostreams::back_insert_device<std::string> > ostream(boost::iostreams::back_inserter(buffer));
    
    switch(format)
    {
        case data_file_format::BINARY :
        {
            boost::archive::binary_oarchive archive(ostream);
            archive << *obstacles;
            break;
        }

        case data_file_format::TEXT :
        {
            boost::archive::text_oarchive archive(ostream);
            archive << *obstacles;
            break;
        }
        
        case data_file_format::GNUPLOT :
        {
            for(unsigned int o = 0; o < obstacles->size(); ++o)
            {
                uint32_t guid = (*obstacles)[o].m_guid;
                const std::vector<lib3dv::stixel>& stixels = (*obstacles)[o].m_data;
                
                for(unsigned int s = 0; s < stixels.size(); ++s)
                {
                    ostream << stixels[s].m_x << ' ' << stixels[s].m_y << ' ' << stixels[s].m_z << ' ' << guid << std::endl;
                    ostream << stixels[s].m_x << ' ' << stixels[s].m_y << ' ' << stixels[s].m_z + stixels[s].m_height << ' ' << guid << std::endl;
                    ostream << std::endl << std::endl;
                }                    
            }

            break;
        }

        case data_file_format::RAW :
        default:
        {
            for(unsigned int o = 0; o < obstacles->size(); ++o)
            {
                uint32_t guid = (*obstacles)[o].m_guid;
                uint16_t stixel_num = (*obstacles)[o].m_data.size();
                
                ostream.write(reinterpret_cast<const char*>(&guid), sizeof(guid));
                ostream.write(reinterpret_cast<const char*>(&stixel_num), sizeof(stixel_num));
                ostream.write(reinterpret_cast<const char*>((*obstacles)[o].m_data.data()), sizeof(lib3dv::stixel) * stixel_num);
            }
        }
    }
    
    std::stringstream filename;
    
    filename << "obstacles";
    
    if(autonumber)
        filename << '-' << std::setfill('0') << std::setw(6) << guid / guid_type;
    
    filename << ((format == data_file_format::TEXT || format == data_file_format::GNUPLOT) ? ".txt" : (format == data_file_format::RAW ? ".raw" : ".bin"));
    
    size_t file_size = buffer.size();
        
    struct archive_entry* entry;
    
    entry = archive_entry_new();
    archive_entry_set_pathname(entry, filename.str().c_str());
    archive_entry_set_ctime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
    archive_entry_set_atime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
    archive_entry_set_mtime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
    archive_entry_set_size(entry, file_size);
    archive_entry_set_filetype(entry, AE_IFREG);
    archive_entry_set_perm(entry, 0644);
    archive_write_header(archive, entry);
    archive_entry_free(entry);
    
    archive_write_data(archive, buffer.data(), buffer.size());

    if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << filename << " (" << file_size / 1024 << " KB)" << std::endl;

    return true;
}

bool write_motion_archive(boost::shared_ptr<const lib3dv::motion> motion, uint32_t guid, uint8_t guid_type, archive* archive, bool autonumber, data_file_format::types format, uint8_t log_level)
{
   if(log_level == 0)
    {
        std::cout << '.';
        std::cout.flush();
    }

    std::string buffer;

    boost::iostreams::stream<boost::iostreams::back_insert_device<std::string> > ostream(boost::iostreams::back_inserter(buffer));

    switch(format)
    {
        case data_file_format::BINARY :
        {
            boost::archive::binary_oarchive archive(ostream);
            archive << *motion;
            break;
        }

        case data_file_format::TEXT :
        {
            boost::archive::text_oarchive archive(ostream);
            archive << *motion;
            break;
        }

        case data_file_format::GNUPLOT :
        {
            for(unsigned int p = 0; p < motion->m_poses.size(); ++p)
            {
                const std::vector<lib3dv::pose>& poses = motion->m_poses;
                std::string type;
                
                if (poses[p].m_type == lib3dv::pose::type::CURRENT_POSE)
                    type = "CURRENT";
                else if (poses[p].m_type == lib3dv::pose::type::RELATIVE_POSE)
                    type = "RELATIVE";
                
                ostream << type << ' ' << poses[p].m_timestamp << std::endl;
                for(unsigned int i = 0; i < poses[p].m_data.size(); ++i)
                {
                    ostream << poses[p].m_data[i] << ' ';
                    if (i>0 && i%4==3) ostream << std::endl;
                }
                ostream << std::endl << std::endl;
            }
            
            break;
        }

        case data_file_format::RAW :
        default:
        {
            for(unsigned int p = 0; p < motion->m_poses.size(); ++p)
            {
                boost::posix_time::time_duration td = (*motion).m_poses[p].m_timestamp - boost::posix_time::ptime(boost::gregorian::date(1970,1,1));
                int64_t timestamp = td.total_microseconds();
                uint8_t type = (*motion).m_poses[p].m_type;
                uint16_t pose_num = (*motion).m_poses[p].m_data.size();
                
                ostream.write(reinterpret_cast<const char*>(&timestamp), sizeof(timestamp));
                ostream.write(reinterpret_cast<const char*>(&type), sizeof(type));
                ostream.write(reinterpret_cast<const char*>(&pose_num), sizeof(pose_num));
                ostream.write(reinterpret_cast<const char*>((*motion).m_poses[p].m_data.data()), sizeof(lib3dv::pose) * pose_num);
            }
        }
    }

    std::stringstream filename;

    filename << "motion";

    if(autonumber)
        filename << '-' << std::setfill('0') << std::setw(6) << guid / guid_type;

    filename << ((format == data_file_format::TEXT || format == data_file_format::GNUPLOT) ? ".txt" : (format == data_file_format::RAW ? ".raw" : ".bin"));

    size_t file_size = buffer.size();

    struct archive_entry* entry;

    entry = archive_entry_new();
    archive_entry_set_pathname(entry, filename.str().c_str());
    archive_entry_set_ctime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
    archive_entry_set_atime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
    archive_entry_set_mtime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
    archive_entry_set_size(entry, file_size);
    archive_entry_set_filetype(entry, AE_IFREG);
    archive_entry_set_perm(entry, 0644);
    archive_write_header(archive, entry);
    archive_entry_free(entry);

    archive_write_data(archive, buffer.data(), buffer.size());

    if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << filename << " (" << file_size / 1024 << " KB)" << std::endl;

    return true;
}

bool write_classification_archive(boost::shared_ptr<const lib3dv::classification> classificat, uint32_t guid, uint8_t guid_type, archive* archive, bool autonumber, data_file_format::types format, uint8_t log_level)
{
    if(log_level == 0)
     {
         std::cout << '.';
         std::cout.flush();
     }
    
     std::string buffer;
    
     boost::iostreams::stream<boost::iostreams::back_insert_device<std::string> > ostream(boost::iostreams::back_inserter(buffer));
    
     switch(format)
     {
         case data_file_format::BINARY :
         {
             boost::archive::binary_oarchive archive(ostream);
             archive << *classificat;
             break;
         }
    
         case data_file_format::TEXT :
         {
             boost::archive::text_oarchive archive(ostream);
             archive << *classificat;
             break;
         }
    
         case data_file_format::GNUPLOT :
         {
             break;
         }
    
         case data_file_format::RAW :
         default:
         {
             ostream.write(reinterpret_cast<const char*>(&classificat->m_candidates), sizeof(classificat->m_candidates));
             for(unsigned int p = 0; p < classificat->m_candidates.size(); ++p)
             {
    
                 uint32_t guid = classificat->m_candidates[p].m_guid;
    
                 ostream.write(reinterpret_cast<const char*>(&guid), sizeof(guid));
                 ostream.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_category), sizeof(classificat->m_candidates[p].m_category));
                 ostream.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_confidence), sizeof(classificat->m_candidates[p].m_confidence));
                 ostream.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_x0), sizeof(classificat->m_candidates[p].m_x0));
                 ostream.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_y0), sizeof(classificat->m_candidates[p].m_y0));
                 ostream.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_x1), sizeof(classificat->m_candidates[p].m_x1));
                 ostream.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_y1), sizeof(classificat->m_candidates[p].m_y1));
             }
    
         }
     }

    std::stringstream filename;
    
    filename << "motion";
    
    if(autonumber)
        filename << '-' << std::setfill('0') << std::setw(6) << guid / guid_type;
    
    filename << ((format == data_file_format::TEXT || format == data_file_format::GNUPLOT) ? ".txt" : (format == data_file_format::RAW ? ".raw" : ".bin"));
    
    size_t file_size = buffer.size();
    
    struct archive_entry* entry;
    
    entry = archive_entry_new();
    archive_entry_set_pathname(entry, filename.str().c_str());
    archive_entry_set_ctime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
    archive_entry_set_atime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
    archive_entry_set_mtime(entry , (boost::posix_time::second_clock::universal_time() - boost::posix_time::ptime(boost::gregorian::date(1970,1,1))).total_seconds(), 0);
    archive_entry_set_size(entry, file_size);
    archive_entry_set_filetype(entry, AE_IFREG);
    archive_entry_set_perm(entry, 0644);
    archive_write_header(archive, entry);
    archive_entry_free(entry);
    
    archive_write_data(archive, buffer.data(), buffer.size());
    
    if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << filename << " (" << file_size / 1024 << " KB)" << std::endl;
    
    return true;
}
#endif

bool write_image_file(boost::shared_ptr<const lib3dv::image> image, uint32_t guid, uint8_t guid_type ,const boost::filesystem::path& path, bool autonumber, uint8_t log_level)
{
    if(log_level == 0)
    {
        std::cout << '*';
        std::cout.flush();
    }

    bool is_mono = image->m_format == lib3dv::image::format::MONO8 ||
                  image->m_format == lib3dv::image::format::MONO16;

    bool is_rgb = image->m_format == lib3dv::image::format::RGB24 ||
                  image->m_format == lib3dv::image::format::RGB48;

    bool is_bayer = image->m_format == lib3dv::image::format::BAYER8_BGGR ||
                    image->m_format == lib3dv::image::format::BAYER8_GBRG ||
                    image->m_format == lib3dv::image::format::BAYER8_GRBG ||
                    image->m_format == lib3dv::image::format::BAYER8_RGGB ||
                    image->m_format == lib3dv::image::format::BAYER16_BGGR ||
                    image->m_format == lib3dv::image::format::BAYER16_GBRG ||
                    image->m_format == lib3dv::image::format::BAYER16_GRBG ||
                    image->m_format == lib3dv::image::format::BAYER16_RGGB;

    bool is_16bpp = (image->m_bpp / image->m_channels) == 16;

    std::stringstream filename;
    
    if(!boost::filesystem::exists(path))
    {
        std::cout << "[II] 3dv-client: writing to " << path << std::endl;
        boost::filesystem::create_directories(path);
    }

    switch(image->m_type)
    {
        case lib3dv::image::type::LEFT_RECTIFIED :
        {
            filename << "left_rectified";
            break;
        }

        case lib3dv::image::type::RIGHT_RECTIFIED :
        {
            filename << "right_rectified";
            break;
        }

        case lib3dv::image::type::LEFT_RAW :
        {
            filename << "left_raw";
            break;
        }

        case lib3dv::image::type::RIGHT_RAW:
        {
            filename << "right_raw";
            break;
        }

        case lib3dv::image::type::DSI :
        {
            filename << "dsi";
            break;
        }
    }

    boost::posix_time::ptime timestamp = image->m_timestamp;

    if(autonumber)
        filename << "-" << std::setfill('0') << std::setw(6) << guid / guid_type;
   
    boost::filesystem::path path_filename = path / filename.str();
    path_filename.make_preferred();

    if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << path_filename.string() << ((is_mono || is_bayer) ? ".pgm" : ".ppm") << " (" << image->m_buffer.size() / 1024 << " KB)" << std::endl;
    
    bool success = false;

    if(is_bayer){
        success = is_16bpp ? write_pgm(path_filename.string()+".pgm", reinterpret_cast<const uint16_t*>(image->m_buffer.data()), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint16_t>((1 << (image->m_bpp/image->m_channels)) - 1))
                           : write_pgm(path_filename.string()+".pgm", image->m_buffer.data(), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint8_t>((1 << (image->m_bpp/image->m_channels)) - 1));
    }
    else if(is_mono){
        success = is_16bpp ? write_pgm(path_filename.string()+".pgm", reinterpret_cast<const uint16_t*>(image->m_buffer.data()), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint16_t>((1 << image->m_bpp) - 1))
                           : write_pgm(path_filename.string()+".pgm", image->m_buffer.data(), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint8_t>((1 << image->m_bpp) - 1));
    }
    else if(is_rgb){
        success = is_16bpp ? write_ppm(path_filename.string()+".ppm", reinterpret_cast<const uint16_t*>(image->m_buffer.data()), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint16_t>((1 << image->m_bpp) - 1))
                           : write_ppm(path_filename.string()+".ppm", image->m_buffer.data(), image->m_width, image->m_height, to_simple_string(timestamp), static_cast<uint8_t>((1 << image->m_bpp) - 1));
    }

    if(!success)
        std::cerr << "[EE] client::write_file(): unsupported image type" << std::endl;

    return success;
}

bool write_terrain_file(boost::shared_ptr<const lib3dv::terrain> terrain, uint32_t guid, uint8_t guid_type, const boost::filesystem::path& path, bool autonumber, data_file_format::types format, uint8_t log_level)
{
    if(log_level == 0)
    {
        std::cout << '.';
        std::cout.flush();
    }

    const std::vector<lib3dv::point3>& points = terrain->m_data;
        
    if(!boost::filesystem::exists(path))
    {
        std::cout << "[II] 3dv-client: writing to " << path << std::endl;
        boost::filesystem::create_directories(path);
    }

    std::stringstream filename;
    
    filename << "terrain";
    
    if(autonumber)
        filename << '-' << std::setfill('0') << std::setw(6) << guid / guid_type;
   
    filename << ((format == data_file_format::TEXT || format == data_file_format::GNUPLOT) ? ".txt" : (format == data_file_format::RAW ? ".raw" : ".bin"));

    boost::filesystem::path path_filename = path / filename.str();
    path_filename.make_preferred();
    
    std::ofstream file(path_filename.string().c_str(), std::ios::out);

    if(file)
    {
        switch(format)
        {
            case data_file_format::BINARY :
            {
                boost::archive::binary_oarchive archive(file);
                archive << *terrain;
                break;
            }

            case data_file_format::TEXT :
            {
                boost::archive::text_oarchive archive(file);
                archive << *terrain;
                break;
            }
            
            case data_file_format::GNUPLOT :
            {
                for(unsigned int i = 0; i < points.size(); ++i)
                    file << points[i].m_x << ' ' << points[i].m_y << ' ' << points[i].m_z << std::endl;
                break;
            }

            case data_file_format::RAW :
            default:
            {
                file.write(reinterpret_cast<const char*>(points.data()), points.size() * sizeof(float));
            }
        }

        if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << path_filename.string() << " (" << file.tellp() / 1024 << " KB)" << std::endl;

        return true;
    }

    return false;
}

bool write_obstacles_file(boost::shared_ptr<const std::vector<lib3dv::obstacle> > obstacles, uint32_t guid, uint8_t guid_type, const boost::filesystem::path& path, bool autonumber, data_file_format::types format, uint8_t log_level)
{
    if(log_level == 0)
    {
        std::cout << '.';
        std::cout.flush();
    }
    
    if(!boost::filesystem::exists(path))
    {
        std::cout << "[II] 3dv-client: writing to " << path << std::endl;
        boost::filesystem::create_directories(path);
    }

    std::stringstream filename;
    
    filename << "obstacles";
    
    if(autonumber)
        filename << '-' << std::setfill('0') << std::setw(6) << guid / guid_type;
    
    filename << ((format == data_file_format::TEXT || format == data_file_format::GNUPLOT) ? ".txt" : (format == data_file_format::RAW ? ".raw" : ".bin"));

    boost::filesystem::path path_filename = path / filename.str();
    path_filename.make_preferred();
    
    std::ofstream file(path_filename.string().c_str(), std::ios::out);

    if(file)
    {
        switch(format)
        {
            case data_file_format::BINARY :
            {
                boost::archive::binary_oarchive archive(file);
                archive << *obstacles;
                break;
            }

            case data_file_format::TEXT :
            {
                boost::archive::text_oarchive archive(file);
                archive << *obstacles;
                break;
            }
            
            case data_file_format::GNUPLOT :
            {
                for(unsigned int o = 0; o < obstacles->size(); ++o)
                {
                    uint32_t guid = (*obstacles)[o].m_guid;
                    const std::vector<lib3dv::stixel>& stixels = (*obstacles)[o].m_data;
                    
                    for(unsigned int s = 0; s < stixels.size(); ++s)
                    {
                        file << stixels[s].m_x << ' ' << stixels[s].m_y << ' ' << stixels[s].m_z << ' ' << guid << std::endl;
                        file << stixels[s].m_x << ' ' << stixels[s].m_y << ' ' << stixels[s].m_z + stixels[s].m_height << ' ' << guid << std::endl;
                        file << std::endl << std::endl;
                    }                    
                }

                break;
            }

            case data_file_format::RAW :
            default:
            {
                for(unsigned int o = 0; o < obstacles->size(); ++o)
                {
                    uint32_t guid = (*obstacles)[o].m_guid;
                    uint16_t stixel_num = (*obstacles)[o].m_data.size();
                    
                    file.write(reinterpret_cast<const char*>(&guid), sizeof(guid));
                    file.write(reinterpret_cast<const char*>(&stixel_num), sizeof(stixel_num));
                    file.write(reinterpret_cast<const char*>((*obstacles)[o].m_data.data()), sizeof(lib3dv::stixel) * stixel_num);
                }
            }
        }

        if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << path_filename.string() << " (" << file.tellp() / 1024 << " KB)" << std::endl;

        return true;
    }

    return false;   
}

bool write_motion_file(boost::shared_ptr<const lib3dv::motion> motion, uint32_t guid, uint8_t guid_type, const boost::filesystem::path& path, bool autonumber, data_file_format::types format, uint8_t log_level)
{
    if(log_level == 0)
    {
        std::cout << '.';
        std::cout.flush();
    }

    if(!boost::filesystem::exists(path))
    {
        std::cout << "[II] 3dv-client: writing to " << path << std::endl;
        boost::filesystem::create_directories(path);
    }

    std::stringstream filename;

    filename << "motion";

    if(autonumber)
        filename << '-' << std::setfill('0') << std::setw(6) << guid / guid_type;

    filename << ((format == data_file_format::TEXT || format == data_file_format::GNUPLOT) ? ".txt" : (format == data_file_format::RAW ? ".raw" : ".bin"));

    boost::filesystem::path path_filename = path / filename.str();
    path_filename.make_preferred();

    std::ofstream file(path_filename.string().c_str(), std::ios::out);

    if(file)
    {
        switch(format)
        {
            case data_file_format::BINARY :
            {
                boost::archive::binary_oarchive archive(file);
                archive << *motion;
                break;
            }

            case data_file_format::TEXT :
            {
                boost::archive::text_oarchive archive(file);
                archive << *motion;
                break;
            }

            case data_file_format::GNUPLOT :
            {
                for(unsigned int p = 0; p < motion->m_poses.size(); ++p)
                {
                    const std::vector<lib3dv::pose>& poses = motion->m_poses;
                    std::string type;
                    
                    if (poses[p].m_type == lib3dv::pose::type::CURRENT_POSE)
                        type = "CURRENT";
                    else if (poses[p].m_type == lib3dv::pose::type::RELATIVE_POSE)
                        type = "RELATIVE";
                    
                    file << type << ' ' << poses[p].m_timestamp << std::endl;
                    for(unsigned int i = 0; i < poses[p].m_data.size(); ++i)
                    {
                        file << poses[p].m_data[i] << ' ';
                        if (i>0 && i%4==3) file << std::endl;
                    }
                    file << std::endl << std::endl;
                }
                
                break;
            }

            case data_file_format::RAW :
            default:
            {
                for(unsigned int p = 0; p < motion->m_poses.size(); ++p)
                {
                    
                    boost::posix_time::time_duration td = (*motion).m_poses[p].m_timestamp - boost::posix_time::ptime(boost::gregorian::date(1970,1,1));
                    int64_t timestamp = td.total_microseconds();
                    uint8_t type = motion->m_poses[p].m_type;
                    uint16_t pose_num = motion->m_poses[p].m_data.size();
                    
                    file.write(reinterpret_cast<const char*>(&timestamp), sizeof(timestamp));
                    file.write(reinterpret_cast<const char*>(&type), sizeof(type));
                    file.write(reinterpret_cast<const char*>(&pose_num), sizeof(pose_num));
                    file.write(reinterpret_cast<const char*>((*motion).m_poses[p].m_data.data()), sizeof(lib3dv::pose) * pose_num);
                }
            }
        }

        if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << path_filename.string() << " (" << file.tellp() / 1024 << " KB)" << std::endl;

        return true;
    }

    return false;
}

bool write_classification_file(boost::shared_ptr<const lib3dv::classification> classificat, uint32_t guid, uint8_t guid_type, const boost::filesystem::path& path, bool autonumber, data_file_format::types format, uint8_t log_level)
{

    //std::cout << "classification size" <<classificat->m_candidates.size()<<std::endl;
    if(log_level == 0)
    {
        std::cout << '.';
        std::cout.flush();
    }

    if(!boost::filesystem::exists(path))
    {
        std::cout << "[II] 3dv-client: writing to " << path << std::endl;
        boost::filesystem::create_directories(path);
    }

    std::stringstream filename;

    filename << "classification";

    if(autonumber)
        filename << '-' << std::setfill('0') << std::setw(6) << guid / guid_type;

    filename << ((format == data_file_format::TEXT || format == data_file_format::GNUPLOT) ? ".txt" : (format == data_file_format::RAW ? ".raw" : ".bin"));

    boost::filesystem::path path_filename = path / filename.str();
    path_filename.make_preferred();

    std::ofstream file(path_filename.string().c_str(), std::ios::out);

    if(file)
    {
        switch(format)
        {
            case data_file_format::BINARY :
            {
                boost::archive::binary_oarchive archive(file);
                archive << *classificat;
                break;
            }

            case data_file_format::TEXT :
            {
                boost::archive::text_oarchive archive(file);
                archive << *classificat;
                break;
            }

            case data_file_format::GNUPLOT :
            {
                break;
            }

            case data_file_format::RAW :
            default:
            {
                file.write(reinterpret_cast<const char*>(&classificat->m_candidates), sizeof(classificat->m_candidates));
                for(unsigned int p = 0; p < classificat->m_candidates.size(); ++p)
                {

                    uint32_t guid = classificat->m_candidates[p].m_guid;

                    file.write(reinterpret_cast<const char*>(&guid), sizeof(guid));
                    file.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_category), sizeof(classificat->m_candidates[p].m_category));
                    file.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_confidence), sizeof(classificat->m_candidates[p].m_confidence));
                    file.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_x0), sizeof(classificat->m_candidates[p].m_x0));
                    file.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_y0), sizeof(classificat->m_candidates[p].m_y0));
                    file.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_x1), sizeof(classificat->m_candidates[p].m_x1));
                    file.write(reinterpret_cast<const char*>(&classificat->m_candidates[p].m_y1), sizeof(classificat->m_candidates[p].m_y1));
                }
            }
        }

        if(log_level > 0) std::cout << std::endl << "[II] client::write_file(): saving " << path_filename.string() << " (" << file.tellp() / 1024 << " KB)" << std::endl;

        return true;
    }

    return false;
}