compress.cpp

#include <iostream>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include "dispatch/dispatch.h"
#include "serialize/blockserializer.h"
#include "locate_system/localize_utils/sys_utils.h"

using namespace std;
using namespace boost::filesystem;

DEFINE_string(pcd_dir, "", "pcd_dir");
DEFINE_string(base_dir, "", "base_dir");

void signaleHandle(const char* data, int size){
    string str = data;
    LOG(INFO) << "signaleHandle: " << str;
}
Vector3d getCenter(const string &center_dir){
    Vector3d center;
    std::ifstream ifs_center(center_dir);
    if(!ifs_center) {
        LOG(WARNING) << "open centerPath_ file error: " << center_dir;
    }else{
        string line;
        getline(ifs_center, line);
        {
            double lat, lon, h;
            istringstream iss(line);
            string subline;
            getline(iss, subline, ',');
            istringstream(subline) >> lat;
            getline(iss, subline, ',');
            istringstream(subline) >> lon;
            getline(iss, subline, ',');
            istringstream(subline) >> h;
            center = {lat, lon, h};
            LOG(INFO) << setprecision(15) << "get center: " << center.transpose();
        }
        ifs_center.close();
    }
    return center;
}

int main(int argc, char *argv[])
{
    gflags::ParseCommandLineFlags(&argc, &argv, true);
    google::InitGoogleLogging(argv[0]);
    google::InstallFailureSignalHandler();
    google::InstallFailureWriter(&signaleHandle);
    FLAGS_log_dir = FLAGS_base_dir + "/log/";
    FLAGS_alsologtostderr = true;
    FLAGS_colorlogtostderr = true;
    FLAGS_stderrthreshold = 0;
    FLAGS_logbufsecs = 0;

//    auto R = get_rotation(31.27532958984375, 121.65985107421875, 0,
//                              31.26983642578125, 121.65435791015625, 0);
//    LOG(INFO) << R;
//    Vector3d blh(31.2682286704459, 121.641227908251, 0);
//    Dispatch dispatch(FLAGS_pcd_dir);
//    dispatch.dispatch();
//    LOG(INFO) << "dispatch done";
//    boost::shared_ptr<GridManager> blockManager =
//            dispatch.getBlockManager();
//    BlockSerializer blockSerializer(blockManager);
//    blockSerializer.setMeshId(dispatch.getMeshId());
//    blockSerializer.setOffset(dispatch.getOffset());
//    blockSerializer.setBLH(blh);
//    blockSerializer.setOutputPath("/home/juefx/temp.stream");
//    blockSerializer.configSerialization();

    LOG(INFO) << "FLAGS_base_dir: " << FLAGS_base_dir;
    auto whole_start = system_clock::now();

    string outputDir = FLAGS_base_dir + "/stream/";
    if(!is_directory(outputDir)){
        create_directory(outputDir);
    }
    // string pcdDir = FLAGS_base_dir + "/pcd_cart/";
    string pcdDir = FLAGS_base_dir;
    if(!is_directory(pcdDir)){
        return -1;
    }
    string center_dir = FLAGS_base_dir + "/meshes.center";
    Vector3d center = getCenter(center_dir);
    directory_iterator pcd_iter(pcdDir);
    directory_iterator pcd_end_iter;
    map<string, vector<string>> meshPcdPathsMap;
    for(; pcd_iter != pcd_end_iter; pcd_iter++) {
        if(pcd_iter->path().extension().string() != ".pcd"){
            continue;
        }
        string pcd_path = pcd_iter->path().string();
        string pcd_name = pcd_iter->path().stem().string();
        string meshId;
        if(pcd_name.find("_") != string::npos){
            vector<string> pcd_name_parts;
            boost::split(pcd_name_parts, pcd_name, boost::is_any_of("_"), boost::token_compress_on);
            meshId = pcd_name_parts[0];
        }else{
            meshId = pcd_name;
        }
        auto iter = meshPcdPathsMap.find(meshId);
        if(iter == meshPcdPathsMap.end()){
            meshPcdPathsMap.insert(make_pair(meshId, vector<string>{pcd_path}));
        }else{
            iter->second.push_back(pcd_path);
        }
    }
    for(auto iter = meshPcdPathsMap.begin();
        iter != meshPcdPathsMap.end();
        iter++){
        // ulong meshId = stoi(iter->first);
        LOG(INFO) << iter->first;
        auto start = system_clock::now();
        vector<string> pcd_paths = iter->second;
        PointCloudExport::Ptr pointcloud(new PointCloudExport);
        for(string pcd_path : pcd_paths){
            LOG(INFO) << pcd_path;
            PointCloudExport pc;
            pcl::io::loadPCDFile(pcd_path, pc);
            *pointcloud += pc;
        }
        {
            rusage usage;
            getrusage(RUSAGE_SELF, &usage);
            LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB";
            print_mem(getpid());
        }
        string slamstart_dir = FLAGS_base_dir + "/slam.start";
        std::ifstream ifs_slamstart(slamstart_dir);
        if(!ifs_slamstart){
            LOG(WARNING) << "open /slam/slam.start error!" << endl;
        }
        else{
            string line;
            Vector3d center_to_slam_start_offset_;
            getline(ifs_slamstart, line);
            {
                double x, y, z;
                istringstream iss(line);
                string subline;
                getline(iss, subline, ',');
                istringstream(subline) >> x;
                getline(iss, subline, ',');
                istringstream(subline) >> y;
                getline(iss, subline, ',');
                istringstream(subline) >> z;
                center_to_slam_start_offset_ = {x, y, z};

            }
            ifs_slamstart.close();
            for(auto &point : pointcloud->points){
                point.x -= center_to_slam_start_offset_.x();
                point.y -= center_to_slam_start_offset_.y();
                point.z -= center_to_slam_start_offset_.z();
            }
            pcl::io::savePCDFileBinary(FLAGS_base_dir + "/local.pcd", *pointcloud);
        }
        boost::shared_ptr<Dispatch> dispatch(new Dispatch(pointcloud));
        // dispatch.setMeshId(meshId);
        dispatch->setBLH(center);
        dispatch->configOffset();
        dispatch->dispatch();
        LOG(INFO) << "dispatch done";
        boost::shared_ptr<GridLayer> meshLayer =
                dispatch->getMeshLayer();

        BlockSerializer blockSerializer(meshLayer);
        blockSerializer.setMeshId(iter->first);
        blockSerializer.setOffset(dispatch->getOffset());
        blockSerializer.setBLH(center);
        blockSerializer.setOutputPath(
                    outputDir + iter->first + ".stream");

        dispatch.reset();
        pointcloud.reset();

        {
            rusage usage;
            getrusage(RUSAGE_SELF, &usage);
            LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB";
            print_mem(getpid());
        }

        blockSerializer.configSerialization();

        {
            rusage usage;
            getrusage(RUSAGE_SELF, &usage);
            LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB";
            print_mem(getpid());
        }

        auto end = system_clock::now();
        auto duration = duration_cast<milliseconds>(end - start);
        LOG(INFO) << iter->first << " load take "
                  << double(duration.count()) / 1000 << " seconds";
    }
    auto whole_end = system_clock::now();
    auto whole_duration = duration_cast<milliseconds>(whole_end - whole_start);
    LOG(INFO) << "whole load take "
              << double(whole_duration.count()) / 1000 << " seconds";
    return 0;
}