可见光相机与红外相机标定

目录

1.打开相机

2.可见光相机畸变矫正

安装相关库

查看相机参数

修改launch文件

启动标定程序

启动矫正图像节点

3.查看相机图像

4.计算H举矩阵

手动标注获取每一组的H矩阵（h.py）

计算出平均H矩阵并查看标定效果（MeanH_Test.py）

5.接受、处理、发送图像（biaoding.py）

6.标定效果

---

1.打开相机

cd camera_ws
source devel/setup.bash
roslaunch usb_cam usb_cam-dual_model_rectified.launch

2.可见光相机畸变矫正

安装相关库

catkin_make -DCATKIN_WHITELIST_PACKAGES="aruco;aruco_ros;aruco_msgs"
catkin_make -DCATKIN_WHITELIST_PACKAGES="aruco_mapping;lidar_camera_calibration"
catkin_make -DCATKIN_WHITELIST_PACKAGES=""

查看相机参数

ls /dev/video* #查看video个数和名称
v4l2-ctl -d 0 --all #查看video0的参数

修改launch文件

启动标定程序

rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.06 image:=/usb_cam/image_raw camera:=/usb_cam

• 移动标定板时，标定窗口中的各指标增加。
• 当CALIBRATE按钮亮起时，单击计算相机内参矩阵及矫正参数，并输出至终端，此过程可能需要1-2分钟。
• 当SAVE按钮亮起时，单击将采集的样本图片和计算结果保存
• 当COMMIT按钮亮起时，单击将标定结果写入~/.ros/camera_info/head_camera.yaml。注：此标定文件将被原始图像节点usb_cam读取，标定参数会被发布至话题/usb_cam/camera_info以供矫正图像节点image_proc使用。

启动矫正图像节点

ROS_NAMESPACE=/cam_rgb/usb_cam rosrun image_proc image_proc

3.查看相机图像

rqt_image_view

4.计算H举矩阵

手动标注获取每一组的H矩阵（h.py）

#!/usr/bin/python
# -*- coding: UTF-8 -*-
from PIL import Image
import cv2
import numpy as np
import pylab as pl
import matplotlib.pyplot as plt
import scipy.io as sio
from PIL import Image

def re_img(path):
    img = cv2.imread(path)
    a=int(20) #x start
    b=int(700) #x end
    c=int(0) #y start
    d=int(510) #y end
    cropimg=img[c:d,a:b]
    imgresize=cv2.resize(cropimg,(640,480))
    return imgresize

 
if __name__ == '__main__' :
    
    num = 9
    h=np.zeros((3,3))
        
    # 读取红外图.
    inf_Fig = "./rgb1/" + str(num) +"_dual.png"
    img_dst = re_img(inf_Fig)
    pl.figure(), pl.imshow(img_dst[:, :, ::-1]), pl.title('dual')
    # 红外图中的4个点
    dst_point = plt.ginput(4)  
    dst_point = np.float32(dst_point)
    
    # 读取RGB图像.
    RGB_Fig_1 = "./rgb1/"+ str(num) +".png"
    im_src_1 = cv2.imread(RGB_Fig_1)
    pl.figure(), pl.imshow(im_src_1[:, :, ::-1]), pl.title('rgb1')
    # RGB图中的4个点
    src_point_1 = plt.ginput(4)  
    src_point_1 = np.float32(src_point_1)


    # Calculate Homography
    h, status = cv2.findHomography(src_point_1, dst_point)

    # Warp source image to destination based on homography
    im_out_1 = cv2.warpPerspective(im_src_1,h,(640,480),borderValue=(255,255,255))
    pl.imshow(im_out_1[:, :, ::-1]), pl.title('out')
    pl.show()  #show dst

    #保存H矩阵
    H1_name = 'h'+str(num)+'.mat'
    print('H1:',h)
    sio.savemat(H1_name, {'Homography_Mat_1':h})

计算出平均H矩阵并查看标定效果（MeanH_Test.py）

#!/usr/bin/python
# -*- coding: UTF-8 -*-

from PIL import Image
import cv2
import numpy as np
#import pylab as pl
#import matplotlib.pyplot as plt
import scipy.io as sio
import time

