LidarMeasure/test/locate_sample.cpp

//
// Created by zx on 2019/12/30.
//
/*
 * locater 模块测试例子
 */
#include <fcntl.h>
#include "../locate/locate_task.h"
#include "../locate/locater.h"
#include <glog/logging.h>
#include <google/protobuf/io/coded_stream.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>
using google::protobuf::io::FileInputStream;
using google::protobuf::io::FileOutputStream;
using google::protobuf::io::ZeroCopyInputStream;
using google::protobuf::io::CodedInputStream;
using google::protobuf::io::ZeroCopyOutputStream;
using google::protobuf::io::CodedOutputStream;
using google::protobuf::Message;

bool read_proto_param(std::string path, ::google::protobuf::Message& param)
{
    int fd = open(path.c_str(), O_RDONLY);
    if (fd == -1) return false;
    FileInputStream* input = new FileInputStream(fd);
    bool success = google::protobuf::TextFormat::Parse(input, &param);
    delete input;
    close(fd);
    return success;
}
bool string2point(std::string str,pcl::PointXYZ& point)
{
    std::istringstream iss;
    iss.str(str);
    float value;
    float data[3]={0};
    for(int i=0;i<3;++i)
    {
        if(iss>>value)
        {
            data[i]=value;
        }
        else
        {
            return false;
        }
    }
    point.x=data[0];
    point.y=data[1];
    point.z=data[2];
    return true;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr ReadTxtCloud(std::string file)
{
    std::ifstream fin(file.c_str());
    const int line_length=64;
    char str[line_length]={0};
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    while(fin.getline(str,line_length))
    {
        pcl::PointXYZ point;
        if(string2point(str,point))
        {
            cloud->push_back(point);
        }
    }
    return cloud;
}

int main(int argc,char* argv[])
{
    std::string input_file="20191217-215217.txt";
    std::string out_path="./test";
    if(argc>=2)
    {
        input_file=argv[1];
        input_file+=".txt";
    }
    if(argc>=3)
    {
        out_path=argv[2];
    }
    Error_manager  code;
    Locater* locater=new Locater();
    Measure::LocateParameter parameter;
    if(false==read_proto_param("./setting/locate.prototxt",parameter))
    {
        std::cout<<" read parameter failed..."<<std::endl;
    }
    code=locater->init(parameter);
    if(code!=SUCCESS)
    {
        LOG(ERROR)<<code.to_string();
    }
    Locate_task* task=new Locate_task();

    std::string cloud_path="/home/zx/data/samples/src_txt/balck_suv/";
    cloud_path+=input_file;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;

    cloud=ReadTxtCloud(cloud_path);
    std::cout<<"  input file: "<<cloud_path<<std::endl;
    code=task->set_locate_cloud(cloud);
    if(code!=SUCCESS)
    {
        LOG(ERROR)<<code.to_string();
        return 0;
    }
    task->set_save_path(out_path);
    code=locater->execute_task(task,5);
    if(code==SUCCESS)
    {
        LOG(INFO)<<" LOCATE SUCCESS";
    }
    else
    {
        LOG(ERROR)<<code.to_string();
    }
    return 0;
}