深视 线扫相机 获取点云数据

Qt hello - 专注于Qt的技术分享平台

最近项目上用到了深视的线扫相机，集成了三天才搞定，分享下代码。

顺便吐槽一下，想用相机取图，这么简单的功能，搞得如此麻烦。

1，文档有三份，就不能集成到一份么，维护起来也简单。

2，并且文档不更新，我看了好久，按照文档开发，结果不行，技术说文档太旧了。

3，文档里好多代码，特别多的魔数，突然出现一个数字，也不解释啥意思。

4，获取点云应该是一个常见的需求，但是接口不能直接获取，还需要自己运算，直接提供一个接口给到客户不是更好么。

一，打开相机

int Open() {
        if (StartStatus)
            return 0;
        std::regex pattern("((25[0-5]|2[0-4]\\d|1\\d{2}|[1-9]?\\d)\\.){3}(25[0-5]|2[0-4]\\d|1\\d{2}|[1-9]?\\d)");
        if (!regex_match(_Address, pattern))
        {
            std::cout << "Error _Address:" << _Address << std::endl;
            return -1;
        }

        //sdk dll 连接参数配置
        SR7IF_ETHERNET_CONFIG SREthernetConFig;

        //获取IP控件地址4个值
        std::vector<std::string> ip_list = SplitString(_Address, ".");
        for (unsigned int i = 0; i < 4; i++)
        {
            SREthernetConFig.abyIpAddress[i] = std::atoi(ip_list[i].c_str());
        }

        //连接相机 
        int reT = SR7IF_EthernetOpen(SR_DEVICEID, &SREthernetConFig);
        if (reT < 0)   //失败
        {
            std::cout << "connect sr error: " << reT << std::endl;
            return -1;
        }
        else {
            std::cout << "connect sr ok " << reT << std::endl;
        }

        //批处理输出 轮廓+亮度
        int pData = 0;
        SR7IF_SetSetting(SR_DEVICEID, 0x02, 0x10, 0x00, 0x21, 0, &pData, 1);

        //循环 关
        int cycle = 0;
        SR7IF_SetSetting(SR_DEVICEID, 0x02, 0x10, 0x00, 0x10, 0, &cycle, 1);

        StartStatus = true;
        return 0;
    }

二，获取数据

我这里使用阻塞方式获取数据。他们还提供了非阻塞也就是回调方式获取数据，但是我的代码在线程同步的时候经常崩溃，没找到原因。还有一种是无线循环获取数据，我这里也没跑通，出来的数据不对。

int interface_sr::getData() {
        //获取参数
        std::vector<std::string> listParameter =  SplitString(privateParameter,":");
        if (listParameter.size() == 2) {
            if (listParameter.at(0) == "BatchPoints") {
                int BatchPoints = std::stoi(listParameter.at(1));
                std::cout << "set BatchPoints " << BatchPoints << std::endl;
                //批处理点数
                SR7IF_SetSetting(SR_DEVICEID, 0x02, 0x10, 0x00, 0x0a, 0, &BatchPoints, 2);
            }
        }

        //开始批处理
        SR7IF_StartMeasure(SR_DEVICEID, 50000);
        
        SR7IF_Data DataObject = NULL;
       int res= SR7IF_ReceiveData(SR_DEVICEID, DataObject);
       if (res != 0) {
           std::cout << "SR7IF_ReceiveData error:" << res << std::endl;
           return -1;
       }

        int BatchPoint = SR7IF_ProfilePointSetCount(SR_DEVICEID, DataObject);
        int DataWidth = SR7IF_ProfileDataWidth(SR_DEVICEID, DataObject);


        //高度数据
        int* heightData = new int[BatchPoint * DataWidth];
        res = SR7IF_GetProfileData(SR_DEVICEID, DataObject, heightData);
        if (res != 0) {
            std::cout << "SR7IF_GetProfileData error:" << res << std::endl;
            return -1;
        }

        //灰度数据
        unsigned char* grayData = new unsigned char[BatchPoint * DataWidth];
        res = SR7IF_GetIntensityData(SR_DEVICEID, DataObject, grayData);
        if (res != 0) {
            std::cout << "SR7IF_GetIntensityData error:" << res << std::endl;
            return -1;
        }

        double xpitch = SR7IF_ProfileData_XPitch(SR_DEVICEID, DataObject);
        double ypitch = xpitch;
        int width = DataWidth;
        int height = BatchPoint;

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud <pcl::PointXYZ>);
        std::vector<cv::Point3f> points;
        for (int i = 0; i < width * height; i++) {
            double x, y, z;
            x = i % width * xpitch;     //x 坐标
            y = i / width * ypitch;     //y 坐标
            z = (double)(heightData[i] / 100000.0 ); //z  转成mm为单位
            points.push_back(cv::Point3f(x, y, z));

            pcl::PointXYZ point(x, y, z);
            cloud->points.push_back(point);
        }
        //保存点云
        pcl::io::savePLYFileBinary("point_cloud.ply", *cloud);

        //点云
        cv::Mat tempCloudImage = cv::Mat(cv::Size(width, height), CV_32FC3, points.data());

        //深度
        cv::Mat tempDepthImage = cv::Mat(cv::Size(width, height), CV_32FC1, cv::Scalar(0));
        for (int i = 0; i < height; ++i) {
            for (int j = 0; j < width; ++j) {
                int index = i * width + j;
                tempDepthImage.at<float>(i, j) = (double)(heightData[index] / 100000.0 );
            }
        }

        //灰度图
        cv::Mat tempGrayImage = cv::Mat(cv::Size(DataWidth, BatchPoint), CV_8UC1, const_cast<unsigned char*>(grayData));

        SR7IF_StopMeasure(SR_DEVICEID);
        delete[] heightData;
        delete[] grayData;

        return 0;
    }

Qt hello - 专注于Qt的技术分享平台