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 <pcl/filters/passthrough.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;
#include <sys/stat.h>
#include <dirent.h>

void init_glog()
{
    FLAGS_max_log_size = 100;
    FLAGS_logbufsecs = 0;
    google::InitGoogleLogging("LidarMeasurement_test");
    google::SetStderrLogging(google::INFO);
    google::InstallFailureSignalHandler();
    FLAGS_colorlogtostderr = true;        // Set log color
    FLAGS_logbufsecs = 0;                // Set log output speed(s)
    FLAGS_max_log_size = 1024;            // Set max log file size(GB)
    FLAGS_stop_logging_if_full_disk = true;
}
void list_dir( const char * dir_name,std::vector<std::string>& files)
{
    if( 0 == dir_name )
    {
        std::cout<<" dir_name is null ! "<<std::endl;
        return;
    }

    struct stat s;
    lstat( dir_name , &s );
    if( ! S_ISDIR( s.st_mode ) )
    {
        return;
    }
    struct dirent * filename;
    DIR * dir;
    dir = opendir( dir_name );
    if( NULL == dir )
    {
        return;
    }

    int iName=0;
    while( ( filename = readdir(dir) ) != NULL )
    {
        if( strcmp( filename->d_name , "." ) == 0 ||
            strcmp( filename->d_name , "..") == 0)
            continue;

        char wholePath[128] = {0};
        sprintf(wholePath, "%s", filename->d_name);
        files.push_back(wholePath);
    }
}

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

void ReadTxtCloud(std::string file, pcl::PointCloud<pcl::PointXYZ> &cloud)
{
    cloud.clear();
    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[])
{
    init_glog();
    std::string input_file="cloud";
    std::string out_path="./test";
    if(argc>=2)
    {
        input_file=argv[1];
    }
    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();
    }
    

    std::string cloud_path="/home/zx/zzw/code/LidarMeasure/build/";
    cloud_path+=input_file;
    std::vector<std::string> files;
    list_dir(cloud_path.c_str(),files);
    int count=files.size();
    int correct_size = 0;
    while (1)
    {
        int count = 20;
        while(count-->0){

            for (int i = 0; i < files.size(); ++i)
            {

                pcl::PointCloud<pcl::PointXYZ> cloud;
                std::string t_file = cloud_path + "/" + files[i];
                ReadTxtCloud(t_file, cloud);
                std::cout << "  input file: " << t_file << std::endl;

                //第二步,根据区域筛选点云
                pcl::PointCloud<pcl::PointXYZ>::Ptr locate_cloud(new pcl::PointCloud<pcl::PointXYZ>);
                pcl::PassThrough<pcl::PointXYZ> passX;
                pcl::PassThrough<pcl::PointXYZ> passY;
                pcl::PassThrough<pcl::PointXYZ> passZ;
                passX.setInputCloud(cloud.makeShared());
                passX.setFilterFieldName("x");
                passX.setFilterLimits(-100000, 100000);
                passX.filter(*locate_cloud);

                passY.setInputCloud(locate_cloud);
                passY.setFilterFieldName("y");
                passY.setFilterLimits(-100000, 100000);
                passY.filter(*locate_cloud);

                passY.setInputCloud(locate_cloud);
                passY.setFilterFieldName("z");
                passY.setFilterLimits(-100000, 100000);
                passY.filter(*locate_cloud);
                LOG(INFO) << "筛选点云点数 : " << locate_cloud->size();

                Locate_task* task=new Locate_task();
                code = task->set_locate_cloud(locate_cloud);
                if (code != SUCCESS)
                {
                    LOG(ERROR) << code.to_string();
                    return 0;
                }
                task->set_save_path(out_path);
                code = locater->execute_task(task, 5);
				Locate_information dj_locate_information = task->get_locate_information();
                if (code == SUCCESS)
                {
                    LOG(INFO) << " LOCATE SUCCESS";
                    correct_size++;
                }
                else
                {
                    LOG(ERROR) << code.to_string();
                    //移动文件
                    usleep(300);
                    std::string src = "./test/cnn3d.txt";
                    std::string dst = "./error/";
                    dst += files[i];
                    std::string name = "mv " + src + " " + dst;
                    const char *des_name = name.c_str();

                    system(des_name); //调用系统命令
                    usleep(200);
                }
                delete task;
            }

        }
        getchar();
    }
    if(count>0)
    {
        std::cout<<std::endl<<"  acc : "<<correct_size<<"/"<<count<<
        "    percent:"<<float(correct_size)/float(count)<<std::endl;
    }
    return 0;
}