예제 #1
0
    def gotimage(self, imgmsg):
        if imgmsg.encoding == "bayer_bggr8":
            imgmsg.encoding = "8UC1"
        img = CvBridge().imgmsg_to_cv(imgmsg)
        # cv.ShowImage("DataMatrix", img)
        # cv.WaitKey(6)

        self.track(img)

        # monocular case
        print self.cams
        if 'l' in self.cams:
            for (code, corners) in self.tracking.items():
                model = cv.fromarray(numpy.array([[0,0,0], [.1, 0, 0], [.1, .1, 0], [0, .1, 0]], numpy.float32))

                rot = cv.CreateMat(3, 1, cv.CV_32FC1)
                trans = cv.CreateMat(3, 1, cv.CV_32FC1)
                cv.FindExtrinsicCameraParams2(model,
                                              cv.fromarray(numpy.array(corners, numpy.float32)),
                                              self.cams['l'].intrinsicMatrix(),
                                              self.cams['l'].distortionCoeffs(),
                                              rot,
                                              trans)

                ts = geometry_msgs.msg.TransformStamped()
                ts.header.frame_id = imgmsg.header.frame_id
                ts.header.stamp = imgmsg.header.stamp
                ts.child_frame_id = code
                posemsg = pm.toMsg(pm.fromCameraParams(cv, rot, trans))
                ts.transform.translation = posemsg.position
                ts.transform.rotation = posemsg.orientation

                tfm = tf.msg.tfMessage([ts])
                self.pub_tf.publish(tfm)
예제 #2
0
    def handle_mono(self, qq):
        (imgmsg, caminfo) = qq
        img = CvBridge().imgmsg_to_cv(imgmsg, "mono8")
        pcm = image_geometry.PinholeCameraModel()
        pcm.fromCameraInfo(caminfo)

        self.track(img)

        for (code, corners) in self.tracking.items():
            # detector numbers vertices clockwise like this:
            # 1 2
            # 0 3
            if isinstance(self.dim, (float, int)):
                d = self.dim
            else:
                d = self.dim[code]
            # User specifies black bar length, but detector uses
            # full module, including the 2-unit quiet zone,
            # so scale by 14/10
            d = d * 14 / 10
            model = cv.fromarray(numpy.array([[0, 0, 0], [0, d, 0], [d, d, 0], [d, 0, 0]], numpy.float32))

            rot = cv.CreateMat(3, 1, cv.CV_32FC1)
            trans = cv.CreateMat(3, 1, cv.CV_32FC1)
            cv.FindExtrinsicCameraParams2(
                model,
                cv.fromarray(numpy.array(corners, numpy.float32)),
                pcm.intrinsicMatrix(),
                pcm.distortionCoeffs(),
                rot,
                trans,
            )

            ts = geometry_msgs.msg.TransformStamped()
            ts.header.frame_id = imgmsg.header.frame_id
            ts.header.stamp = imgmsg.header.stamp
            ts.child_frame_id = code
            posemsg = pm.toMsg(pm.fromCameraParams(cv, rot, trans))
            ts.transform.translation = posemsg.position
            ts.transform.rotation = posemsg.orientation

            tfm = tf.msg.tfMessage([ts])
            self.pub_tf.publish(tfm)
예제 #3
0
    def handle_mono(self, qq):
        (imgmsg, caminfo) = qq
        img = CvBridge().imgmsg_to_cv(imgmsg, "mono8")
        pcm = image_geometry.PinholeCameraModel()
        pcm.fromCameraInfo(caminfo)

        self.track(img)

        for (code, corners) in self.tracking.items():
            # detector numbers vertices clockwise like this:
            # 1 2
            # 0 3
            if isinstance(self.dim, (float, int)):
                d = self.dim
            else:
                d = self.dim[code]
            # User specifies black bar length, but detector uses
            # full module, including the 2-unit quiet zone,
            # so scale by 14/10
            d = d * 14 / 10     
            model = cv.fromarray(numpy.array([[0,0,0], [0, d, 0], [d, d, 0], [d, 0, 0]], numpy.float32))

            rot = cv.CreateMat(3, 1, cv.CV_32FC1)
            trans = cv.CreateMat(3, 1, cv.CV_32FC1)
            cv.FindExtrinsicCameraParams2(model,
                                          cv.fromarray(numpy.array(corners, numpy.float32)),
                                          pcm.intrinsicMatrix(),
                                          pcm.distortionCoeffs(),
                                          rot,
                                          trans)

            ts = geometry_msgs.msg.TransformStamped()
            ts.header.frame_id = imgmsg.header.frame_id
            ts.header.stamp = imgmsg.header.stamp
            ts.child_frame_id = code
            posemsg = pm.toMsg(pm.fromCameraParams(cv, rot, trans))
            ts.transform.translation = posemsg.position
            ts.transform.rotation = posemsg.orientation

            tfm = tf.msg.tfMessage([ts])
            self.pub_tf.publish(tfm)
예제 #4
0
    def gotimage(self, imgmsg):
        if imgmsg.encoding == "bayer_bggr8":
            imgmsg.encoding = "8UC1"
        img = CvBridge().imgmsg_to_cv(imgmsg)
        # cv.ShowImage("DataMatrix", img)
        # cv.WaitKey(6)

        self.track(img)

        # monocular case
        print self.cams
        if "l" in self.cams:
            for (code, corners) in self.tracking.items():
                model = cv.fromarray(numpy.array([[0, 0, 0], [0.1, 0, 0], [0.1, 0.1, 0], [0, 0.1, 0]], numpy.float32))

                rot = cv.CreateMat(3, 1, cv.CV_32FC1)
                trans = cv.CreateMat(3, 1, cv.CV_32FC1)
                cv.FindExtrinsicCameraParams2(
                    model,
                    cv.fromarray(numpy.array(corners, numpy.float32)),
                    self.cams["l"].intrinsicMatrix(),
                    self.cams["l"].distortionCoeffs(),
                    rot,
                    trans,
                )

                ts = geometry_msgs.msg.TransformStamped()
                ts.header.frame_id = imgmsg.header.frame_id
                ts.header.stamp = imgmsg.header.stamp
                ts.child_frame_id = code
                posemsg = pm.toMsg(pm.fromCameraParams(cv, rot, trans))
                ts.transform.translation = posemsg.position
                ts.transform.rotation = posemsg.orientation

                tfm = tf.msg.tfMessage([ts])
                self.pub_tf.publish(tfm)