class bd():

   
   def get_time_stamp(self):
       ct = time.time()
       local_time = time.localtime(ct)
       data_head = time.strftime("%Y-%m-%d %H:%M:%S", local_time)
       data_secs = (ct - int(ct)) * 1000
       time_stamp = "%s.%03d" % (data_head, data_secs)
       #print(time_stamp)
       stamp = ("".join(time_stamp.split()[0].split("-"))+"".join(time_stamp.split()[1].split(":"))).replace('.', '')
       #print(stamp)
       return stamp
   
   def re_img(self,img):
       #img = cv2.imread(path)
       a=int(20) #x start
       b=int(700) #x end
       c=int(0) #y start
       d=int(510) #y end
       cropimg=img[c:d,a:b]
       imgresize=cv2.resize(cropimg,(640,480))
       return imgresize

   def H_use(self,fig1):
       name_1 = 'H11.mat'
       data = sio.loadmat(name_1)
       h1 = data['Homography_Mat_1']
       h1[0][2]=h1[0][2]+30 #根据实际效果的修正
       h1[1][2]=h1[1][2]+10
       #print(h1[0][2])
       #print(h1)
       im_out_1 = cv2.warpPerspective(fig1,h1,(640,480))
       return im_out_1

   def Htest(self,path1,path2):
       im_src_1 = cv2.imread(path1)
       img_dst = self.re_img(path2)
       im_out_1= self.H_use(im_src_1)
       #pl.figure(),pl.imshow(img_dst[:, :, ::-1]), pl.title('dst'),
       #pl.figure(),pl.imshow(im_out_1[:, :, ::-1]), pl.title('out1'),
       #pl.show()
       return im_out_1, img_dst
       
   def Htest_t(self,img_t):
       img_dst = self.re_img(img_t)
       return img_dst

   def Htest_rgb(self,img_rgb):
       im_out_1= self.H_use(img_rgb)
       return im_out_1
   
   def cal_H(self):
       num=0
       H=np.zeros((3,3))
       while int(num) < 10:
             print(num)
             name = 'h' + str(num) +'.mat'
             data=sio.loadmat(name)
             h = data['Homography_Mat_1']
             print("h:",h)
             num = num + 1
             H=H+h
       H=H/10
       H1_name = 'H.mat'
       print('H1:',H)
       sio.savemat(H1_name, {'Homography_Mat_1':H})

if __name__ == '__main__' :
    B=bd()
    num=0
    #B.cal_H()
    while int(num)<10:
          rgb_path="./rgb1/"+str(num)+".png"  
          t_path="./rgb1/"+str(num)+"_t.png"
          img_rgb,img_t = B.Htest(rgb_path,t_path)
          cv2.imwrite('./out/'+str(num)+'.png',img_rgb)
          cv2.imwrite('./out/'+str(num)+'_t.png',img_t)
          num=num+1
    time=B.get_time_stamp()
    print(time)

5.接受、处理、发送图像（biaoding.py）

#! /usr/bin/env python2

from __future__ import print_function

#python2
import os
import roslib
import rospy
from std_msgs.msg import Header
from std_msgs.msg import String
from sensor_msgs.msg import Image
# from ros_numpy import msgify
from cv_bridge import CvBridge
import sys
sys.path.remove('/opt/ros/melodic/lib/python2.7/dist-packages')
import threading
import time
import numpy as np
import MeanH_Test as biao
import cv2
import threading
#python3


