Tags: SLAM

本篇博客是慕课《ROS机器人操作系统入门》的第六篇笔记,慕课网址是这里。本篇笔记主要记录Parameter Server的使用及需要注意的问题。

1.Parameter Server基本知识

在我们启动roscore的时候,ROS除了会自动启动Master,还会自动帮我们启动Parameter Serverrosout。相比于之前的Topic和Service,它更加的“静态”。它维护着一个字典(多个键值对),用于存放各种参数。这点来说有点类似于C++里类的静态数据成员变量,可以实现在同类的对象中共享参数的功能。同时若一个对象修改了值,其它对象调用时也会同步更新,“一改全改”。

参数服务器可以使用命令行、launch文件以及ROS提供的API进行读写。在之前的第一篇笔记中就初步学习了参数服务器的相关知识以及第一种用法,点击回顾。本篇博客则主要从代码的角度介绍。

2.需求与描述

这里利用参数服务器实现之前在这篇博客实现过的功能:利用OpenCV对相机进行标定,获得内参以及畸变参数,然后再将获得的参数用于影像的校正。

我们创建一个包名称叫camera_calibration,其中包含两个节点(Node):calibrationundistortioncalibration负责读取一系列的相片,根据相片进行相机的内参以及畸变参数标定,并将结果保存到calib_res.yaml文件中。undistortion负责读取这些参数,并基于它们对相片进行去畸变。

3.Parameter Server的C++实现

(1)创建Package

打开RoboWare,在”资源管理器”中找到src文件夹,右键单击选择”新建ROS包”,输入camera_calibration即可创建Package。创建完成后右键Package选择”编辑依赖的ROS包列表”,输入roscpp即可添加依赖。

(2)创建calibration节点

在RoboWare的资源管理器中找到camera_calibration,右键选择”新建Src文件夹”,在src文件夹上右键,然后选择”新建CPP源文件”,输入名称为calibration.cpp并回车,然后在弹出的对话框中选择”加入到新的可执行文件中”即可。

需要注意的是由于涉及到图片,因此还需要用到OpenCV库。所以我们还需要打开CMakeLists.txt文件手动添加依赖,修改好的内容如下。

# 新增寻找OpenCV
find_package(OpenCV 3.4.6 REQUIRED)
# 新增添加include目录
include_directories(${OpenCV_DIRS})

