相机标定+识别二维码+solvepnp解算位姿

一、相机标定原理

深度科普：一文搞懂相机标定 - 知乎 (zhihu.com)

二、安装相机标定包并开始标定

现在已经知道标定相机是为了获得 内参矩阵和畸变系数，而获得内参矩阵和畸变系数是为了后面用opencv自带的函数solvepnp解算外参矩阵。

先安装usb-cam功能包

sudo apt install ros-noetic-usb-cam

再安装ros自带的相机标定的包

#noetic摄像头标定工具安装
sudo apt-get install ros-noetic-camera-calibration

启动摄像头并开始标定

#启动摄像头
roslaunch usb_cam usb_cam-test.launch
#开始标定
rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.024 image:=/usb_cam/image_raw camera:=/usb_cam

标定前注意：

建议最好先看一下处理图像时打开摄像头获取的每一帧的长宽是多少，然后把标定时获取的图像也改成这么大的。因为处理的图像如果和标定时获取的图像大小不一致那么内参也是不一样的。

具体怎么改：

找到usb_cam-test.launch这个文件，一般在/opt/ros/noetic/share/usb_cam/launch这个路径下，原文件一般是只读状态，我们可以复制里面的内容，把它放到一个名为my_usb_cam-test.launch的文件里，将image_width和image_height的参数改为打开摄像头时默认获得的大小，我这里是1280和720。

然后把这个launch文件放到一个功能包下的launch文件夹里

比如我这里就放到了opencv这个功能包里，新建一个launch文件夹然后放进去。

这时候把

#启动摄像头
roslaunch usb_cam usb_cam-test.launch

改为：

#启动摄像头
roslaunch opencv my_usb_cam-test.launch 
roslaunch 包名    launch文件名

然后再

#开始标定
rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.024 image:=/usb_cam/image_raw camera:=/usb_cam

标定命令里的参数意思如下：

1）size:标定棋盘格的内部角点个数，这里使用的棋盘一共有多少行，每行有个内部角点。
2）square:这个参数对应每个棋盘格的边长，单位是米。
3）image和camera:设置摄像头发布的图像话题。

按照x（左右）、y（上下）、size（前后）、skew（倾斜）等方式移动棋盘，直到x,y,size,skew的进度条都变成绿色位置。

如果觉得差不多了就可以按下CALIBRATE。

把终端里的内参和畸变系数拷贝一下，放到一个自己建的txt文件里就可以了。

三、二维码识别功能包安装

二维码识别需要下载的包

1. 安装ZBar库,这里使用apt安装而不是用源码

  sudo apt install libzbar-dev

在Ｕbuntu中使用OpenCV-C++和ZBar库_树莓派安装zbar-CSDN博客

可以通过上面这篇博客看一下自己的zbar有没有装好。

四、opencv安装

如何在 Ubuntu 20.04 上安装 OpenCV_ubuntu import cv2-CSDN博客

可以先试试第一个，有问题再试试下面的教程，或者自己搜别的教程。

如果输入

pkg-config --modversion opencv4

输出：

4.2.0

就说明成功安装了opencv的4.2.0版本。

opencv在/usr/share/opencv4下；头文件在/usr/include/opencv4下，可以自己查看一下。

五、solvepnp和zbar的使用（实现vscode+zbar+opencv+ros的编程）

这是opencv自带的一个函数，可以自己了解一下。我们要通过这个函数计算外参数矩阵。

OpenCV:solvePnP参数介绍_cv::solvepnp-CSDN博客

可以看到这里就用到了我们之前标定相机的结果，即相机的内参和畸变系数。

以下是ros加C++加opencv加zbar的代码实现

在工作空间下建一个功能包“opencv”。取别的名字也行。

在opencv的src目录下新建一个QR_ROS.cpp，代码如下：

#include"ros/ros.h"
#include <iostream>
#include <opencv2/opencv.hpp>
#include <zbar.h>

using namespace std;
using namespace cv;
using namespace zbar;

// 相机内参矩阵
const Mat camera_matrix = (Mat_<double>(3, 3) << 970.59132483, 0, 646.87144122,0, 971.20780134, 427.7375449,0.00, 0.0, 1.0);
// 相机畸变系数
const Mat dist_coeffs = (Mat_<double>(5, 1) << -0.12721462, -1.10308713,0.045049, -0.01256439, 2.92627787);