class SubscribeAndPublish:
    def __init__(self):
        self.all_obstacle_str=''

        self.sub1_name="/cam_rgb/usb_cam/image_rect_color"
        #self.sub1_name="/cam_rgb/usb_cam/image_raw"
        self.sub1= rospy.Subscriber(self.sub1_name, Image,self.callback_rgb)
        self.sub2_name="/cam_t/usb_cam/image_raw"
        self.sub2= rospy.Subscriber(self.sub2_name, Image,self.callback_t)
        
        self.rate = 10
        self.timer = rospy.Timer(rospy.Duration(1.0 / self.rate), self.callback_timer)
        self.lock_rgb = threading.Lock()
        self.lock_t = threading.Lock()
        self.img_rgb_global = []
        self.img_t_global = []

        self.pub1_name="pub_rgb"
        self.pub1= rospy.Publisher(self.pub1_name, Image,queue_size=1)
        self.pub2_name="pub_t"
        self.pub2= rospy.Publisher(self.pub2_name, Image,queue_size=1)
        self.mode=0
        self.path_rgb='./img/img_rgb'
        self.path_t='./img/img_t'
        # self.bridge = CvBridge()

       
    def callback_rgb(self,data):
        print('callback1')
        
        self.lock_rgb.acquire()
        img = np.frombuffer(data.data, dtype=np.uint8).reshape(data.height, data.width, -1)
        if len(self.img_rgb_global) > 0:
            self.img_rgb_global.pop(0)
            self.img_rgb_global.append(img)
        else:
            self.img_rgb_global.append(img)
        self.lock_rgb.release()
        
        
        #print('rgb')
        #img = CvBridge().imgmsg_to_cv2(data, "bgr8")
        #img = np.frombuffer(data.data, dtype=np.uint8).reshape(data.height, data.width, -1)
        #print(img.shape)
        #B=biao.bd()
        #time=B.get_time_stamp()
        #name='rgb_'+str(time)+'.jpg'
        #img_rgb=B.Htest_rgb(img)
        #cv2.imwrite('./img/img_rgb/'+str(name),img_rgb)
        #self.pub1.publish(CvBridge().cv2_to_imgmsg(img_rgb,"bgr8"))
     
    def callback_t(self,data):
        print('callback2')
        
        self.lock_t.acquire()
        img = np.frombuffer(data.data, dtype=np.uint8).reshape(data.height, data.width, -1)
        if len(self.img_t_global) > 0:
            self.img_t_global.pop(0)
            self.img_t_global.append(img)
        else:
            self.img_t_global.append(img)
        self.lock_t.release()
        
        
        #print('t')
        #img = CvBridge().imgmsg_to_cv2(data, "bgr8")
        #img = np.frombuffer(data.data, dtype=np.uint8).reshape(data.height, data.width, -1)
        #print(img.shape)
        #B=biao.bd()
        #time=B.get_time_stamp()
        #name='t_'+str(time)+'.jpg'
        #img_t=B.Htest_t(img)
        #cv2.imwrite('./img/img_t/'+str(name),img_t)
        #  self.pub2.publish(CvBridge().cv2_to_imgmsg(img_t,"bgr8"))

    def callback_timer(self, event):
        print('callback')
        self.lock_rgb.acquire()
        if len(self.img_rgb_global) > 0:
            img_rgb = self.img_rgb_global[0].copy()
        else:
            return
        self.lock_rgb.release()
        
        self.lock_t.acquire()
        if len(self.img_t_global) > 0:
            img_t = self.img_t_global[0].copy()
        else:
            return
        self.lock_t.release()
        B=biao.bd()
        time=B.get_time_stamp()
        img_rgb=B.Htest_rgb(img_rgb)
        img_t=B.Htest_t(img_t)
        img_rgb=img_rgb[:,:,::-1]
        img_t=img_t[:,:,::-1]
        self.mode=mode
        if self.mode==0:
           self.path_rgb=path_rgb
           self.path_t=path_t
           cv2.imwrite(self.path_rgb+'/rgb_'+str(time)+'.jpg',img_rgb)
           cv2.imwrite(self.path_t+'/t_'+str(time)+'.jpg',img_t)
           print('save image successful!')
        self.pub1.publish(CvBridge().cv2_to_imgmsg(img_rgb,"rgb8"))
        self.pub2.publish(CvBridge().cv2_to_imgmsg(img_t,"rgb8"))
        


def main(mode,path_rgb,path_t):
    rospy.init_node('biaoding_ws', anonymous=True)
    
    #####################
    t=SubscribeAndPublish()
    #####################
    rospy.spin()
if __name__ == "__main__":
    mode=0
    if len(sys.argv)>1:
       mode=int(sys.argv[1])
       if mode>1:
          mode=1
    else:
       mode=0
    print(mode)
    if mode==0:
       print("save image")
    else:
       print("no save image")
    
    #create dirs
    b=biao.bd()
    ti=b.get_time_stamp()
    path='./img'+str(ti)
    folder=os.path.exists(path)
    if not folder:
        os.makedirs(path)
    path_rgb=path+'/img_rgb'
    os.makedirs(path_rgb)
    path_t=path+'/img_t'
    os.makedirs(path_t)
    
    main(mode,path_rgb,path_t)

6.标定效果