add_executable(calibration
  src/calibration.cpp
)
add_dependencies(calibration ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(calibration
  ${catkin_LIBRARIES}
  # 新增link opencv的库
  ${OpenCV_LIBS}
)

这里需要十分注意的是在安装ROS的时候自动也装了一个OpenCV,但这个OpenCV并不一定好用。所以你需要用你之前自己装的OpenCV。要想使用自己的OpenCV就要在CMakeLists.txt中精确指定OpenCV版本(ROS装的OpenCV和自己的OpenCV版本号不一样的情况下),可以进行区分了。可以在终端中使用如下命令查看OpenCV版本:

pkg-config opencv --modversion

我电脑上自己装的是3.4.6版本,所以这里就填它了。

(3)编写calibration节点代码

在C++中ROS提供了:

三种获取参数的方法:ros::param::get()ros::NodeHandle::getParam()ros::NodeHandle::param()

两种查找参数的方法:ros::param::search()ros::NodeHandle::searchParam()

两种设置参数的方法:ros::param::set()ros::NodeHandle::setParam()

两种检查参数的方法:ros::param::has()ros::NodeHandle::hasParam()

两种删除参数的方法:ros::param::del()ros::NodeHandle::deleteParam()

这些API都非常简单,很好理解,这里就不一一演示了。结合具体需求代码如下:

#include<ros/ros.h>
#include<opencv2/opencv.hpp>

// 实现参考https://blog.csdn.net/u011574296/article/details/73823569,写得很详细
void calibrateCamera(std::vector<std::string> img_paths, cv::Size boardSize, cv::Size gridSize,
                     cv::Mat &cameraMatrix, cv::Mat &distCoeffs) {
    // 迭代算法的终止准则
    // 第一个参数是类型,CV_TERMCRIT_ITER和CV_TERMCRIT_EPS之一或二者的组合
    // 第二个参数是最大迭代次数
    // 第三个参数是结果的精确性
    cv::TermCriteria criteria = cv::TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 30, 0.001);

    // 每张图片里角点的像素坐标与真实坐标
    std::vector<cv::Point3f> pts;
    std::vector<cv::Point2f> corners;

    // 由每张图片的像素坐标和角点坐标构成的vector
    std::vector<std::vector<cv::Point3f> > objpoints;
    std::vector<std::vector<cv::Point2f> > imgpoints;
    // 构造真实坐标,z分量为0
    for (int i = 0; i < boardSize.height; i++) {
        for (int j = 0; j < boardSize.width; j++) {
            cv::Point3f pt;
            pt.x = i * gridSize.width;
            pt.y = j * gridSize.height;
            pt.z = 0;
            pts.push_back(pt);
        }
    }

    // 依次加载图片,并识别棋盘格
    for (int k = 0; k < img_paths.size(); ++k) {
        cv::Mat tmp_img = cv::imread(img_paths[k]);
        cv::Mat gray;
        cv::cvtColor(tmp_img, gray, cv::COLOR_BGR2GRAY);
        bool ret = cv::findChessboardCorners(gray, boardSize, corners);
        if (ret) {
            objpoints.push_back(pts);
            cv::cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1), criteria);
            imgpoints.push_back(corners);
            cv::drawChessboardCorners(tmp_img, boardSize, corners, ret);
            std::cout << "Load " << k + 1 << "/" << img_paths.size() << std::endl;
            cv::imshow("board", tmp_img);
            cv::waitKey(500);
        }
    }

    cv::Size img_size;
    img_size.height = cv::imread(img_paths[0]).size().height;
    img_size.width = cv::imread(img_paths[0]).size().width;
    std::vector<cv::Mat> rvecsMat, tvecsMat;    // 用于接收不同图片相对于第一张图片的R、t
    // 最后一个参数表示畸变系数只求到k3,完整的畸变参数输出顺序如下
    // (k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])
    cv::calibrateCamera(objpoints,
                        imgpoints,
                        img_size,
                        cameraMatrix,
                        distCoeffs,
                        rvecsMat,
                        tvecsMat,
                        CV_CALIB_FIX_K3);
}

// 针对本场景,按行优先存储,将Mat转换成vector形式,从而让ROS可以读取
std::vector<float> cvtMat2Vec(cv::Mat mat){
    std::vector<float> vec;
    for(int i=0;i<mat.size().height;i++){
        for(int j=0;j<mat.size().width;j++){
            vec.push_back(mat.at<double>(i,j));
        }
    }
    return vec;
}

int main(int argc, char *argv[])
{   
    // 指定图片路径
    std::string dir_path = "/root/imgs/";
    // 构造图片路径,这里只加载9张图片
    std::vector<std::string> paths;
    for (int i = 1; i < 10; ++i) {
        char tmp_path[100];
        sprintf(tmp_path, (dir_path + "img%02d.jpg").c_str(), i);
        paths.push_back(tmp_path);
    }
    cv::Size boardSize = cv::Size(8, 6);    // 棋盘格大小
    cv::Size gridSize = cv::Size(30, 30);   // 棋盘格每一格的长度,单位mm
    cv::Mat camera_mat, dist;               // 用于接收相机内参与畸变参数
    // 校正相机
    calibrateCamera(paths, boardSize, gridSize, camera_mat, dist);
    std::cout<<"Calibration finished."<<std::endl;

    // 将Mat类型的数据转换为vector类型
    std::vector<float> cam_mat;
    cam_mat = cvtMat2Vec(camera_mat);
    std::vector<float> dist_mat;
    dist_mat = cvtMat2Vec(dist);
    std::cout<<"Data conversion finished."<<std::endl;

    // 初始化节点并指定名称
    ros::init(argc, argv, "calibration");
    // 设置参数
    ros::param::set("cam_mat",cam_mat);
    ros::param::set("dist_mat",dist_mat);
    std::cout<<"Parameter set finished."<<std::endl;
    
    // 保存参数也有多种方法
    // 1.调用OpenCV的FileStorage库保存参数
    cv::FileStorage fsw = cv::FileStorage(dir_path + "calib_res.yaml", cv::FileStorage::WRITE);
    fsw.write("cam_mat", camera_mat);
    fsw.write("dist_mat", dist);
    fsw.release();

    // 2.调用ROS的dump命令将所有参数保存到指定文件中
    system(("rosparam dump "+ dir_path + "all_params.yaml").c_str());
    std::cout<<"Parameter save finished."<<std::endl;
    return 0;
}
(4)创建undistortion节点