int main(int argc, char *argv[]){
    ros::init(argc,argv,"QR_ROS");//ros节点名称不可重复
    ros::NodeHandle nh;
    cv::VideoCapture cap(0);  // 打开摄像头设备，0表示默认摄像头

    if (!cap.isOpened()) {
        std::cerr << "Error: Couldn't open the camera." << std::endl;
        return 1;
    }

    // Initialize the ZBar scanner
    zbar::ImageScanner scanner;
    cv::Mat frame;
    cv::Mat frame_gray;
    cv::Scalar color = cv::Scalar(0, 255, 255);
    cv::Scalar color1 = cv::Scalar(0, 0, 255);
    cv::Scalar color2 = cv::Scalar(0, 255, 0);
    cv::Scalar color3 = cv::Scalar(255, 0, 0);
    cv::Scalar color4 = cv::Scalar(255, 255, 0);


    while (true) {
        //cv::Mat frame;
        cap >> frame;  // 从摄像头捕捉帧

        // Convert OpenCV frame to grayscale
        cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);

        // Create a ZBar image
        zbar::Image zbar_image(frame_gray.cols, frame_gray.rows, "Y800", frame_gray.data, frame_gray.cols * frame_gray.rows);

        // Scan the image for barcodes
        int result = scanner.scan(zbar_image);

        // Iterate through the results
        for (zbar::Image::SymbolIterator symbol = zbar_image.symbol_begin(); symbol != zbar_image.symbol_end(); ++symbol) {
            std::cout << "Data: " << symbol->get_data() << std::endl;

            //定位信息，
		    int x = symbol->get_location_x(0);
		    int y = symbol->get_location_y(0);
            cv::circle(frame, cv::Point(x,y), 15, color, 2, cv::LINE_8);
            //根据zbar 返回的多边形的像素点位置 计算宽高 
            //默认二维码是垂直水平的
            int min_x = 0, min_y=0, max_x=0, max_y=0;
            Symbol::PointIterator pt = symbol->point_begin();
            Symbol::Point p = *pt;
            Symbol::PointIterator pt1 = symbol->point_begin();
            Symbol::Point p1 = *pt1;
            Symbol::PointIterator begin = symbol->point_begin();
            Symbol::Point begin_1 = *begin;
            
            
            //逐点画圆并把二维码框起来
            ++pt1;
            p1 = *pt1;
            //画一个实心圆  
            cv::circle(frame, cv::Point(p.x,p.y), 10, color1, 2, cv::LINE_8);
            cv::line(frame, cv::Point(p.x,p.y), cv::Point(p1.x, p1.y), color, 3);
            ++pt;
            ++pt1;
            p1 = *pt1;
            p = *pt;
            cv::circle(frame, cv::Point(p.x,p.y), 10, color2, 2, cv::LINE_8);
            cv::line(frame, cv::Point(p.x,p.y), cv::Point(p1.x, p1.y), color, 3);
            ++pt;
            ++pt1;
            p1 = *pt1;
            p = *pt;
            cv::circle(frame, cv::Point(p.x,p.y), 10, color3, 2, cv::LINE_8);
            cv::line(frame, cv::Point(p.x,p.y), cv::Point(p1.x, p1.y), color, 3);
            ++pt;
            p = *pt;
            cv::circle(frame, cv::Point(p.x,p.y), 10, color4, 2, cv::LINE_8);
            cv::line(frame, cv::Point(p.x,p.y), cv::Point(begin_1.x, begin_1.y), color, 3);
             
            //从左上角开始逆时针
            std::vector<Point2d> image_points;
            pt = symbol->point_begin();
            for (; pt != (Symbol::PointIterator)symbol->point_end(); ++pt) {
                p = *pt;
                image_points.push_back(Point2d(p.x, p.y));
            }

            // 3D 特征点世界坐标，与像素坐标对应，单位是mm
            std::vector<Point3d> model_points;
            model_points.push_back(Point3d(-30.0, -30.0, 0.0)); // 左上角 单位 mm
            model_points.push_back(Point3d(-30.0, +30.0, 0.0));
            model_points.push_back(Point3d(+30.0, +30.0, 0.0));
            model_points.push_back(Point3d(+30.0, -30.0, 0.0));
            cout << "Camera Matrix " << endl << camera_matrix << endl << endl;
            // 旋转向量
            Mat rotation_vector;
            // 平移向量
            Mat translation_vector;

            // pnp求解
            solvePnP(model_points, image_points, camera_matrix, dist_coeffs, rotation_vector, translation_vector);
            // 默认ITERATIVE方法，可尝试修改为EPNP（CV_EPNP）,P3P（CV_P3P）

            cout << "Rotation Vector " << endl << rotation_vector << endl << endl;
            cout << "Translation Vector" << endl << translation_vector << endl << endl;


        }

        // Display the frame with detected barcodes
        cv::imshow("Barcode Scanner", frame);

        // Exit on 'q' key press
        if (cv::waitKey(1) == 'q') {
            break;
        }
    }

    //zbar_image.set_data(NULL, 0); //清除缓存

    return 0;
}

 目录结构如下，那个open_camera.cpp不用管它

 编写CMake文件（src目录下的那个，不要搞错了）参考如下：

