Total s panoramic camera of ubuntu system is transformed by ROS image

Keywords: OpenCV Python Ubuntu

Total s panoramic camera of ubuntu system is transformed by ROS image

Following a blog post, after the Theta S camera was connected to the ROS system, the image matrix was published on the / image_raw topic. Let's start the file libuvc_camera.launch in the previous blog. At this time, the image has been transferred to the / image_raw topic. Open a new terminal and use the rostopic list to check the existing topic.
Image information is on the topic of / image · raw. Write a program like this to subscribe. The original image can be seen as follows.

#!/usr/bin/env python2.7
# -*- coding: utf-8 -*
import rospy
from sensor_msgs.msg import Image
import cv2
import cv_bridge


class Follower:
  def __init__(self):
    self.bridge = cv_bridge.CvBridge()
    self.image_sub = rospy.Subscriber('/image_raw', Image, self.image_callback)

  def image_callback(self,msg):
    image = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
    cv2.imshow("a",image)
    cv2.waitKey(1)
    
rospy.init_node('follower_1')
follower = Follower()
rospy.spin()


However, such an image is not a panoramic image, but a fish eye image of the front and back lenses of Theta S camera. Two images need to be transformed. There are several specific methods found on a Japanese blog (5 in total). The mathematical principle is not understood carefully, but the python program written by the blogger is inefficient (basically all for loops), and the real-time transformation effect is not good. I used numpy to improve the conversion speed of every picture, which is 16 times faster. The procedure is as follows.

#!/usr/bin/env python2.7
# -*- coding: utf-8 -*
import rospy
from sensor_msgs.msg import Image
import cv2
import cv_bridge
import numpy as np


class Follower:
  def __init__(self):
    self.bridge = cv_bridge.CvBridge()
    self.image_sub = rospy.Subscriber('/image_raw', Image, self.image_callback)

  def image_transfer(self, img):
    self.vertex = 640
    self.src_cx = 319
    self.src_cy = 319
    self.src_r = 283
    self.src_cx2 = 1280 - self.src_cx

    map_x = np.zeros((self.vertex, self.vertex*2))
    map_y = np.zeros((self.vertex, self.vertex*2))

    range_arr_y = np.array([range(self.vertex)])
    range_arr_x = np.array([range(self.vertex*2)])
    phi1 = (np.pi * range_arr_x) / self.vertex
    theta1 = (np.pi * range_arr_y.T) / self.vertex

    X = np.sin(theta1) * np.cos(phi1)
    Y = np.sin(theta1) * np.sin(phi1)
    Z = np.cos(theta1)

    phi2 = np.arccos(- X)
    theta2 = np.sign(Y) * np.arccos(-Z/np.sqrt(Y*Y + Z*Z))

    # Equidistant projection
    map_x = np.squeeze(np.where([phi2 < np.pi / 2], self.src_r * (phi2 / np.pi * 2) * np.cos(theta2) + self.src_cx\
    , self.src_r * ((np.pi - phi2)/np.pi * 2) * np.cos(np.pi - theta2) + self.src_cx2))
    map_y = np.squeeze(np.where([phi2 < np.pi / 2], self.src_r * (phi2 / np.pi * 2) * np.sin(theta2) + self.src_cy\
    , self.src_r * ((np.pi - phi2)/np.pi * 2) * np.sin(np.pi - theta2) + self.src_cy))

    # Stereoscopic projection
    # map_x = np.squeeze(np.where([phi2 < np.pi / 2], src_r * (np.tan((phi2) / 2)) * np.cos(theta2) + src_cx\
    # , src_r * (np.tan((np.pi - phi2) / 2)) * np.cos(np.pi - theta2) + src_cx2))
    # map_y = np.squeeze(np.where([phi2 < np.pi / 2], src_r * (np.tan((phi2) / 2)) * np.sin(theta2) + src_cy\
    # , src_r * (np.tan((np.pi - phi2) / 2)) * np.sin(np.pi - theta2) + src_cy))

    # Stereoscopic projective inverse transformation
    # map_x = np.squeeze(np.where([phi2 < np.pi / 2], src_r * (1 - np.tan((np.pi / 2 - phi2) / 2)) * np.cos(theta2) + src_cx\
    # , src_r * (1 - np.tan((-np.pi/2 + phi2) / 2)) * np.cos(np.pi - theta2) + src_cx2))
    # map_y = np.squeeze(np.where([phi2 < np.pi / 2], src_r * (1 - np.tan((np.pi / 2 - phi2) / 2)) * np.sin(theta2) + src_cy\
    # , src_r * (1 - np.tan((-np.pi/2 + phi2) / 2)) * np.sin(np.pi - theta2) + src_cy))
 
    # Orthography
    # map_x = np.squeeze(np.where([phi2 < np.pi / 2], src_r * (np.sin(phi2)) * np.cos(theta2) + src_cx\
    # , src_r * (np.sin(np.pi - phi2)) * np.cos(np.pi - theta2) + src_cx2))
    # map_y = np.squeeze(np.where([phi2 < np.pi / 2], src_r * (np.sin(phi2)) * np.sin(theta2) + src_cy\
    # , src_r * (np.sin(np.pi - phi2)) * np.sin(np.pi - theta2) + src_cy))

    #Positive projective inverse commutation
    # map_x = np.squeeze(np.where([phi2 < np.pi / 2], src_r * (1 - np.sin(np.pi / 2 - phi2)) * np.cos(theta2) + src_cx\
    # , src_r * (1 - np.sin(- np.pi / 2 + phi2)) * np.cos(np.pi - theta2) + src_cx2))
    # map_y = np.squeeze(np.where([phi2 < np.pi / 2], src_r * (1 - np.sin(np.pi / 2 - phi2)) * np.sin(theta2) + src_cy\
    # , src_r * (1 - np.sin(- np.pi / 2 + phi2)) * np.sin(np.pi - theta2) + src_cy))
 
    map_x = map_x.astype('float32')
    map_y = map_y.astype('float32')

    img = cv2.remap( img, map_x, map_y, cv2.INTER_LINEAR, cv2.BORDER_CONSTANT)
    img = cv2.resize(img, (0, 0), fx=0.7, fy=0.7, interpolation=cv2.INTER_CUBIC) 
    return img

  def image_callback(self,msg):
    image = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
    image = self.image_transfer(image)
    cv2.imshow("a",image)
    cv2.waitKey(1)

rospy.init_node('follower_1')
follower = Follower()
rospy.spin()

There are 5 transformation methods in total. After I try these methods, I feel that isometric photography is the best one. The picture is also much more continuous, with a little delay later, within the acceptable range.
The renderings are as follows.

It becomes panoramic, and then it can do image processing, object recognition and so on, ha ha.

Here, the opencv version is very important. PIP2 install opencv python = = version number. This version number is very important. Remember that there was a problem with the cv2.remap() function in the previous 4 versions. PIP2 uninstall opencv python = = version number uninstalls the current version and installs the version with more than 3 points. Sometimes, the cv2.imshow() function has a problem. The image does not always show the problem Come out. Anyway, if the 3-point-plus version doesn't work, try uninstalling the 3-point-plus version. Add this section to the program if you want to query your version.

print(cv2.__version__)

The opencv version of my computer is 3.3.0.x. I have forgotten how much this x is. The terminal displays the third place.

Published 3 original articles, won praise 1, visited 163
Private letter follow

Posted by Jove on Sat, 08 Feb 2020 00:36:02 -0800