在RoboWare的资源管理器中找到src文件夹,点击右键,然后选择”新建CPP源文件”,输入名称为undistortion.cpp并回车,然后在弹出的对话框中选择”加入到新的可执行文件中”即可。

同理,由于包含了对OpenCV的引用,在CMakeLists.txtundistortion里的target_link_libraries中要添加OpenCV库(${OpenCV_LIBS})。

(5)编写undistortion节点代码

在这里主要是读取之前的参数。读取参数可以在launch文件里加载,也可以在代码里加载。代码如下:

#include<ros/ros.h>
#include<opencv2/opencv.hpp>

// 校正影像去畸变
void undistortImgs(std::vector<std::string> img_paths, cv::Mat cameraMatrix, cv::Mat distCoeffs) {
    std::vector<cv::Mat> inputImgs;
    for (int i = 0; i < img_paths.size(); ++i) {
        cv::Mat img = cv::imread(img_paths[i]);
        cv::Mat dst;
        cv::undistort(img, dst, cameraMatrix, distCoeffs);
        cv::Mat compare(img.size().width, img.size().height, CV_8UC3);
        cv::hconcat(img, dst, compare);
        cv::imwrite(img_paths[i] + "_undistort.jpg", compare);
        std::cout << "Undistorting.." << i + 1 << "/" << img_paths.size() << std::endl;
    }
}

// 针对本场景,将vector转换成Mat,从而让OpenCV可读取
cv::Mat cvtVec2Mat(std::vector<float> vec, int mat_height, int mat_width) {
    cv::Mat restore_mat(mat_height, mat_width,CV_32F);
    for (int i = 0; i < mat_height; i++) {
        for (int j = 0; j < mat_width; j++) {
            int index = i * mat_width + j;
            float val = vec[index];
            restore_mat.at<float>(i, j) = val;
        }
    }
    return restore_mat;
}


int main(int argc, char *argv[])
{
    // 指定图片路径
    std::string dir_path = "/root/imgs/";
    // 构造图片路径,这里只加载9张图片
    std::vector<std::string> paths;
    for (int i = 1; i < 10; ++i) {
        char tmp_path[100];
        sprintf(tmp_path, (dir_path + "img%02d.jpg").c_str(), i);
        paths.push_back(tmp_path);
    }
    
    // 加载参数也有多种方式
    // 1.通过OpenCV的FileStorage加载
    cv::FileStorage fsr = cv::FileStorage(dir_path + "calib_res.yaml", cv::FileStorage::READ);
    cv::Mat cv_camera_mat = fsr["cam_mat"].mat();
    cv::Mat cv_dist = fsr["dist_mat"].mat();

    // 2.通过ROS的load加载
    // 初始化节点并指定节点名称
    ros::init(argc, argv, "undistortion");
    // 先将所有参数载入
    system(("rosparam load "+ dir_path + "all_params.yaml").c_str());
    
    // 找到对应数据,注意get函数的用法,数据通过函数的引用返回
    std::vector<float> cam_vec;
    ros::param::get("cam_mat",cam_vec);
    std::vector<float> dist_vec;
    ros::param::get("dist_mat",dist_vec);
    
    // 将数据转换成Mat类型
    cv::Mat camera_mat, dist;
    camera_mat = cvtVec2Mat(cam_vec,3,3);
    dist = cvtVec2Mat(dist_vec,1,5);
    std::cout<<"Parameter load finished."<<std::endl;
    
    // 校正图片
    undistortImgs(paths,camera_mat,dist);
    std::cout<<"Undistortion finished."<<std::endl;
    
    return 0;
}
(6)编译运行测试