cmake_minimum_required(VERSION 3.0.2)
project(opencv)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
#set(OpenCV_DIR /usr/share/opencv4/)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/home/qly/VS_Project/Ros_Debug/src/opencv/cmake") #写FindZBar的绝对路径
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
)

find_package(OpenCV REQUIRED)
#set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake")
# 在CMake中查找ZBar库
find_package(ZBar REQUIRED)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)


## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()

################################################
## Declare ROS messages, services and actions ##
################################################

## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
##   * add a build_depend tag for "message_generation"
##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
##     but can be declared for certainty nonetheless:
##     * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
##   * add "message_generation" and every package in MSG_DEP_SET to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * add "message_runtime" and every package in MSG_DEP_SET to
##     catkin_package(CATKIN_DEPENDS ...)
##   * uncomment the add_*_files sections below as needed
##     and list every .msg/.srv/.action file to be processed
##   * uncomment the generate_messages entry below
##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)

## Generate messages in the 'msg' folder
# add_message_files(
#   FILES
#   Message1.msg
#   Message2.msg
# )

## Generate services in the 'srv' folder
# add_service_files(
#   FILES
#   Service1.srv
#   Service2.srv
# )

## Generate actions in the 'action' folder
# add_action_files(
#   FILES
#   Action1.action
#   Action2.action
# )

## Generate added messages and services with any dependencies listed here
# generate_messages(
#   DEPENDENCIES
#   std_msgs
# )

################################################
## Declare ROS dynamic reconfigure parameters ##
################################################

## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
##   * add "dynamic_reconfigure" to
##     find_package(catkin REQUIRED COMPONENTS ...)
##   * uncomment the "generate_dynamic_reconfigure_options" section below
##     and list every .cfg file to be processed

## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
#   cfg/DynReconf1.cfg
#   cfg/DynReconf2.cfg
# )

###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES opencv
#  CATKIN_DEPENDS roscpp rospy std_msgs
#  DEPENDS system_lib
)

###########
## Build ##
###########

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

## Declare a C++ library
# add_library(${PROJECT_NAME}
#   src/${PROJECT_NAME}/opencv.cpp
# )

## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(open_camera src/open_camera.cpp)
add_executable(QR_ROS src/QR_ROS.cpp)

## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")

## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

## Specify libraries to link a library or executable target against
target_link_libraries(open_camera
${OpenCV_LIBS}
)
target_link_libraries(open_camera
  ${catkin_LIBRARIES}
)

target_link_libraries(QR_ROS
${OpenCV_LIBS}
${ZBAR_LIBRARIES}
)
target_link_libraries(QR_ROS
  ${catkin_LIBRARIES}         #没有这个就会报错跟ros有关的代码
)






#############
## Install ##
#############

# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
#   scripts/my_python_script
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )

## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )

## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
#   FILES_MATCHING PATTERN "*.h"
#   PATTERN ".svn" EXCLUDE
# )

## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
#   # myfile1
#   # myfile2
#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

#############
## Testing ##
#############

## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_opencv.cpp)
# if(TARGET ${PROJECT_NAME}-test)
#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()

## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

其实要添加的就是这几处地方：

1.set一下FindZBar.cmake包的绝对路径，这里是我自己的路径。大家要改一下。这里是为了编译的时候能找到ZBAR这个包

set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "/home/qly/VS_Project/Ros_Debug/src/opencv/cmake") #写FindZBar的绝对路径

2.找到opencv和zbar的包

find_package(OpenCV REQUIRED)
#set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake")
# 在CMake中查找ZBar库
find_package(ZBar REQUIRED)

3.添加可执行文件

add_executable(QR_ROS src/QR_ROS.cpp)

4. 说明要链接的库，第一个是链接Zbar和opencv，第二个应该是链接跟ros有关的库，没有这个ros 的代码就会报错。

target_link_libraries(QR_ROS
${OpenCV_LIBS}
${ZBAR_LIBRARIES}
)
target_link_libraries(QR_ROS
  ${catkin_LIBRARIES}         #没有这个就会报错跟ros有关的代码
)

然后编译运行一下就可以了。