Add stereo matching validator for multiple camera calibration

aslam_offline_calibration/kalibr/python/kalibr_camera_validator_stereo_match

@@ -0,0 +1,369 @@
+#!/usr/bin/env python
+# coding=utf-8
+from __future__ import division, print_function
+
+import argparse
+import thread
+import time
+
+import aslam_cv as acv
+import cv2
+import igraph
+import message_filters
+import numpy as np
+import rospy
+import sm
+from cv_bridge import CvBridge, CvBridgeError
+from sensor_msgs.msg import Image, CompressedImage
+
+import kalibr_common as kc
+
+# make numpy print prettier
+np.set_printoptions(suppress=True)
+
+
+class CameraChainValidator(object):
+    def __init__(self, chainConfig):
+
+        self.current_cam_msgs = None
+
+        self.chainConfig = chainConfig
+        self.numCameras = chainConfig.numCameras()
+        self.bridge = CvBridge()
+
+        # initialize the cameras in the chain
+        self.G = igraph.Graph(self.numCameras)
+        self.monovalidators = []
+        for cidx in range(0, self.numCameras):
+            camConfig = chainConfig.getCameraParameters(cidx)
+
+            # create a mono instance for each cam (detection and mono view)
+            monovalidator = MonoCameraValidator(camConfig)
+            self.monovalidators.append(monovalidator)
+
+            # add edges to overlap graph
+            overlaps = chainConfig.getCamOverlaps(cidx)
+            for overlap in overlaps:
+                # add edge if it isn't existing yet
+                try:
+                    edge_idx = self.G.get_eid(cidx, overlap)
+                except:
+                    self.G.add_edges([(cidx, overlap)])
+
+        # prepare the rectification maps
+        for edge in self.G.es:
+            cidx_src = edge.source
+            cidx_dest = edge.target
+
+            edge["rect_map"] = dict();
+            edge["R"] = dict();
+            edge["A"] = dict();
+
+            edge["rect_map"][cidx_src], \
+            edge["rect_map"][cidx_dest], \
+            edge["R"][cidx_src], \
+            edge["R"][cidx_dest], \
+            edge["A"][cidx_src], \
+            edge["A"][cidx_dest] = self.prepareStereoRectificationMaps(cidx_src, cidx_dest)
+
+        # register the callback for the synchronized images
+        sync_sub = message_filters.TimeSynchronizer([val.image_sub for val in self.monovalidators], 1000)
+        sync_sub.registerCallback(self.synchronizedCallback)
+
+        # initialize message throttler
+        self.timeLast = 0
+
+        thread.start_new_thread(self.processing_thread, ())
+
+    def processing_thread(self):
+        while True:
+            cam_msgs = self.current_cam_msgs
+            if cam_msgs is None:
+                continue
+
+            # process the images of all cameras
+            for cam_nr, msg in enumerate(cam_msgs):
+
+                # convert image to numpy
+                try:
+                    if (type(msg) is CompressedImage):
+                        cv_image = cv2.imdecode(np.fromstring(msg.data, np.uint8), cv2.IMREAD_COLOR)
+                    else:
+                        if (msg.encoding == "rgb8"):
+                            cv_image = np.squeeze(np.array(self.bridge.imgmsg_to_cv2(msg, "mono8")))
+                        else:
+                            cv_image = self.bridge.imgmsg_to_cv2(msg)
+                    np_image = np.array(cv_image)
+                    if np_image.shape[1] > 1:
+                        np_image = cv2.cvtColor(np_image, cv2.COLOR_RGB2GRAY)
+                except CvBridgeError, e:
+                    print(e)
+
+                # get the corresponding monovalidator instance
+                validator = self.monovalidators[cam_nr]
+
+                # undistort the image
+                if type(validator.camera.geometry) == acv.DistortedOmniCameraGeometry:
+                    validator.undist_image = validator.undistorter.undistortImageToPinhole(np_image)
+                else:
+                    validator.undist_image = validator.undistorter.undistortImage(np_image)
+
+            # generate all rectification views
+            for edge in self.G.es:
+                cidx_src = edge.source
+                cidx_dest = edge.target
+                self.generatePairView(cidx_src, cidx_dest)
+
+            cv2.waitKey(1)
+
+    def synchronizedCallback(self, *cam_msgs):
+        # throttle image processing
+        rate = 100  # Hz
+        timeNow = time.time()
+        if (timeNow - self.timeLast < 1.0 / rate) and self.timeLast != 0:
+            return
+        self.timeLast = timeNow
+        self.current_cam_msgs = cam_msgs
+
+    # returns transformation T_to_from
+    def getTransformationCamFromTo(self, cidx_from, cidx_to):
+        # build pose chain (target->cam0->baselines->camN)
+        lowid = min((cidx_from, cidx_to))
+        highid = max((cidx_from, cidx_to))
+
+        T_high_low = sm.Transformation()
+        for cidx in range(lowid, highid):
+            baseline_HL = self.chainConfig.getExtrinsicsLastCamToHere(cidx + 1)
+            T_high_low = baseline_HL * T_high_low
+
+        if cidx_from < cidx_to:
+            T_BA = T_high_low
+        else:
+            T_BA = T_high_low.inverse()
+
+        return T_BA
+
+    def rectifyAndStereoMatching(self, imageA, mapA, imageB, mapB):
+        # rectify images
+        rect_image_A = cv2.remap(imageA,
+                                 mapA[0],
+                                 mapA[1],
+                                 cv2.INTER_LINEAR)
+
+        rect_image_B = cv2.remap(imageB,
+                                 mapB[0],
+                                 mapB[1],
+                                 cv2.INTER_LINEAR)
+
+        # stereo match
+        scale_downsample = parsed.scale
+        rect_image_A = cv2.resize(rect_image_A, dsize=(
+            rect_image_A.shape[1] // scale_downsample,
+            rect_image_A.shape[0] // scale_downsample,
+        ))
+        rect_image_B = cv2.resize(rect_image_B, dsize=(
+            rect_image_B.shape[1] // scale_downsample,
+            rect_image_B.shape[0] // scale_downsample,
+        ))
+        # flip
+        rect_image_A = cv2.rotate(rect_image_A, cv2.ROTATE_180)
+        rect_image_B = cv2.rotate(rect_image_B, cv2.ROTATE_180)
+
+        if parsed.matcher == 'bm':
+            # BM Matching
+            stereo = cv2.StereoBM_create(numDisparities=32, blockSize=15)
+        elif parsed.matcher == 'sgbm':
+            # SGBM Matching
+            window_size = 5
+            stereo = cv2.StereoSGBM_create(
+                minDisparity=0,
+                numDisparities=48,  # max_disp has to be dividable by 16 f. E. HH 192, 256
+                blockSize=5,
+                P1=8 * 3 * window_size ** 2,
+                # wsize default 3; 5; 7 for SGBM reduced size image; 15 for SGBM full size image (1300px and above); 5 Works nicely
+                P2=32 * 3 * window_size ** 2,
+                disp12MaxDiff=1,
+                uniquenessRatio=15,
+                speckleWindowSize=0,
+                speckleRange=2,
+                preFilterCap=63,
+                mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY
+            )
+        else:
+            raise NotImplementedError('Stereo matching method {} not supported'.format(parsed.method))
+
+        # stereo compute
+        disparity = stereo.compute(rect_image_A, rect_image_B)
+        # normalize
+        disparity = np.int_(256 * np.float_(disparity - disparity.min()) / (disparity.max() - disparity.min())) \
+            .astype(np.uint8)
+        # print(disparity.max(), disparity.min())
+        depth_map = cv2.applyColorMap((255 - disparity).astype(np.uint8), cv2.COLORMAP_JET)
+        # remove useless boundary
+        depth_map[np.where((disparity == 0) | (rect_image_A == 0) | (rect_image_B == 0))] = 0
+        # combine the images
+        np_rect_image = np.hstack((rect_image_A, rect_image_B))
+
+        return np_rect_image, depth_map
+
+    def generatePairView(self, camAnr, camBnr):
+        # prepare the window
+        windowName = "Rectified view (cam{0} and cam{1})".format(camAnr, camBnr)
+        windowNameDisparity = "Disparity from (cam{0} and cam{1})".format(camAnr, camBnr)
+        # cv2.namedWindow(windowName, 1)
+
+        # get the mono validators for each cam
+        camA = self.monovalidators[camAnr]
+        camB = self.monovalidators[camBnr]
+
+        # rectify the undistorted images
+        edge_idx = self.G.get_eid(camAnr, camBnr)
+        edge = self.G.es[edge_idx]
+
+        np_image_rect, depth_map = self.rectifyAndStereoMatching(camA.undist_image,
+                                                                 edge["rect_map"][camAnr],
+                                                                 camB.undist_image,
+                                                                 edge["rect_map"][camBnr])
+
+        # draw some epilines
+        np_image_rect = cv2.cvtColor(np_image_rect, cv2.COLOR_GRAY2BGR)
+        n = 10
+        for i in range(0, n):
+            y = int(np_image_rect.shape[0] * i / n)
+            cv2.line(np_image_rect, (0, y), (2 * np_image_rect.shape[1], y), (0, 255, 0))
+
+        # cv2.imshow(windowName, cv2.resize(np_image_rect, (np_image_rect.shape[1] // 2, np_image_rect.shape[0] // 2)))
+
+        # depth_map[np.where(rect_image_B == 0)] = 0
+        cv2.imshow(windowName, np_image_rect)
+        cv2.imshow(windowNameDisparity, depth_map)
+
+    def prepareStereoRectificationMaps(self, camAnr, camBnr):
+        # get the camera parameters for the undistorted cameras
+        camIdealA = self.monovalidators[camAnr].undist_camera.projection().getParameters().flatten()
+        camIdealB = self.monovalidators[camBnr].undist_camera.projection().getParameters().flatten()
+        camIdealA = np.array([[camIdealA[0], 0, camIdealA[2]], [0, camIdealA[1], camIdealA[3]], [0, 0, 1]])
+        camIdealB = np.array([[camIdealB[0], 0, camIdealB[2]], [0, camIdealB[1], camIdealB[3]], [0, 0, 1]])
+        imageSize = (self.monovalidators[camAnr].undist_camera.projection().ru(),
+                     self.monovalidators[camAnr].undist_camera.projection().rv())
+
+        # get the baseline between the cams
+        baseline_BA = self.getTransformationCamFromTo(camAnr, camBnr)
+
+        ##
+        # A.Fusiello, E. Trucco, A. Verri: A compact algorithm for recification of stereo pairs, 1999
+        ##
+        Poa = np.matrix(camIdealA) * np.hstack(
+            (np.matrix(np.eye(3)), np.matrix(np.zeros((3, 1)))))  # use camA coords as world frame...
+        Pob = np.matrix(camIdealB) * np.hstack((np.matrix(baseline_BA.C()), np.matrix(baseline_BA.t()).T))
+
+        # optical centers (in camA's coord sys)
+        c1 = -np.linalg.inv(Poa[:, 0:3]) * Poa[:, 3]
+        c2 = -np.linalg.inv(Pob[:, 0:3]) * Pob[:, 3]
+
+        # get "mean" rotation between cams
+        old_z_mean = (baseline_BA.C()[2, :].flatten() + sm.Transformation().T()[2, 0:3]) / 2.0
+        v1 = c1 - c2  # newx-axis = direction of baseline
+        v2 = np.cross(np.matrix(old_z_mean).flatten(), v1.flatten()).T  # new y axis orthogonal to new x and mean old z
+        v3 = np.cross(v1.flatten(), v2.flatten()).T  # orthogonal to baseline and new y
+
+        # normalize
+        v1 = v1 / np.linalg.norm(v1)
+        v2 = v2 / np.linalg.norm(v2)
+        v3 = v3 / np.linalg.norm(v3)
+
+        # create rotation matrix
+        R = np.hstack((np.hstack((v1, v2)), v3)).T
+
+        # new intrinsic parameters
+        A = (camIdealA + camIdealB) / 2.0
+
+        # new projection matrices
+        Pna = A * np.hstack((R, -R * c1))
+        Pnb = A * np.hstack((R, -R * c2))
+
+        # rectyfing transforms
+        Ta = Pna[0:3, 0:3] * np.linalg.inv(Poa[0:3, 0:3])
+        Tb = Pnb[0:3, 0:3] * np.linalg.inv(Pob[0:3, 0:3])
+
+        Ra = R  # camA=world, then to rectified coords
+        Rb = R * baseline_BA.inverse().C()  # to world then to rectified coords
+
+        # create the rectification maps
+        rect_map_x_a, rect_map_y_a = cv2.initUndistortRectifyMap(camIdealA,
+                                                                 np.zeros((4, 1)),
+                                                                 Ra,
+                                                                 A,
+                                                                 imageSize,
+                                                                 cv2.CV_16SC2)
+
+        rect_map_x_b, rect_map_y_b = cv2.initUndistortRectifyMap(camIdealB,
+                                                                 np.zeros((4, 1)),
+                                                                 Rb,
+                                                                 A,
+                                                                 imageSize,
+                                                                 cv2.CV_16SC2)
+
+        return (rect_map_x_a, rect_map_y_a), (rect_map_x_b, rect_map_y_b), Ra, Rb, A, A
+
+
+class MonoCameraValidator(object):
+    def __init__(self, camConfig):
+
+        print("initializing camera geometry")
+        self.camera = kc.ConfigReader.AslamCamera.fromParameters(camConfig)
+
+        # print details
+        print("Camera {0}:".format(camConfig.getRosTopic()))
+        camConfig.printDetails();
+
+        self.topic = camConfig.getRosTopic()
+        self.windowName = "Camera: {0}".format(self.topic)
+        # cv2.namedWindow(self.windowName, 0)
+        # register the cam topic to the message synchronizer
+        if parsed.compressed_image:
+            self.image_sub = message_filters.Subscriber(self.topic, CompressedImage)
+        else:
+            self.image_sub = message_filters.Subscriber(self.topic, Image)
+
+        # create image undistorter
+        alpha = 1.0
+        scale = 1.0
+        self.undistorter = self.camera.undistorterType(self.camera.geometry, cv2.INTER_LINEAR, alpha, scale)
+
+        if type(self.camera.geometry) == acv.DistortedOmniCameraGeometry:
+            # convert omni image to pinhole image aswell
+            self.undist_camera = self.undistorter.getIdealPinholeGeometry()
+        else:
+            self.undist_camera = self.undistorter.getIdealGeometry()
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description='Validate the intrinsics of a camera.')
+    parser.add_argument('--cam', dest='chainYaml', help='Camera configuration as yaml file', required=True)
+    parser.add_argument('--verbose', action='store_true', dest='verbose', help='Verbose output')
+    parser.add_argument('--compressed_image', action='store_true', help='Using CompressedImage msg', required=False)
+    parser.add_argument('--scale', type=int, default=2,
+                        help='Downsample scale, using larger downsample scale to speed up stereo matching',
+                        required=False)
+    parser.add_argument('--matcher', default='sgbm', help='Stereo matching algorithm [sgbm|bm]', required=False)
+    parsed = parser.parse_args()
+
+    if parsed.verbose:
+        sm.setLoggingLevel(sm.LoggingLevel.Debug)
+    else:
+        sm.setLoggingLevel(sm.LoggingLevel.Info)
+
+    camchain = kc.ConfigReader.CameraChainParameters(parsed.chainYaml)
+
+    # create the validator
+    chain_validator = CameraChainValidator(camchain)
+
+    # ros message loops
+    rospy.init_node('kalibr_validator', anonymous=True)
+    try:
+        rospy.spin()
+    except KeyboardInterrupt:
+        print("Shutting down")
+
+    cv2.destroyAllWindows()