最后在Catkin工作空间根目录下打开终端,使用catkin_make进行编译。然后先输入roscore打开Master,然后在新终端中输入rosrun camera_calibration calibration启动calibration节点,新终端中输入rosrun camera_calibration undistortion启动undistortion节点。如下: 可以看到成功运行了刚刚写的两个节点,并且实现了参数的发送与接收。打开/root/imgs文件夹可以看到多了两个yaml文件以及校正后的图片,如下: 校正前后的对比图片如下: ROS保存的参数文件如下: 最后简单说一下,生成的两个节点的可执行文件放在了/root/catkin_ws/devel/lib/camera_calibration里,直接在终端中运行这个可执行文件也是可以的(Master已经运行的情况下)。

4.Parameter Server的Python实现

在Python中也有和C++类似的API,基本相同,这里就不再意义列举了。

(1)创建Package

打开RoboWare,在”资源管理器”中找到src文件夹,右键单击选择”新建ROS包”,输入camera_calibration即可创建Package。创建完成后右键Package选择”编辑依赖的ROS包列表”,输入rospy即可添加依赖。

(2)创建calibration节点文件

根据之前的习惯,src文件夹里一般放C/C++文件,Python代码一般放在scripts文件夹里。在包名上右键点击,选择”新建文件夹”,输入名称为scripts,在scripts文件夹上右键选择”新建文件”,输入calibration.py即可。

(3)编写calibration代码

Python代码如下:

# coding=utf-8
import rospy
import cv2
import numpy as np
import os


def calibrateCamera(img_paths, row, col):
    ROWS = row
    COLOMONS = col

    # termination criteria
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

    # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
    objp = np.zeros((ROWS * COLOMONS, 3), np.float32)
    objp[:, :2] = np.mgrid[0:ROWS, 0:COLOMONS].T.reshape(-1, 2)

    # Arrays to store object points and image points from all the images.
    objpoints = []  # 3d point in real world space
    imgpoints = []  # 2d points in image planeself

    for fname in img_paths:
        img = cv2.imread(fname)
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        # Find the chess board corners
        ret, corners = cv2.findChessboardCorners(gray, (ROWS, COLOMONS), None)

        # If found, add object points, image points (after refining them)
        if ret == True:
            objpoints.append(objp)

            corners2 = cv2.cornerSubPix(
                gray, corners, (11, 11), (-1, -1), criteria)
            imgpoints.append(corners2)

            # Draw and display the corners
            img = cv2.drawChessboardCorners(
                img, (ROWS, COLOMONS), corners2, ret)
            cv2.imshow('img', img)
            cv2.waitKey(500)

    cv2.destroyAllWindows()
    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
        objpoints, imgpoints, gray.shape[::-1], None, None)
    return mtx, dist


# 针对本场景,按行优先存储,将NDArray转换成list形式,从而让ROS可以读取
def cvtNDArray2List(mat):
    res_list = []
    for i in range(mat.shape[0]):
        for j in range(mat.shape[1]):
            res_list.append(float(mat[i, j]))
            print mat[i, j]
    return res_list


if __name__ == "__main__":
    # 指定图片路径
    dir_path = "/root/imgs/"
    # 构造图片路径,这里只加载9张图片
    paths = []
    for i in range(1, 10):
        tmp_path = dir_path+"img"+i.__str__().zfill(2)+".jpg"
        paths.append(tmp_path)
    # 校正相机
    camera_mat, dist = calibrateCamera(paths, 8, 6)
    print "Calibration finished."

    # 将NDArray类型的数据转换为list类型
    cam_mat = cvtNDArray2List(camera_mat)
    dist_mat = cvtNDArray2List(dist)

    # 初始化节点并指定名称
    rospy.init_node("calibration")
    # 设置参数
    rospy.set_param("cam_mat", cam_mat)
    rospy.set_param("dist_mat", dist_mat)
    print "Parameter set finished."

    # 保存参数也有多种方法
    # 1.调用OpenCV的FileStorage库保存参数
    fs = cv2.FileStorage(dir_path + "calib_res.yaml", cv2.FILE_STORAGE_WRITE)
    fs.write("cam_mat", camera_mat)
    fs.write("dist_mat", dist)
    fs.release()

    # 2.调用ROS的dump命令将所有参数保存到指定文件中
    os.system("rosparam dump " + dir_path + "all_params.yaml")
    print "Parameter save finished."
