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)
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)
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)
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)
