def kinematicCalculation(self, objMsg, joint):
# Get ball error
ballError = objMsg.ball_error
# Get camera kinematics
transformationMatrix = self.forwardKinematics(joint)
# If not find the ball
if objMsg.ball_confidence == False:
# Set ball 3D Cartesion is None
ball3DCartesian = None
else:
# Calculate 3D coordinate respect to base frame
ballPosition = np.array(objMsg.ball,
dtype=np.float64).reshape(-1, 2)
ball3DCartesian = self.calculate3DCoor(
ballPosition, HCamera=transformationMatrix)
ball3DCartesian = ball3DCartesian[0][1]
# If ball 3D cartesion is None
if ball3DCartesian is not None:
# Calculate polar coordinate change x, y -> r, theta
ball2DPolar = cv2.cartToPolar(ball3DCartesian[0],
ball3DCartesian[1])
ball2DPolar = np.array([ball2DPolar[0][0], ball2DPolar[0][0]])
else:
# If can't calculate 3D return empty vector
ball3DCartesian = np.array([])
ball2DPolar = np.array([])
rospy.logdebug(
"\nPosition in 3D coordinate : {}\n".format(ball3DCartesian))
# Get 2D projection back to camera
self.point2D1 = self.calculate2DCoor(self.points,
"ground",
HCamera=transformationMatrix)
self.point2D1 = np.array(self.point2D1)
# Publist positon dict message
msg = postDictMsg()
msg.ball_cart = ball3DCartesian.reshape(-1)
msg.ball_polar = ball2DPolar.reshape(-1)
msg.ball_img = objMsg.ball
msg.imgW = objMsg.imgW
msg.imgH = objMsg.imgH
msg.ball_error = ballError
msg.ball_confidence = objMsg.ball_confidence
msg.header.stamp = rospy.Time.now()
return msg
def kinematicCalculation(self, objMsg, joint):
# Get ball error
ballError = objMsg.ball_error
# Get camera kinematics
# transformationMatrix = self.forwardKinematics( joint )
qPan = getJsPosFromName(joint, "pan")
qTilt = getJsPosFromName(joint, "tilt")
transformationMatrix = getMatrixForForwardKinematic(qPan, qTilt)
# If not find the ball
if objMsg.ball_confidence == False:
# Set ball 3D Cartesion is None
ball3DCartesian = None
else:
# Calculate 3D coordinate respect to base frame
ballPosition = np.array(objMsg.ball,
dtype=np.float64).reshape(-1, 2)
ball3DCartesian = self.calculate3DCoor(
ballPosition, HCamera=transformationMatrix)
ball3DCartesian = ball3DCartesian[0][1]
# If ball 3D cartesion is None
if ball3DCartesian is not None:
# Calculate polar coordinate change x, y -> r, theta
ball2DPolar = cv2.cartToPolar(ball3DCartesian[0],
ball3DCartesian[1])
ball2DPolar = np.array([ball2DPolar[0][0], ball2DPolar[0][0]])
else:
# If can't calculate 3D return empty vector
ball3DCartesian = np.array([])
ball2DPolar = np.array([])
rospy.loginfo(
"\nPosition in 3D coordinate : {}\n".format(ball3DCartesian))
# Publist positon dict message
self.msg = postDictMsg()
self.msg.ball_cart = ball3DCartesian.reshape(-1)
self.msg.ball_polar = ball2DPolar.reshape(-1)
self.msg.ball_img = objMsg.ball
self.msg.imgW = objMsg.imgW
self.msg.imgH = objMsg.imgH
self.msg.ball_error = ballError
self.msg.ball_confidence = objMsg.ball_confidence
self.msg.header.stamp = rospy.Time.now()
return self.msg
def kinematicCalculation(self, objMsg, js, cortexMsg, rconfig=None):
t0 = time.time()
pointsClound2D = []
boundary2D = []
object2D = []
indexList = []
for i, (p, n) in enumerate(zip(objMsg.pos2D, objMsg.object_name)):
if n == 'point':
pointsClound2D.append([p.x, p.y])
elif n == 'field':
boundary2D.append([p.x, p.y])
else:
object2D.append([p.x, p.y])
indexList.append(i)
t1 = time.time()
pitch = math.radians(cortexMsg.pitch) if cortexMsg is not None else 0.0
roll = math.radians(cortexMsg.roll) if cortexMsg is not None else 0.0
# print math.degrees(pitch), math.degrees(roll)
# roll = pitch = 0.0
tranvec = np.zeros((3, 1))
rotvec = np.array([roll, pitch, 0.0])
HRotate = self.create_transformationMatrix(tranvec,
rotvec,
'rpy',
order="tran-first")
H = getMatrixForForwardKinematic(js.position[0], js.position[1], roll,
pitch)
H = np.matmul(HRotate, H)
t2 = time.time()
pointsClound3D = self.calculate3DCoor(pointsClound2D, HCamera=H)
t3 = time.time()
boundary3D = self.calculate3DCoor(boundary2D, HCamera=H)
t4 = time.time()
object3D = self.calculate3DCoor(object2D, HCamera=H)
t5 = time.time()
polarList = []
cartList = []
names = []
confidences = []
errorList = []
pos2DList = []
landmarkPose3D = []
ranges = []
points = []
splitIndexes = []
imgH = objMsg.imgH
imgW = objMsg.imgW
for (plane, p3D), i in zip(object3D, indexList):
name = objMsg.object_name[i]
confidence = objMsg.object_confidence[i]
p2D = objMsg.pos2D[i]
err = objMsg.object_error[i]
if name == 'magenta' or name == 'cyan':
cartList.append(Point32(x=err.x, y=err.y))
landmarkPose3D.append(Point32(x=err.x, y=err.y))
polarList.append(Point32(x=err.x, y=err.y))
names.append(name)
confidences.append(confidence)
errorList.append(Point32(x=err.x, y=err.y))
pos2DList.append(p2D)
elif plane is None:
pass
else:
x, y = p3D[:2]
cartList.append(Point32(x=x, y=y))
landmarkPose3D.append(point2D(x=x, y=y))
rho = math.sqrt(x**2 + y**2)
phi = math.atan2(y, x)
polarList.append(Point32(x=rho, y=phi))
names.append(name)
confidences.append(confidence)
errorList.append(Point32(x=err.x, y=err.y))
pos2DList.append(p2D)
t6 = time.time()
for plane, p3D in pointsClound3D:
if plane is None:
continue
else:
x, y = p3D[:2]
# x *= 0.8
# y *= 0.8
ranges.append(math.sqrt(x**2 + y**2))
points.append(point2D(x=x, y=y))
splitIndexes.append(len(points))
t7 = time.time()
for plane, p3D in boundary3D:
if plane is None:
continue
else:
x, y = p3D[:2]
# x *= 0.8
# y *= 0.8
ranges.append(math.sqrt(x**2 + y**2))
points.append(point2D(x=x, y=y))
circle = self.findCircle_ransac(np.array([[p.x, p.y] for p in points]))
if circle is not None and circle[0] is not None and circle[
1] is not None and circle[2] is not None:
names.append('circle')
landmarkPose3D.append(point2D(x=circle[0], y=circle[1]))
confidences.append(
self.getDensityProbability_normalize(circle[2], 0.75, 0.5))
t8 = time.time()
# print points3D
msg = postDictMsg()
msg.object_name = names
msg.pos3D_cart = cartList
msg.pos2D_polar = polarList
msg.pos2D = pos2DList
msg.object_error = errorList
msg.object_confidence = confidences
msg.imgH = imgH
msg.imgW = imgW
pointCloundMSG = scanlinePointClound()
pointCloundMSG.num_scanline = 20
pointCloundMSG.min_range = 0.5
pointCloundMSG.max_range = 4
pointCloundMSG.range = ranges
pointCloundMSG.points = points
pointCloundMSG.splitting_index = splitIndexes
pointCloundMSG.jointState = js
msgLocalize = localizationMsg()
msgLocalize.pointClound = pointCloundMSG
msgLocalize.landmark.names = names
msgLocalize.landmark.pose = landmarkPose3D
msgLocalize.landmark.confidences = confidences
t9 = time.time()
# print "Create npy :", t1 - t0
# print "Creat H mat:", t2 - t1
# print "Cal pointC :", t3 - t2
# print "Cal boundy :", t4 - t3
# print "Cal objMsg :", t5 - t4
# print "Add obj2lis:", t6 - t5
# print "Add poitntC:", t7 - t6
# print "Add boundar:", t8 - t7
# print "create msg :", t9 - t8
# print "Overall :", t9 - t0
# print '---------\n'
return msg, msgLocalize
def kinematicCalculation(self, objMsg, js, cortexMsg, rconfig=None):
self.points2D = [[p.x, p.y] for p in objMsg.pos2D]
pitch = math.radians(cortexMsg.pitch) if cortexMsg is not None else 0.0
roll = math.radians(cortexMsg.roll) if cortexMsg is not None else 0.0
# print math.degrees(pitch), math.degrees(roll)
# roll = pitch = 0.0
tranvec = np.zeros((3, 1))
rotvec = np.array([roll, pitch, 0.0])
HRotate = self.create_transformationMatrix(tranvec,
rotvec,
'rpy',
order="tran-first")
H = getMatrixForForwardKinematic(js.position[0], js.position[1], roll,
pitch)
H = np.matmul(HRotate, H)
points3D = self.calculate3DCoor(self.points2D, HCamera=H)
polarList = []
cartList = []
names = []
confidences = []
errorList = []
pos2DList = []
imgH = objMsg.imgH
imgW = objMsg.imgW
for (plane,
p3D), name, confidence, p2D in zip(points3D, objMsg.object_name,
objMsg.object_confidence,
objMsg.pos2D):
if plane is None:
pass
else:
x, y = p3D[:2]
if math.sqrt(x**2 + y**2) > 6:
continue
cartList.append(Point32(x=x, y=y))
rho = math.sqrt(x**2 + y**2)
phi = math.atan2(y, x)
polarList.append(Point32(x=rho, y=phi))
names.append(name)
confidences.append(confidence)
errorList.append(Point32(x=p2D.x / imgW, y=p2D.y / imgH))
pos2DList.append(p2D)
# print points3D
msg = postDictMsg()
msg.object_name = names
msg.pos3D_cart = cartList
msg.pos2D_polar = polarList
msg.pos2D = pos2DList
msg.object_error = objMsg.object_error
msg.object_confidence = confidences
msg.imgH = imgH
msg.imgW = imgW
return msg
def kinematicCalculation( self, objMsg, joint ):
# get object name and passthrough
objectNameList = objMsg.object_name
# Get camera kinematics
# transformationMatrix = self.forwardKinematics( joint )
qPan = getJsPosFromName( joint, "pan" )
qTilt = getJsPosFromName( joint, "tilt" )
transformationMatrix = getMatrixForForwardKinematic( qPan, qTilt )
# initial list
ball3DCartesianList = list()
ball2DPolarList = list()
#
# convert for 'ball' only
#
# loop every object to calculate 3D position
for objIndex in range( len( objMsg.object_name ) ):
if objMsg.object_confidence[ objIndex ] > 0.50:
# get 2D ball position
ballPosition = np.array( [ objMsg.pos2D[ objIndex ].x,
objMsg.pos2D[ objIndex ].y ], dtype = np.float64 )
ballPosition = ballPosition.reshape( -1, 2 )
# get ball in world coordinate
# ball3DCartesian is returned in form [ ( 'projection', arrayOfPos ) ]
ball3DCartesian = self.calculate3DCoor( ballPosition,
HCamera = transformationMatrix )[ 0 ][ 1 ]
# get polar coordinate
# handle error when 3D coordinate is cannot calculate 3D position
try:
r = np.sqrt( ball3DCartesian[ 0 ] ** 2 + ball3DCartesian[ 1 ] ** 2 )
theta = np.arctan2( ball3DCartesian[ 1 ], ball3DCartesian[ 0 ] )
ball2DPolar = np.array( [ r, theta ] )
# compress to msg
ball3DCartesianMsg = Point32( ball3DCartesian[ 0 ],
ball3DCartesian[ 1 ],
ball3DCartesian[ 2 ] )
ball2DPolarMsg = Point32( ball2DPolar[ 0 ],
ball2DPolar[ 1 ],
0 )
except Exception as e:
rospy.logwarn( e )
ball3DCartesianMsg = Point32()
ball2DPolarMsg = Point32()
else:
ball3DCartesianMsg = Point32()
ball2DPolarMsg = Point32()
ball3DCartesianList.append( ball3DCartesianMsg )
ball2DPolarList.append( ball2DPolarMsg )
# If not find the ball
# if objMsg.ball_confidence == False or len( objMsg.ball ) == 0:
#
# # Set ball 3D Cartesion is None
# ball3DCartesian = None
#
# else:
# # Calculate 3D coordinate respect to base frame
# ballPosition = np.array( objMsg.ball, dtype = np.float64 ).reshape( -1, 2 )
# ball3DCartesian = self.calculate3DCoor( ballPosition, HCamera = transformationMatrix )
# ball3DCartesian = ball3DCartesian[ 0 ][ 1 ]
#
# # If ball 3D cartesion is None
# if ball3DCartesian is not None:
#
# # Calculate polar coordinate change x, y -> r, theta
# #ball2DPolar = cv2.cartToPolar( ball3DCartesian[ 0 ], ball3DCartesian[ 1 ] )
# #ball2DPolar = np.array( [ ball2DPolar[ 0 ][ 0 ], ball2DPolar[ 0 ][ 0 ] ] )
# r = np.sqrt( ball3DCartesian[ 0 ] ** 2 + ball3DCartesian[ 1 ] ** 2 )
# theta = np.arctan2( ball3DCartesian[ 1 ], ball3DCartesian[ 0 ] )
# ball2DPolar = np.array( [ r, theta ] )
#
# else:
#
# # If can't calculate 3D return empty vector
# ball3DCartesian = np.array( [ ] )
# ball2DPolar = np.array( [ ] )
#
# rospy.loginfo( "\nPosition in 3D coordinate : {}\n".format( ball3DCartesian ) )
# rospy.logdebug( "\nPosition in Polar coordinate : {}\n".format( ball2DPolar ) )
# Get 2D projection back to camera
self.point2D1 = self.calculate2DCoor( self.points, "ground", HCamera= transformationMatrix )
self.point2D1 = np.array( self.point2D1 )
# Publist positon dict message
msg = postDictMsg()
msg.object_name = objectNameList
msg.pos3D_cart = ball3DCartesianList
msg.pos2D_polar = ball2DPolarList
msg.pos2D = objMsg.pos2D
msg.imgW = objMsg.imgW
msg.imgH = objMsg.imgH
msg.object_error = objMsg.object_error
msg.object_confidence = objMsg.object_confidence
msg.header.stamp = rospy.Time.now()
return msg