(4)创建undistortion节点文件

scripts文件夹上右键选择”新建文件”,输入undistortion.py即可。

(5)编写undistortion代码

代码如下:

# coding=utf-8
import rospy
import cv2
import numpy as np
import os


# 校正影像去畸变
def undistortImgs(img_paths, cameraMatrix, distCoeffs):
    for i in range(img_paths.__len__()):
        img = cv2.imread(img_paths[i])
        h, w = img.shape[:2]
        newcameramtx, roi = cv2.getOptimalNewCameraMatrix(
            cameraMatrix, distCoeffs, (w, h), 1, (w, h))
        # undistort
        dst = cv2.undistort(img, cameraMatrix, dist, None, newcameramtx)

        # crop the image
        x, y, w, h = roi
        dst = dst[y:y + h, x:x + w]

        cv2.imwrite(img_paths[i]+"_undistort.jpg", dst)
        print "Undistorting...", (i + 1).__str__() + \
            "/" + img_paths.__len__().__str__()

# 针对本场景,将list转换成NDArray,从而让OpenCV可读取


def cvtList2NDArray(vec, mat_height, mat_width):
    restore_mat = np.ndarray([mat_height, mat_width], float)
    for i in range(mat_height):
        for j in range(mat_width):
            index = i * mat_width+j
            val = vec[index]
            restore_mat[i, j] = val
    return restore_mat


if __name__ == "__main__":
    # 指定图片路径
    dir_path = "/root/imgs/"
    # 构造图片路径,这里只加载9张图片
    paths = []
    for i in range(1, 10):
        tmp_path = dir_path+"img"+i.__str__().zfill(2)+".jpg"
        paths.append(tmp_path)

    # 加载参数也有多种方式
    # 1.通过OpenCV的FileStorage加载
    fs = cv2.FileStorage("config.yml", cv2.FILE_STORAGE_READ)
    cv_camera_mat = fs.getNode("cam_mat")
    cv_dist_mat = fs.getNode("dist_mat")

    # 2.通过ROS的load加载
    # 初始化节点并指定节点名称
    rospy.init_node("undistortion")
    # 先将所有参数载入
    os.system("rosparam load " + dir_path + "all_params.yaml")

    # 找到对应数据
    cam_vec = rospy.get_param("cam_mat")
    dist_vec = rospy.get_param("dist_mat")

    # 将数据转换成NDArray类型
    camera_mat = cvtList2NDArray(cam_vec, 3, 3)
    dist = cvtList2NDArray(dist_vec, 1, 5)

    # 校正图片
    undistortImgs(paths, camera_mat, dist)
    print "Undistortion finished."
(6)运行测试

最后在终端中输入roscore启动Master,在scripts目录下打开新终端然后分别输入python calibration.py启动服务节点,python undistortion.py启动客户节点。运行界面如下:

5.launch文件用法

在RoboWare的文件管理器中,在项目名称上单击右键,然后选择”新建Launch文件夹”,然后在新建的launch文件夹上右键,选择”新建LAUNCH文件”,输入名称undistort.launch。然后在文件中写入如下内容:

<launch>
	<!--param参数配置演示-->
	<param name="param1" value="1" />
	<param name="param2" value="2" />
	<!--rosparam参数配置演示-->
	<rosparam>   
        param3: 3
        param4: 4
        param5: 5
    </rosparam>
	<!--以上写法将参数转成YAML文件加载,注意param前面必须为空格,不能用Tab,否则YAML解析错误-->
	<!--launch文件中直接装载参数文件-->
    <rosparam file="/root/imgs/all_params.yaml" command="load"/>
	<!--启动的节点信息-->
	<node pkg="camera_calibration" type="undistortion" name="camera_calibration" output="screen" />
</launch>

在终端中输入roslaunch camera_calibration undistort.launch即可启动launch文件。或者直接在RoboWare中,右键点击launch文件,选择”运行Launch文件”即可。 采用launch文件启动方式的好处是不用再手动启动roscore,而且可以同时启动多个节点,还可以设置参数等等。

最后所有代码上传到了Github,点击查看

本文作者原创,未经许可不得转载,谢谢配合

返回顶部